GCC Code Coverage Report

Directory: ./
File: unittest/classic-acceleration.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 73 0.0%
Branches: 0 410 0.0%
Line Branch Exec Source
1 //
2 // Copyright(c) 2019 INRIA
3 //
4
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/kinematics.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/multibody/sample-models.hpp"
10
11 #include "pinocchio/spatial/classic-acceleration.hpp"
12
13 #include <iostream>
14
15 #include <boost/test/unit_test.hpp>
16 #include <boost/utility/binary.hpp>
17
18 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19
20 ✗ BOOST_AUTO_TEST_CASE(test_classic_acceleration)
21 {
22 using namespace Eigen;
23 using namespace pinocchio;
24
25 static const int num_tests = 1e2;
26
27 ✗ Model model;
28 ✗ buildModels::humanoidRandom(model);
29
30 ✗ model.upperPositionLimit.head<3>().fill(100);
31 ✗ model.upperPositionLimit.segment<4>(3).setOnes();
32 ✗ model.lowerPositionLimit.head<7>() = -model.upperPositionLimit.head<7>();
33
34 ✗ VectorXd q(model.nq);
35 ✗ q = randomConfiguration(model);
36 ✗ VectorXd v(VectorXd::Random(model.nv));
37 ✗ VectorXd a(VectorXd::Zero(model.nv));
38
39 ✗ Data data_ref(model), data(model);
40
41 ✗ const std::string RF_joint_name = "rleg6_joint";
42 ✗ const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name);
43
44 ✗ for (int k = 0; k < num_tests; ++k)
45 {
46 ✗ q = randomConfiguration(model);
47 ✗ v = VectorXd::Random(model.nv);
48
49 ✗ forwardKinematics(model, data, q, v, a);
50 ✗ const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), SE3::Vector3::Zero());
51
52 ✗ const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]);
53 ✗ const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]);
54 ✗ const Motion::Vector3 classic_acc = classicAcceleration(RF_v_global, RF_a_global);
55
56 ✗ Motion::Vector3 classic_acc_other_signature;
57 ✗ classicAcceleration(RF_v_global, RF_a_global, classic_acc_other_signature);
58 ✗ BOOST_CHECK(classic_acc_other_signature.isApprox(classic_acc));
59
60 // Computes with finite differences
61 ✗ const double eps = 1e-5;
62 ✗ const double eps2 = eps * eps;
63 ✗ forwardKinematics(model, data_ref, q);
64 ✗ const SE3::Vector3 pos = data_ref.oMi[RF_joint_id].translation();
65
66 ✗ VectorXd v_plus = v + eps * a;
67 ✗ VectorXd q_plus = integrate(model, q, v * eps + a * eps2 / 2.);
68 ✗ forwardKinematics(model, data_ref, q_plus);
69 ✗ const SE3::Vector3 pos_plus = data_ref.oMi[RF_joint_id].translation();
70
71 ✗ VectorXd v_minus = v - eps * a;
72 ✗ VectorXd q_minus = integrate(model, q, -v * eps - a * eps2 / 2.);
73 ✗ forwardKinematics(model, data_ref, q_minus);
74 ✗ const SE3::Vector3 pos_minus = data_ref.oMi[RF_joint_id].translation();
75
76 ✗ const SE3::Vector3 classic_acc_ref = (pos_plus + pos_minus - 2. * pos) / eps2;
77
78 ✗ BOOST_CHECK(classic_acc.isApprox(classic_acc_ref, math::sqrt(eps) * 1e1));
79 }
80 }
81
82 ✗ BOOST_AUTO_TEST_CASE(test_classic_acceleration_with_placement)
83 {
84 using namespace Eigen;
85 using namespace pinocchio;
86
87 static const int num_tests = 1e2;
88
89 ✗ Model model;
90 ✗ buildModels::humanoidRandom(model);
91
92 ✗ model.upperPositionLimit.head<3>().fill(1);
93 ✗ model.upperPositionLimit.segment<4>(3).setOnes();
94 ✗ model.lowerPositionLimit.head<7>() = -model.upperPositionLimit.head<7>();
95
96 ✗ VectorXd q(model.nq);
97 ✗ q = randomConfiguration(model);
98 ✗ VectorXd v(VectorXd::Random(model.nv));
99 ✗ VectorXd a(VectorXd::Zero(model.nv));
100
101 ✗ Data data_ref(model), data(model);
102
103 ✗ const std::string RF_joint_name = "rleg6_joint";
104 ✗ const Model::JointIndex RF_joint_id = model.getJointId(RF_joint_name);
105
106 ✗ for (int k = 0; k < num_tests; ++k)
107 {
108 ✗ q = randomConfiguration(model);
109 ✗ v = VectorXd::Random(model.nv);
110
111 ✗ forwardKinematics(model, data, q, v, a);
112
113 ✗ const SE3 RF_world_transf = SE3(data.oMi[RF_joint_id].rotation(), SE3::Vector3::Zero());
114
115 ✗ const Motion RF_v_global = RF_world_transf.act(data.v[RF_joint_id]);
116 ✗ const Motion RF_a_global = RF_world_transf.act(data.a[RF_joint_id]);
117 ✗ const Motion::Vector3 RF_classic_acc_ref = classicAcceleration(RF_v_global, RF_a_global);
118
119 ✗ const SE3 identity_placement = SE3::Identity();
120 Motion::Vector3 RF_classic_acc =
121 ✗ classicAcceleration(RF_v_global, RF_a_global, identity_placement);
122
123 ✗ BOOST_CHECK(RF_classic_acc.isApprox(RF_classic_acc_ref));
124
125 ✗ const SE3 random_placement = SE3::Random();
126
127 ✗ const Motion & v_A = data.v[RF_joint_id];
128 ✗ const Motion & a_A = data.a[RF_joint_id];
129
130 ✗ const Motion v_B = random_placement.actInv(data.v[RF_joint_id]);
131 ✗ const Motion a_B = random_placement.actInv(data.a[RF_joint_id]);
132
133 ✗ Motion::Vector3 classic_acc_B_ref = classicAcceleration(v_B, a_B);
134 ✗ Motion::Vector3 classic_acc_B = classicAcceleration(v_A, a_A, random_placement);
135
136 ✗ BOOST_CHECK(classic_acc_B.isApprox(classic_acc_B_ref));
137
138 // Check other signature
139 ✗ Motion::Vector3 classic_acc_B_other_signature;
140 ✗ classicAcceleration(v_A, a_A, random_placement, classic_acc_B_other_signature);
141
142 ✗ BOOST_CHECK(classic_acc_B_other_signature.isApprox(classic_acc_B));
143 }
144 }
145
146 BOOST_AUTO_TEST_SUITE_END()
147