GCC Code Coverage Report

Directory: ./
File: unittest/joint-jacobian.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 164 0.0%
Branches: 0 882 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/kinematics.hpp"
9 #include "pinocchio/algorithm/rnea.hpp"
10 #include "pinocchio/spatial/act-on-set.hpp"
11 #include "pinocchio/multibody/sample-models.hpp"
12 #include "pinocchio/utils/timer.hpp"
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14
15 #include <iostream>
16
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19
20 template<typename Derived>
21 ✗ inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
22 {
23 ✗ return ((x - x).array() == (x - x).array()).all();
24 }
25
26 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
27
28 ✗ BOOST_AUTO_TEST_CASE(test_jacobian)
29 {
30 using namespace Eigen;
31 using namespace pinocchio;
32
33 ✗ pinocchio::Model model;
34 ✗ pinocchio::buildModels::humanoidRandom(model);
35 ✗ pinocchio::Data data(model);
36
37 ✗ VectorXd q = VectorXd::Zero(model.nq);
38 ✗ computeJointJacobians(model, data, q);
39
40 Model::Index idx =
41 ✗ model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1);
42 ✗ Data::Matrix6x Jrh(6, model.nv);
43 ✗ Jrh.fill(0);
44 ✗ getJointJacobian(model, data, idx, WORLD, Jrh);
45
46 /* Test J*q == v */
47 ✗ VectorXd qdot = VectorXd::Random(model.nv);
48 ✗ VectorXd qddot = VectorXd::Zero(model.nv);
49 ✗ rnea(model, data, q, qdot, qddot);
50 ✗ Motion v = data.oMi[idx].act(data.v[idx]);
51 ✗ BOOST_CHECK(v.toVector().isApprox(Jrh * qdot, 1e-12));
52
53 /* Test local jacobian: rhJrh == rhXo oJrh */
54 ✗ Data::Matrix6x rhJrh(6, model.nv);
55 ✗ rhJrh.fill(0);
56 ✗ getJointJacobian(model, data, idx, LOCAL, rhJrh);
57 ✗ Data::Matrix6x XJrh(6, model.nv);
58 ✗ motionSet::se3Action(data.oMi[idx].inverse(), Jrh, XJrh);
59 ✗ BOOST_CHECK(XJrh.isApprox(rhJrh, 1e-12));
60
61 ✗ XJrh.setZero();
62 ✗ Data data_jointJacobian(model);
63 ✗ computeJointJacobian(model, data_jointJacobian, q, idx, XJrh);
64 ✗ BOOST_CHECK(XJrh.isApprox(rhJrh, 1e-12));
65
66 /* Test computeJointJacobians with pre-computation of the forward kinematics */
67 ✗ Data data_fk(model);
68 ✗ forwardKinematics(model, data_fk, q);
69 ✗ computeJointJacobians(model, data_fk);
70
71 ✗ BOOST_CHECK(data_fk.J.isApprox(data.J));
72 }
73
74 ✗ BOOST_AUTO_TEST_CASE(test_jacobian_time_variation)
75 {
76 using namespace Eigen;
77 using namespace pinocchio;
78
79 ✗ pinocchio::Model model;
80 ✗ pinocchio::buildModels::humanoidRandom(model);
81 ✗ pinocchio::Data data(model);
82 ✗ pinocchio::Data data_ref(model);
83
84 VectorXd q = randomConfiguration(
85 ✗ model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq));
86 ✗ VectorXd v = VectorXd::Random(model.nv);
87 ✗ VectorXd a = VectorXd::Random(model.nv);
88
89 ✗ computeJointJacobiansTimeVariation(model, data, q, v);
90
91 ✗ BOOST_CHECK(isFinite(data.dJ));
92
93 ✗ forwardKinematics(model, data_ref, q, v, a);
94 Model::Index idx =
95 ✗ model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1);
96
97 ✗ Data::Matrix6x J(6, model.nv);
98 ✗ J.fill(0.);
99 ✗ Data::Matrix6x dJ(6, model.nv);
100 ✗ dJ.fill(0.);
101
102 // Regarding to the WORLD origin
103 ✗ getJointJacobian(model, data, idx, WORLD, J);
104 ✗ BOOST_CHECK(J.isApprox(getJointJacobian(model, data, idx, WORLD)));
105 ✗ getJointJacobianTimeVariation(model, data, idx, WORLD, dJ);
106
107 ✗ Motion v_idx(J * v);
108 ✗ BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));
109
110 ✗ Motion a_idx(J * a + dJ * v);
111 ✗ const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
112 ✗ BOOST_CHECK(a_idx.isApprox(a_ref));
113
114 // Regarding to the LOCAL frame
115 ✗ getJointJacobian(model, data, idx, LOCAL, J);
116 ✗ BOOST_CHECK(J.isApprox(getJointJacobian(model, data, idx, LOCAL)));
117 ✗ getJointJacobianTimeVariation(model, data, idx, LOCAL, dJ);
118
119 ✗ v_idx = (Motion::Vector6)(J * v);
120 ✗ BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));
121
122 ✗ a_idx = (Motion::Vector6)(J * a + dJ * v);
123 ✗ BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));
124
125 // Regarding to the LOCAL_WORLD_ALIGNED frame
126 ✗ getJointJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, J);
127 ✗ BOOST_CHECK(J.isApprox(getJointJacobian(model, data, idx, LOCAL_WORLD_ALIGNED)));
128 ✗ getJointJacobianTimeVariation(model, data, idx, LOCAL_WORLD_ALIGNED, dJ);
129
130 ✗ Data::SE3 worldMlocal = data.oMi[idx];
131 ✗ worldMlocal.translation().setZero();
132
133 ✗ v_idx = (Motion::Vector6)(J * v);
134 ✗ BOOST_CHECK(v_idx.isApprox(worldMlocal.act(data_ref.v[idx])));
135
136 ✗ a_idx = (Motion::Vector6)(J * a + dJ * v);
137 ✗ BOOST_CHECK(a_idx.isApprox(worldMlocal.act(data_ref.a[idx])));
138
139 // compare to finite differencies : WORLD
140 {
141 ✗ Data data_ref(model), data_ref_plus(model);
142
143 ✗ const double alpha = 1e-8;
144 ✗ Eigen::VectorXd q_plus(model.nq);
145 ✗ q_plus = integrate(model, q, alpha * v);
146
147 ✗ Data::Matrix6x J_ref(6, model.nv);
148 ✗ J_ref.fill(0.);
149 ✗ computeJointJacobians(model, data_ref, q);
150 ✗ getJointJacobian(model, data_ref, idx, WORLD, J_ref);
151
152 ✗ Data::Matrix6x J_ref_plus(6, model.nv);
153 ✗ J_ref_plus.fill(0.);
154 ✗ computeJointJacobians(model, data_ref_plus, q_plus);
155 ✗ getJointJacobian(model, data_ref_plus, idx, WORLD, J_ref_plus);
156
157 ✗ Data::Matrix6x dJ_ref(6, model.nv);
158 ✗ dJ_ref.fill(0.);
159 ✗ dJ_ref = (J_ref_plus - J_ref) / alpha;
160
161 ✗ computeJointJacobiansTimeVariation(model, data, q, v);
162 ✗ Data::Matrix6x dJ(6, model.nv);
163 ✗ dJ.fill(0.);
164 ✗ getJointJacobianTimeVariation(model, data, idx, WORLD, dJ);
165
166 ✗ BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(alpha)));
167 }
168
169 // compare to finite differencies : LOCAL
170 {
171 ✗ Data data_ref(model), data_ref_plus(model);
172
173 ✗ const double alpha = 1e-8;
174 ✗ Eigen::VectorXd q_plus(model.nq);
175 ✗ q_plus = integrate(model, q, alpha * v);
176
177 ✗ Data::Matrix6x J_ref(6, model.nv);
178 ✗ J_ref.fill(0.);
179 ✗ computeJointJacobians(model, data_ref, q);
180 ✗ getJointJacobian(model, data_ref, idx, LOCAL, J_ref);
181
182 ✗ Data::Matrix6x J_ref_plus(6, model.nv);
183 ✗ J_ref_plus.fill(0.);
184 ✗ computeJointJacobians(model, data_ref_plus, q_plus);
185 ✗ getJointJacobian(model, data_ref_plus, idx, LOCAL, J_ref_plus);
186
187 ✗ const Data::SE3 M_plus = data_ref.oMi[idx].inverse() * data_ref_plus.oMi[idx];
188
189 ✗ Data::Matrix6x dJ_ref(6, model.nv);
190 ✗ dJ_ref.fill(0.);
191 ✗ dJ_ref = (M_plus.toActionMatrix() * J_ref_plus - J_ref) / alpha;
192
193 ✗ computeJointJacobiansTimeVariation(model, data, q, v);
194 ✗ Data::Matrix6x dJ(6, model.nv);
195 ✗ dJ.fill(0.);
196 ✗ getJointJacobianTimeVariation(model, data, idx, LOCAL, dJ);
197
198 ✗ BOOST_CHECK(dJ.isApprox(dJ_ref, sqrt(alpha)));
199 }
200 }
201
202 ✗ BOOST_AUTO_TEST_CASE(test_timings)
203 {
204 using namespace Eigen;
205 using namespace pinocchio;
206
207 ✗ pinocchio::Model model;
208 ✗ pinocchio::buildModels::humanoidRandom(model);
209 ✗ pinocchio::Data data(model);
210
211 ✗ long flag = BOOST_BINARY(1111);
212 ✗ PinocchioTicToc timer(PinocchioTicToc::US);
213 #ifdef NDEBUG
214 #ifdef _INTENSE_TESTING_
215 const size_t NBT = 1000 * 1000;
216 #else
217 const size_t NBT = 10;
218 #endif
219 #else
220 ✗ const size_t NBT = 1;
221 ✗ std::cout << "(the time score in debug mode is not relevant) ";
222 #endif
223
224 ✗ bool verbose = flag & (flag - 1); // True is two or more binaries of the flag are 1.
225 ✗ if (verbose)
226 ✗ std::cout << "--" << std::endl;
227 ✗ Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
228
229 ✗ if (flag >> 0 & 1)
230 {
231 ✗ timer.tic();
232 ✗ SMOOTH(NBT)
233 {
234 ✗ computeJointJacobians(model, data, q);
235 }
236 ✗ if (verbose)
237 ✗ std::cout << "Compute =\t";
238 ✗ timer.toc(std::cout, NBT);
239 }
240
241 ✗ if (flag >> 1 & 1)
242 {
243 ✗ computeJointJacobians(model, data, q);
244 Model::Index idx =
245 ✗ model.existJointName("rarm6") ? model.getJointId("rarm6") : (Model::Index)(model.njoints - 1);
246 ✗ Data::Matrix6x Jrh(6, model.nv);
247 ✗ Jrh.fill(0);
248
249 ✗ timer.tic();
250 ✗ SMOOTH(NBT)
251 {
252 ✗ getJointJacobian(model, data, idx, WORLD, Jrh);
253 }
254 ✗ if (verbose)
255 ✗ std::cout << "Copy =\t";
256 ✗ timer.toc(std::cout, NBT);
257 }
258
259 ✗ if (flag >> 2 & 1)
260 {
261 ✗ computeJointJacobians(model, data, q);
262 Model::Index idx =
263 ✗ model.existJointName("rarm6") ? model.getJointId("rarm6") : (Model::Index)(model.njoints - 1);
264 ✗ Data::Matrix6x Jrh(6, model.nv);
265 ✗ Jrh.fill(0);
266
267 ✗ timer.tic();
268 ✗ SMOOTH(NBT)
269 {
270 ✗ getJointJacobian(model, data, idx, LOCAL, Jrh);
271 }
272 ✗ if (verbose)
273 ✗ std::cout << "Change frame =\t";
274 ✗ timer.toc(std::cout, NBT);
275 }
276
277 ✗ if (flag >> 3 & 1)
278 {
279 ✗ computeJointJacobians(model, data, q);
280 Model::Index idx =
281 ✗ model.existJointName("rarm6") ? model.getJointId("rarm6") : (Model::Index)(model.njoints - 1);
282 ✗ Data::Matrix6x Jrh(6, model.nv);
283 ✗ Jrh.fill(0);
284
285 ✗ timer.tic();
286 ✗ SMOOTH(NBT)
287 {
288 ✗ computeJointJacobian(model, data, q, idx, Jrh);
289 }
290 ✗ if (verbose)
291 ✗ std::cout << "Single jacobian =\t";
292 ✗ timer.toc(std::cout, NBT);
293 }
294 }
295
296 BOOST_AUTO_TEST_SUITE_END()
297