GCC Code Coverage Report

Directory: ./
File: unittest/aba.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 198 0.0%
Branches: 0 1440 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
3 //
4
5 #include "pinocchio/algorithm/aba.hpp"
6 #include "pinocchio/algorithm/rnea.hpp"
7 #include "pinocchio/algorithm/jacobian.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/algorithm/crba.hpp"
10 #include "pinocchio/multibody/sample-models.hpp"
11
12 #include "pinocchio/algorithm/compute-all-terms.hpp"
13 #include "pinocchio/utils/timer.hpp"
14
15 #include <iostream>
16
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19
20 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21
22 template<typename JointModel>
23 ✗ void test_joint_methods(const pinocchio::JointModelBase<JointModel> & jmodel)
24 {
25 ✗ std::cout << "shortname: " << jmodel.shortname() << std::endl;
26 typedef typename pinocchio::JointModelBase<JointModel>::JointDataDerived JointData;
27 typedef typename JointModel::ConfigVector_t ConfigVector_t;
28 typedef typename pinocchio::LieGroup<JointModel>::type LieGroupType;
29
30 ✗ JointData jdata = jmodel.createData();
31
32 ✗ ConfigVector_t ql(ConfigVector_t::Constant(jmodel.nq(), -M_PI));
33 ✗ ConfigVector_t qu(ConfigVector_t::Constant(jmodel.nq(), M_PI));
34
35 ✗ ConfigVector_t q = LieGroupType().randomConfiguration(ql, qu);
36 ✗ pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Random().matrix());
37 ✗ pinocchio::Inertia::Matrix6 I_check = I;
38 ✗ const Eigen::VectorXd armature =
39 ✗ Eigen::VectorXd::Random(jmodel.nv()) + Eigen::VectorXd::Ones(jmodel.nv());
40
41 ✗ jmodel.calc(jdata, q);
42 ✗ jmodel.calc_aba(jdata, armature, I, true);
43
44 ✗ std::cout << "armature: " << armature.transpose() << std::endl;
45 ✗ Eigen::MatrixXd S = jdata.S.matrix();
46 ✗ Eigen::MatrixXd U_check = I_check * S;
47 ✗ Eigen::MatrixXd StU_check = S.transpose() * U_check;
48 ✗ StU_check.diagonal() += armature;
49 ✗ Eigen::MatrixXd Dinv_check = StU_check.inverse();
50 ✗ Eigen::MatrixXd UDinv_check = U_check * Dinv_check;
51 ✗ Eigen::MatrixXd update_check = UDinv_check * U_check.transpose();
52 ✗ I_check -= update_check;
53
54 ✗ std::cout << "I_check:\n" << I_check << std::endl;
55 ✗ std::cout << "I:\n" << I << std::endl;
56 ✗ BOOST_CHECK(jdata.U.isApprox(U_check));
57 ✗ BOOST_CHECK(jdata.Dinv.isApprox(Dinv_check));
58 ✗ BOOST_CHECK(jdata.UDinv.isApprox(UDinv_check));
59
60 // Checking the inertia was correctly updated
61 // We use isApprox as usual, except for the freeflyer,
62 // where the correct result is exacly zero and isApprox would fail.
63 // Only for this single case, we use the infinity norm of the difference
64 ✗ if (jmodel.shortname() == "JointModelFreeFlyer")
65 ✗ BOOST_CHECK((I - I_check).isZero());
66 else
67 ✗ BOOST_CHECK(I.isApprox(I_check));
68 }
69
70 struct TestJointMethods
71 {
72
73 template<typename JointModel>
74 ✗ void operator()(const pinocchio::JointModelBase<JointModel> &) const
75 {
76 ✗ JointModel jmodel;
77 ✗ jmodel.setIndexes(0, 0, 0);
78
79 ✗ test_joint_methods(jmodel);
80 }
81
82 void operator()(const pinocchio::JointModelBase<pinocchio::JointModelComposite> &) const
83 {
84 pinocchio::JointModelComposite jmodel_composite;
85 jmodel_composite.addJoint(pinocchio::JointModelRX());
86 jmodel_composite.addJoint(pinocchio::JointModelRY());
87 jmodel_composite.setIndexes(0, 0, 0);
88
89 // TODO: correct LieGroup
90 // test_joint_methods(jmodel_composite);
91 }
92
93 ✗ void operator()(const pinocchio::JointModelBase<pinocchio::JointModelRevoluteUnaligned> &) const
94 {
95 ✗ pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
96 ✗ jmodel.setIndexes(0, 0, 0);
97
98 ✗ test_joint_methods(jmodel);
99 }
100
101 ✗ void operator()(const pinocchio::JointModelBase<pinocchio::JointModelPrismaticUnaligned> &) const
102 {
103 ✗ pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
104 ✗ jmodel.setIndexes(0, 0, 0);
105
106 ✗ test_joint_methods(jmodel);
107 }
108 };
109
110 ✗ BOOST_AUTO_TEST_CASE(test_joint_basic)
111 {
112 using namespace pinocchio;
113
114 typedef boost::variant<
115 JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical,
116 JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned,
117 JointModelFreeFlyer, JointModelPlanar, JointModelTranslation, JointModelRUBX, JointModelRUBY,
118 JointModelRUBZ>
119 Variant;
120
121 ✗ boost::mpl::for_each<Variant::types>(TestJointMethods());
122 }
123
124 ✗ BOOST_AUTO_TEST_CASE(test_aba_simple)
125 {
126 using namespace Eigen;
127 using namespace pinocchio;
128
129 ✗ pinocchio::Model model;
130 ✗ buildModels::humanoidRandom(model);
131
132 ✗ pinocchio::Data data(model);
133 ✗ pinocchio::Data data_ref(model);
134
135 ✗ model.lowerPositionLimit.head<7>().fill(-1.);
136 ✗ model.upperPositionLimit.head<7>().fill(1.);
137 ✗ VectorXd q = randomConfiguration(model);
138
139 ✗ VectorXd v = VectorXd::Ones(model.nv);
140 ✗ VectorXd tau = VectorXd::Zero(model.nv);
141 ✗ VectorXd a = VectorXd::Ones(model.nv);
142
143 ✗ tau = rnea(model, data_ref, q, v, a);
144 ✗ forwardKinematics(model, data_ref, q);
145 ✗ aba(model, data, q, v, tau, Convention::WORLD);
146
147 ✗ for (size_t k = 1; k < (size_t)model.njoints; ++k)
148 {
149 ✗ BOOST_CHECK(data_ref.liMi[k].isApprox(data.liMi[k]));
150 ✗ BOOST_CHECK(data_ref.oMi[k].act(data_ref.v[k]).isApprox(data.ov[k]));
151 ✗ BOOST_CHECK((data_ref.oMi[k].act(data_ref.a_gf[k])).isApprox(data.oa_gf[k]));
152 }
153
154 ✗ BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
155
156 // Test against deprecated ABA
157 ✗ Data data_deprecated(model);
158 ✗ aba(model, data_deprecated, q, v, tau, Convention::LOCAL);
159 ✗ BOOST_CHECK(data_deprecated.ddq.isApprox(data.ddq));
160
161 // Test multiple calls
162 {
163 ✗ Data datas(model);
164 ✗ VectorXd a1 = aba(model, datas, q, v, tau, Convention::LOCAL);
165 ✗ VectorXd a2 = aba(model, datas, q, v, tau, Convention::LOCAL);
166 ✗ VectorXd a3 = aba(model, datas, q, v, tau, Convention::LOCAL);
167
168 ✗ BOOST_CHECK(a1.isApprox(a2));
169 ✗ BOOST_CHECK(a1.isApprox(a3));
170 ✗ BOOST_CHECK(a2.isApprox(a3));
171 }
172
173 // Test multiple calls
174 {
175 ✗ Data datas(model);
176 ✗ VectorXd a1 = aba(model, datas, q, v, tau, Convention::WORLD);
177 ✗ VectorXd a2 = aba(model, datas, q, v, tau, Convention::WORLD);
178 ✗ VectorXd a3 = aba(model, datas, q, v, tau, Convention::WORLD);
179
180 ✗ BOOST_CHECK(a1.isApprox(a2));
181 ✗ BOOST_CHECK(a1.isApprox(a3));
182 ✗ BOOST_CHECK(a2.isApprox(a3));
183 }
184 }
185
186 ✗ BOOST_AUTO_TEST_CASE(test_aba_with_fext)
187 {
188 using namespace Eigen;
189 using namespace pinocchio;
190
191 ✗ pinocchio::Model model;
192 ✗ buildModels::humanoidRandom(model);
193
194 ✗ pinocchio::Data data(model);
195
196 ✗ model.lowerPositionLimit.head<7>().fill(-1.);
197 ✗ model.upperPositionLimit.head<7>().fill(1.);
198 ✗ VectorXd q = randomConfiguration(model);
199
200 ✗ VectorXd v = VectorXd::Random(model.nv);
201 ✗ VectorXd a = VectorXd::Random(model.nv);
202
203 ✗ PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Random());
204
205 ✗ crba(model, data, q, Convention::WORLD);
206 ✗ computeJointJacobians(model, data, q);
207 ✗ nonLinearEffects(model, data, q, v);
208 ✗ data.M.triangularView<Eigen::StrictlyLower>() =
209 ✗ data.M.transpose().triangularView<Eigen::StrictlyLower>();
210
211 ✗ VectorXd tau = data.M * a + data.nle;
212 ✗ Data::Matrix6x J = Data::Matrix6x::Zero(6, model.nv);
213 ✗ for (Model::Index i = 1; i < (Model::Index)model.njoints; ++i)
214 {
215 ✗ getJointJacobian(model, data, i, LOCAL, J);
216 ✗ tau -= J.transpose() * fext[i].toVector();
217 ✗ J.setZero();
218 }
219 ✗ aba(model, data, q, v, tau, fext, Convention::WORLD);
220
221 ✗ BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
222
223 // Test against deprecated ABA
224 ✗ Data data_deprecated(model);
225 ✗ aba(model, data_deprecated, q, v, tau, fext, Convention::LOCAL);
226 ✗ BOOST_CHECK(data_deprecated.ddq.isApprox(data.ddq));
227 }
228
229 ✗ BOOST_AUTO_TEST_CASE(test_aba_vs_rnea)
230 {
231 using namespace Eigen;
232 using namespace pinocchio;
233
234 ✗ pinocchio::Model model;
235 ✗ buildModels::humanoidRandom(model);
236
237 ✗ pinocchio::Data data(model);
238 ✗ pinocchio::Data data_ref(model);
239
240 ✗ model.lowerPositionLimit.head<7>().fill(-1.);
241 ✗ model.upperPositionLimit.head<7>().fill(1.);
242 ✗ VectorXd q = randomConfiguration(model);
243
244 ✗ VectorXd v = VectorXd::Ones(model.nv);
245 ✗ VectorXd tau = VectorXd::Zero(model.nv);
246 ✗ VectorXd a = VectorXd::Ones(model.nv);
247
248 ✗ crba(model, data_ref, q, Convention::WORLD);
249 ✗ nonLinearEffects(model, data_ref, q, v);
250 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
251 ✗ data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
252
253 ✗ tau = data_ref.M * a + data_ref.nle;
254 ✗ aba(model, data, q, v, tau, Convention::WORLD);
255
256 ✗ VectorXd tau_ref = rnea(model, data_ref, q, v, a);
257 ✗ BOOST_CHECK(tau_ref.isApprox(tau, 1e-12));
258 ✗ BOOST_CHECK(data.ddq.isApprox(a, 1e-12));
259
260 ✗ Data data_deprecated(model);
261 ✗ aba(model, data_deprecated, q, v, tau, Convention::LOCAL);
262 ✗ BOOST_CHECK(data_deprecated.ddq.isApprox(a, 1e-12));
263 }
264
265 ✗ BOOST_AUTO_TEST_CASE(test_computeMinverse)
266 {
267 using namespace Eigen;
268 using namespace pinocchio;
269
270 ✗ pinocchio::Model model;
271 ✗ buildModels::humanoidRandom(model);
272 ✗ model.gravity.setZero();
273
274 ✗ pinocchio::Data data(model);
275 ✗ pinocchio::Data data_ref(model);
276
277 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
278 ✗ model.upperPositionLimit.head<3>().fill(1.);
279 ✗ VectorXd q = randomConfiguration(model);
280 ✗ VectorXd v = VectorXd::Random(model.nv);
281
282 ✗ crba(model, data_ref, q, Convention::WORLD);
283 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
284 ✗ data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
285 ✗ MatrixXd Minv_ref(data_ref.M.inverse());
286
287 ✗ computeMinverse(model, data, q);
288
289 ✗ BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));
290
291 ✗ data.Minv.triangularView<Eigen::StrictlyLower>() =
292 ✗ data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
293
294 ✗ BOOST_CHECK(data.Minv.isApprox(Minv_ref));
295
296 // std::cout << "Minv:\n" << data.Minv.block<10,10>(0,0) << std::endl;
297 // std::cout << "Minv_ref:\n" << Minv_ref.block<10,10>(0,0) << std::endl;
298 //
299 // std::cout << "Minv:\n" << data.Minv.bottomRows<10>() << std::endl;
300 // std::cout << "Minv_ref:\n" << Minv_ref.bottomRows<10>() << std::endl;
301 }
302
303 ✗ BOOST_AUTO_TEST_CASE(test_computeMinverse_noupdate)
304 {
305 using namespace Eigen;
306 using namespace pinocchio;
307
308 ✗ pinocchio::Model model;
309 ✗ buildModels::humanoidRandom(model);
310 ✗ model.gravity.setZero();
311
312 ✗ pinocchio::Data data(model);
313 ✗ pinocchio::Data data_ref(model);
314
315 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
316 ✗ model.upperPositionLimit.head<3>().fill(1.);
317 ✗ VectorXd q = randomConfiguration(model);
318 ✗ VectorXd v = VectorXd::Random(model.nv);
319 ✗ VectorXd tau = VectorXd::Random(model.nv);
320
321 ✗ crba(model, data_ref, q, Convention::WORLD);
322 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
323 ✗ data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
324 ✗ MatrixXd Minv_ref(data_ref.M.inverse());
325
326 ✗ aba(model, data, q, v, tau, Convention::WORLD);
327 ✗ computeMinverse(model, data);
328 ✗ BOOST_CHECK(data.Minv.topRows<6>().isApprox(Minv_ref.topRows<6>()));
329
330 ✗ data.Minv.triangularView<Eigen::StrictlyLower>() =
331 ✗ data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
332
333 ✗ BOOST_CHECK(data.Minv.isApprox(Minv_ref));
334
335 ✗ Data data_ref2(model);
336 ✗ computeMinverse(model, data_ref2, q);
337 ✗ data_ref2.Minv.triangularView<Eigen::StrictlyLower>() =
338 ✗ data_ref2.Minv.transpose().triangularView<Eigen::StrictlyLower>();
339 ✗ BOOST_CHECK(data.Minv.isApprox(data_ref2.Minv));
340 }
341
342 ✗ BOOST_AUTO_TEST_CASE(test_multiple_calls)
343 {
344 using namespace Eigen;
345 using namespace pinocchio;
346
347 ✗ Model model;
348 ✗ buildModels::humanoidRandom(model);
349
350 ✗ Data data1(model), data2(model);
351
352 ✗ model.lowerPositionLimit.head<3>().fill(-1.);
353 ✗ model.upperPositionLimit.head<3>().fill(1.);
354 ✗ VectorXd q = randomConfiguration(model);
355
356 ✗ computeMinverse(model, data1, q);
357 ✗ data2 = data1;
358
359 ✗ for (int k = 0; k < 20; ++k)
360 {
361 ✗ computeMinverse(model, data1, q);
362 }
363
364 ✗ BOOST_CHECK(data1.Minv.isApprox(data2.Minv));
365 }
366
367 ✗ BOOST_AUTO_TEST_CASE(test_roto_inertia_effects)
368 {
369 ✗ pinocchio::Model model;
370 ✗ pinocchio::buildModels::humanoidRandom(model);
371 ✗ model.armature = Eigen::VectorXd::Random(model.nv) + Eigen::VectorXd::Constant(model.nv, 1.);
372
373 ✗ pinocchio::Data data(model), data_ref(model);
374
375 ✗ Eigen::VectorXd q = randomConfiguration(model);
376 ✗ pinocchio::crba(model, data_ref, q, pinocchio::Convention::WORLD);
377 ✗ data_ref.M.triangularView<Eigen::StrictlyLower>() =
378 ✗ data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
379
380 ✗ computeMinverse(model, data, q);
381 ✗ data.Minv.triangularView<Eigen::StrictlyLower>() =
382 ✗ data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
383
384 ✗ BOOST_CHECK((data.Minv * data_ref.M).isIdentity());
385 }
386
387 BOOST_AUTO_TEST_SUITE_END()
388