GCC Code Coverage Report

Directory: ./
File: unittest/test_multiple_contacts.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 368 0.0%
Branches: 0 1620 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include <pinocchio/algorithm/frames.hpp>
13 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14
15 #include "factory/contact.hpp"
16 #include "unittest_common.hpp"
17
18 using namespace boost::unit_test;
19 using namespace crocoddyl::unittest;
20
21 //----------------------------------------------------------------------------//
22
23 /**
24 * These methods modify the return type of the model function in
25 * order to use the boost::execution_monitor::execute method which catch the
26 * assert signal
27 */
28 ✗ int calc(crocoddyl::ContactModelMultiple& model,
29 std::shared_ptr<crocoddyl::ContactDataMultiple> data,
30 Eigen::VectorXd& dx) {
31 ✗ model.calc(data, dx);
32 ✗ return 0;
33 }
34
35 ✗ int calcDiff(crocoddyl::ContactModelMultiple& model,
36 std::shared_ptr<crocoddyl::ContactDataMultiple> data,
37 Eigen::VectorXd& dx) {
38 ✗ model.calcDiff(data, dx);
39 ✗ return 0;
40 }
41
42 ✗ int updateForce(crocoddyl::ContactModelMultiple& model,
43 std::shared_ptr<crocoddyl::ContactDataMultiple> data,
44 Eigen::VectorXd& dx) {
45 ✗ model.updateForce(data, dx);
46 ✗ return 0;
47 }
48
49 ✗ int updateAccelerationDiff(crocoddyl::ContactModelMultiple& model,
50 std::shared_ptr<crocoddyl::ContactDataMultiple> data,
51 const Eigen::MatrixXd& ddv_dx) {
52 ✗ model.updateAccelerationDiff(data, ddv_dx);
53 ✗ return 0;
54 }
55
56 ✗ int updateForceDiff(crocoddyl::ContactModelMultiple& model,
57 std::shared_ptr<crocoddyl::ContactDataMultiple> data,
58 const Eigen::MatrixXd& df_dx,
59 const Eigen::MatrixXd& df_du) {
60 ✗ model.updateForceDiff(data, df_dx, df_du);
61 ✗ return 0;
62 }
63
64 //----------------------------------------------------------------------------//
65
66 ✗ void test_constructor() {
67 // Setup the test
68 ✗ StateModelFactory state_factory;
69 crocoddyl::ContactModelMultiple model(
70 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
71 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
72
73 // Run the print function
74 ✗ std::ostringstream tmp;
75 ✗ tmp << model;
76
77 // Test the initial size of the map
78 ✗ BOOST_CHECK(model.get_contacts().size() == 0);
79
80 // Checking that casted computation is the same
81 #ifdef NDEBUG // Run only in release mode
82 crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
83 BOOST_CHECK(casted_model.get_contacts().size() == 0);
84 #endif
85 }
86
87 ✗ void test_addContact() {
88 // Setup the test
89 ✗ StateModelFactory state_factory;
90 crocoddyl::ContactModelMultiple model(
91 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
92 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
93 ✗ crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
94
95 // add an active contact
96 std::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact_1 =
97 ✗ create_random_contact();
98 ✗ model.addContact("random_contact_1", rand_contact_1);
99 ✗ BOOST_CHECK(model.get_nc() == rand_contact_1->get_nc());
100 ✗ BOOST_CHECK(model.get_nc_total() == rand_contact_1->get_nc());
101
102 // add an inactive contact
103 std::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact_2 =
104 ✗ create_random_contact();
105 ✗ model.addContact("random_contact_2", rand_contact_2, false);
106 ✗ BOOST_CHECK(model.get_nc() == rand_contact_1->get_nc());
107 ✗ BOOST_CHECK(model.get_nc_total() ==
108 rand_contact_1->get_nc() + rand_contact_2->get_nc());
109
110 // change the random contact 2 status
111 ✗ model.changeContactStatus("random_contact_2", true);
112 ✗ BOOST_CHECK(model.get_nc() ==
113 rand_contact_1->get_nc() + rand_contact_2->get_nc());
114 ✗ BOOST_CHECK(model.get_nc_total() ==
115 rand_contact_1->get_nc() + rand_contact_2->get_nc());
116
117 // change the random contact 1 status
118 ✗ model.changeContactStatus("random_contact_1", false);
119 ✗ BOOST_CHECK(model.get_nc() == rand_contact_2->get_nc());
120 ✗ BOOST_CHECK(model.get_nc_total() ==
121 rand_contact_1->get_nc() + rand_contact_2->get_nc());
122
123 // Checking that casted computation is the same
124 #ifdef NDEBUG // Run only in release mode
125 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>>
126 casted_rand_contact_1 = rand_contact_1->cast<float>();
127 casted_model.addContact("random_contact_1", casted_rand_contact_1);
128 BOOST_CHECK(casted_model.get_nc() == casted_rand_contact_1->get_nc());
129 BOOST_CHECK(casted_model.get_nc_total() == casted_rand_contact_1->get_nc());
130 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>>
131 casted_rand_contact_2 = rand_contact_2->cast<float>();
132 casted_model.addContact("random_contact_2", casted_rand_contact_2, false);
133 BOOST_CHECK(casted_model.get_nc() == casted_rand_contact_1->get_nc());
134 BOOST_CHECK(casted_model.get_nc_total() ==
135 casted_rand_contact_1->get_nc() +
136 casted_rand_contact_2->get_nc());
137 casted_model.changeContactStatus("random_contact_2", true);
138 BOOST_CHECK(casted_model.get_nc() == casted_rand_contact_1->get_nc() +
139 casted_rand_contact_2->get_nc());
140 BOOST_CHECK(casted_model.get_nc_total() ==
141 casted_rand_contact_1->get_nc() +
142 casted_rand_contact_2->get_nc());
143 casted_model.changeContactStatus("random_contact_1", false);
144 BOOST_CHECK(casted_model.get_nc() == casted_rand_contact_2->get_nc());
145 BOOST_CHECK(casted_model.get_nc_total() ==
146 casted_rand_contact_1->get_nc() +
147 casted_rand_contact_2->get_nc());
148 #endif
149 }
150
151 ✗ void test_addContact_error_message() {
152 // Setup the test
153 ✗ StateModelFactory state_factory;
154 crocoddyl::ContactModelMultiple model(
155 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
156 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
157
158 // create an contact object
159 std::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact =
160 ✗ create_random_contact();
161
162 // add twice the same contact object to the container
163 ✗ model.addContact("random_contact", rand_contact);
164
165 // test error message when we add a duplicate contact
166 ✗ CaptureIOStream capture_ios;
167 ✗ capture_ios.beginCapture();
168 ✗ model.addContact("random_contact", rand_contact);
169 ✗ capture_ios.endCapture();
170 ✗ std::stringstream expected_buffer;
171 expected_buffer << "Warning: we couldn't add the random_contact contact "
172 ✗ "item, it already existed."
173 ✗ << std::endl;
174 ✗ BOOST_CHECK(capture_ios.str() == expected_buffer.str());
175
176 // test error message when we change the contact status of an inexistent
177 // contact
178 ✗ capture_ios.beginCapture();
179 ✗ model.changeContactStatus("no_exist_contact", true);
180 ✗ capture_ios.endCapture();
181 ✗ expected_buffer.clear();
182 expected_buffer << "Warning: we couldn't change the status of the "
183 ✗ "no_exist_contact contact item, it doesn't exist."
184 ✗ << std::endl;
185 ✗ BOOST_CHECK(capture_ios.str() == expected_buffer.str());
186 }
187
188 ✗ void test_removeContact() {
189 // Setup the test
190 ✗ StateModelFactory state_factory;
191 crocoddyl::ContactModelMultiple model(
192 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
193 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
194 ✗ crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
195
196 // add an active contact
197 std::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact =
198 ✗ create_random_contact();
199 ✗ model.addContact("random_contact", rand_contact);
200 ✗ BOOST_CHECK(model.get_nc() == rand_contact->get_nc());
201 ✗ BOOST_CHECK(model.get_nc_total() == rand_contact->get_nc());
202
203 // remove the contact
204 ✗ model.removeContact("random_contact");
205 ✗ BOOST_CHECK(model.get_nc() == 0);
206 ✗ BOOST_CHECK(model.get_nc_total() == 0);
207
208 // Checking that casted computation is the same
209 #ifdef NDEBUG // Run only in release mode
210 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>>
211 casted_rand_contact = rand_contact->cast<float>();
212 casted_model.addContact("random_contact", casted_rand_contact);
213 BOOST_CHECK(casted_model.get_nc() == casted_rand_contact->get_nc());
214 BOOST_CHECK(casted_model.get_nc_total() == casted_rand_contact->get_nc());
215 casted_model.removeContact("random_contact");
216 BOOST_CHECK(casted_model.get_nc() == 0);
217 BOOST_CHECK(casted_model.get_nc_total() == 0);
218 #endif
219 }
220
221 ✗ void test_removeContact_error_message() {
222 // Setup the test
223 ✗ StateModelFactory state_factory;
224 crocoddyl::ContactModelMultiple model(
225 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
226 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
227
228 // remove a none existing contact form the container, we expect a cout message
229 // here
230 ✗ CaptureIOStream capture_ios;
231 ✗ capture_ios.beginCapture();
232 ✗ model.removeContact("random_contact");
233 ✗ capture_ios.endCapture();
234
235 // Test that the error message is sent.
236 ✗ std::stringstream expected_buffer;
237 expected_buffer << "Warning: we couldn't remove the random_contact contact "
238 ✗ "item, it doesn't exist."
239 ✗ << std::endl;
240 ✗ BOOST_CHECK(capture_ios.str() == expected_buffer.str());
241 }
242
243 ✗ void test_calc() {
244 // Setup the test
245 ✗ StateModelFactory state_factory;
246 crocoddyl::ContactModelMultiple model(
247 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
248 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
249
250 // create the corresponding data object
251 ✗ pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
252 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
253
254 // create and add some contact objects
255 ✗ std::vector<std::shared_ptr<crocoddyl::ContactModelAbstract>> models;
256 ✗ std::vector<std::shared_ptr<crocoddyl::ContactDataAbstract>> datas;
257 ✗ for (std::size_t i = 0; i < 5; ++i) {
258 ✗ std::ostringstream os;
259 ✗ os << "random_contact_" << i;
260 const std::shared_ptr<crocoddyl::ContactModelAbstract>& m =
261 ✗ create_random_contact();
262 ✗ model.addContact(os.str(), m);
263 ✗ models.push_back(m);
264 ✗ datas.push_back(m->createData(&pinocchio_data));
265 }
266
267 // create the data of the multiple-contacts
268 std::shared_ptr<crocoddyl::ContactDataMultiple> data =
269 ✗ model.createData(&pinocchio_data);
270
271 // compute the multiple contact data for the case when all contacts are
272 // defined as active
273 ✗ Eigen::VectorXd x1 = model.get_state()->rand();
274 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
275 x1);
276 ✗ model.calc(data, x1);
277
278 // check that only the Jacobian has been filled
279 ✗ BOOST_CHECK(!data->Jc.isZero());
280 ✗ BOOST_CHECK(!data->a0.isZero());
281 ✗ BOOST_CHECK(data->da0_dx.isZero());
282
283 // check Jc and a0 against single contact computations
284 ✗ std::size_t nc = 0;
285 ✗ std::size_t nv = model.get_state()->get_nv();
286 ✗ for (std::size_t i = 0; i < 5; ++i) {
287 ✗ const std::size_t nc_i = models[i]->get_nc();
288 ✗ models[i]->calc(datas[i], x1);
289 ✗ BOOST_CHECK(data->Jc.block(nc, 0, nc_i, nv) == datas[i]->Jc);
290 ✗ BOOST_CHECK(data->a0.segment(nc, nc_i) == datas[i]->a0);
291 ✗ nc += nc_i;
292 }
293 ✗ nc = 0;
294
295 // compute the multiple contact data for the case when the first three
296 // contacts are defined as active
297 ✗ model.changeContactStatus("random_contact_3", false);
298 ✗ model.changeContactStatus("random_contact_4", false);
299 ✗ Eigen::VectorXd x2 = model.get_state()->rand();
300 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
301 x2);
302 ✗ model.calc(data, x2);
303 ✗ for (std::size_t i = 0; i < 5; ++i) {
304 ✗ const std::size_t nc_i = models[i]->get_nc();
305 ✗ if (i < 3) { // we need to update data because this contacts are active
306 ✗ models[i]->calc(datas[i], x2);
307 }
308 ✗ BOOST_CHECK(data->Jc.block(nc, 0, nc_i, nv) == datas[i]->Jc);
309 ✗ BOOST_CHECK(data->a0.segment(nc, nc_i) == datas[i]->a0);
310 ✗ nc += nc_i;
311 }
312
313 // Checking that casted computation is the same
314 #ifdef NDEBUG // Run only in release mode
315 model.changeContactStatus("random_contact_3", true);
316 model.changeContactStatus("random_contact_4", true);
317 crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
318 pinocchio::ModelTpl<float>& casted_pinocchio_model =
319 *casted_model.get_state()->get_pinocchio().get();
320 pinocchio::DataTpl<float> casted_pinocchio_data(
321 *casted_model.get_state()->get_pinocchio().get());
322 std::vector<std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>>>
323 casted_models;
324 std::vector<std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>>
325 casted_datas;
326 for (std::size_t i = 0; i < 5; ++i) {
327 casted_models.push_back(models[i]->cast<float>());
328 casted_datas.push_back(
329 casted_models[i]->createData(&casted_pinocchio_data));
330 }
331 std::shared_ptr<crocoddyl::ContactDataMultipleTpl<float>> casted_data =
332 casted_model.createData(&casted_pinocchio_data);
333 const Eigen::VectorXf x1_f = x1.cast<float>();
334 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
335 x1);
336 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
337 &casted_pinocchio_data, x1_f);
338 model.calc(data, x1);
339 casted_model.calc(casted_data, x1_f);
340 BOOST_CHECK(!casted_data->Jc.isZero());
341 BOOST_CHECK(!casted_data->a0.isZero());
342 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
343 BOOST_CHECK((data->Jc.cast<float>() - casted_data->Jc).isZero(tol_f));
344 BOOST_CHECK((data->a0.cast<float>() - casted_data->a0).isZero(tol_f));
345 BOOST_CHECK(casted_data->da0_dx.isZero());
346 nc = 0;
347 nv = casted_model.get_state()->get_nv();
348 for (std::size_t i = 0; i < 5; ++i) {
349 const std::size_t nc_i = casted_models[i]->get_nc();
350 casted_models[i]->calc(casted_datas[i], x1_f);
351 BOOST_CHECK(casted_data->Jc.block(nc, 0, nc_i, nv) == casted_datas[i]->Jc);
352 BOOST_CHECK(casted_data->a0.segment(nc, nc_i) == casted_datas[i]->a0);
353 nc += nc_i;
354 }
355 nc = 0;
356 #endif
357 }
358
359 ✗ void test_calc_diff() {
360 // Setup the test
361 ✗ StateModelFactory state_factory;
362 crocoddyl::ContactModelMultiple model(
363 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
364 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
365 // create the corresponding data object
366 ✗ pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
367 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
368
369 // create and add some contact objects
370 ✗ std::vector<std::shared_ptr<crocoddyl::ContactModelAbstract>> models;
371 ✗ std::vector<std::shared_ptr<crocoddyl::ContactDataAbstract>> datas;
372 ✗ for (std::size_t i = 0; i < 5; ++i) {
373 ✗ std::ostringstream os;
374 ✗ os << "random_contact_" << i;
375 const std::shared_ptr<crocoddyl::ContactModelAbstract>& m =
376 ✗ create_random_contact();
377 ✗ model.addContact(os.str(), m);
378 ✗ models.push_back(m);
379 ✗ datas.push_back(m->createData(&pinocchio_data));
380 }
381
382 // create the data of the multiple-contacts
383 std::shared_ptr<crocoddyl::ContactDataMultiple> data =
384 ✗ model.createData(&pinocchio_data);
385
386 // compute the multiple contact data for the case when all contacts are
387 // defined as active
388 ✗ Eigen::VectorXd x1 = model.get_state()->rand();
389 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
390 x1);
391 ✗ model.calc(data, x1);
392 ✗ model.calcDiff(data, x1);
393
394 // check that nothing has been computed and that all value are initialized to
395 // 0
396 ✗ BOOST_CHECK(!data->Jc.isZero());
397 ✗ BOOST_CHECK(!data->a0.isZero());
398 ✗ BOOST_CHECK(!data->da0_dx.isZero());
399
400 // check Jc and a0 against single contact computations
401 ✗ std::size_t nc = 0;
402 ✗ std::size_t nv = model.get_state()->get_nv();
403 ✗ std::size_t ndx = model.get_state()->get_ndx();
404 ✗ for (std::size_t i = 0; i < 5; ++i) {
405 ✗ const std::size_t nc_i = models[i]->get_nc();
406 ✗ models[i]->calc(datas[i], x1);
407 ✗ models[i]->calcDiff(datas[i], x1);
408 ✗ BOOST_CHECK(data->Jc.block(nc, 0, nc_i, nv) == datas[i]->Jc);
409 ✗ BOOST_CHECK(data->a0.segment(nc, nc_i) == datas[i]->a0);
410 ✗ BOOST_CHECK(data->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
411 ✗ nc += nc_i;
412 }
413 ✗ nc = 0;
414
415 // compute the multiple contact data for the case when the first three
416 // contacts are defined as active
417 ✗ model.changeContactStatus("random_contact_3", false);
418 ✗ model.changeContactStatus("random_contact_4", false);
419 ✗ Eigen::VectorXd x2 = model.get_state()->rand();
420 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
421 x2);
422 ✗ model.calc(data, x2);
423 ✗ model.calcDiff(data, x2);
424 ✗ for (std::size_t i = 0; i < 5; ++i) {
425 ✗ const std::size_t nc_i = models[i]->get_nc();
426 ✗ if (i < 3) { // we need to update data because this contacts are active
427 ✗ models[i]->calc(datas[i], x2);
428 ✗ models[i]->calcDiff(datas[i], x2);
429 }
430 ✗ BOOST_CHECK(data->Jc.block(nc, 0, nc_i, nv) == datas[i]->Jc);
431 ✗ BOOST_CHECK(data->a0.segment(nc, nc_i) == datas[i]->a0);
432 ✗ BOOST_CHECK(data->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
433 ✗ nc += nc_i;
434 }
435
436 // Checking that casted computation is the same
437 #ifdef NDEBUG // Run only in release mode
438 model.changeContactStatus("random_contact_3", true);
439 model.changeContactStatus("random_contact_4", true);
440 crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
441 pinocchio::ModelTpl<float>& casted_pinocchio_model =
442 *casted_model.get_state()->get_pinocchio().get();
443 pinocchio::DataTpl<float> casted_pinocchio_data(
444 *casted_model.get_state()->get_pinocchio().get());
445 std::vector<std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>>>
446 casted_models;
447 std::vector<std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>>
448 casted_datas;
449 for (std::size_t i = 0; i < 5; ++i) {
450 casted_models.push_back(models[i]->cast<float>());
451 casted_datas.push_back(
452 casted_models[i]->createData(&casted_pinocchio_data));
453 }
454 std::shared_ptr<crocoddyl::ContactDataMultipleTpl<float>> casted_data =
455 casted_model.createData(&casted_pinocchio_data);
456 const Eigen::VectorXf x1_f = x1.cast<float>();
457 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
458 x1);
459 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
460 &casted_pinocchio_data, x1_f);
461 model.calc(data, x1);
462 model.calcDiff(data, x1);
463 casted_model.calc(casted_data, x1_f);
464 casted_model.calcDiff(casted_data, x1_f);
465 BOOST_CHECK(!casted_data->Jc.isZero());
466 BOOST_CHECK(!casted_data->a0.isZero());
467 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
468 BOOST_CHECK((data->Jc.cast<float>() - casted_data->Jc).isZero(tol_f));
469 BOOST_CHECK((data->a0.cast<float>() - casted_data->a0).isZero(tol_f));
470 BOOST_CHECK((data->da0_dx.cast<float>() - casted_data->da0_dx).isZero(tol_f));
471 nc = 0;
472 nv = casted_model.get_state()->get_nv();
473 ndx = casted_model.get_state()->get_ndx();
474 for (std::size_t i = 0; i < 5; ++i) {
475 const std::size_t nc_i = casted_models[i]->get_nc();
476 casted_models[i]->calc(casted_datas[i], x1_f);
477 casted_models[i]->calcDiff(casted_datas[i], x1_f);
478 BOOST_CHECK(casted_data->Jc.block(nc, 0, nc_i, nv) == casted_datas[i]->Jc);
479 BOOST_CHECK(casted_data->a0.segment(nc, nc_i) == casted_datas[i]->a0);
480 BOOST_CHECK(casted_data->da0_dx.block(nc, 0, nc_i, ndx) ==
481 casted_datas[i]->da0_dx);
482 nc += nc_i;
483 }
484 nc = 0;
485 #endif
486 }
487
488 ✗ void test_calc_diff_no_recalc() {
489 // Setup the test
490 ✗ StateModelFactory state_factory;
491 crocoddyl::ContactModelMultiple model(
492 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
493 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
494 // create the corresponding data object
495 ✗ pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
496 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
497
498 // create and add some contact objects
499 ✗ std::vector<std::shared_ptr<crocoddyl::ContactModelAbstract>> models;
500 ✗ std::vector<std::shared_ptr<crocoddyl::ContactDataAbstract>> datas;
501 ✗ for (std::size_t i = 0; i < 5; ++i) {
502 ✗ std::ostringstream os;
503 ✗ os << "random_contact_" << i;
504 const std::shared_ptr<crocoddyl::ContactModelAbstract>& m =
505 ✗ create_random_contact();
506 ✗ model.addContact(os.str(), m);
507 ✗ models.push_back(m);
508 ✗ datas.push_back(m->createData(&pinocchio_data));
509 }
510
511 // create the data of the multiple-contacts
512 std::shared_ptr<crocoddyl::ContactDataMultiple> data =
513 ✗ model.createData(&pinocchio_data);
514
515 // compute the multiple contact data for the case when all contacts are
516 // defined as active
517 ✗ Eigen::VectorXd x1 = model.get_state()->rand();
518 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
519 x1);
520 ✗ model.calcDiff(data, x1);
521
522 // check that nothing has been computed and that all value are initialized to
523 // 0
524 ✗ BOOST_CHECK(data->Jc.isZero());
525 ✗ BOOST_CHECK(data->a0.isZero());
526 ✗ BOOST_CHECK(!data->da0_dx.isZero());
527
528 // check Jc and a0 against single contact computations
529 ✗ std::size_t nc = 0;
530 ✗ const std::size_t nv = model.get_state()->get_nv();
531 ✗ const std::size_t ndx = model.get_state()->get_ndx();
532 ✗ for (std::size_t i = 0; i < 5; ++i) {
533 ✗ const std::size_t nc_i = models[i]->get_nc();
534 ✗ models[i]->calcDiff(datas[i], x1);
535 ✗ BOOST_CHECK(data->Jc.block(nc, 0, nc_i, nv).isZero());
536 ✗ BOOST_CHECK(data->a0.segment(nc, nc_i).isZero());
537 ✗ BOOST_CHECK(data->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
538 ✗ nc += nc_i;
539 }
540 ✗ nc = 0;
541
542 // compute the multiple contact data for the case when the first three
543 // contacts are defined as active
544 ✗ model.changeContactStatus("random_contact_3", false);
545 ✗ model.changeContactStatus("random_contact_4", false);
546 ✗ Eigen::VectorXd x2 = model.get_state()->rand();
547 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
548 x2);
549 ✗ model.calcDiff(data, x2);
550 ✗ for (std::size_t i = 0; i < 5; ++i) {
551 ✗ const std::size_t nc_i = models[i]->get_nc();
552 ✗ if (i < 3) { // we need to update data because this contacts are active
553 ✗ models[i]->calcDiff(datas[i], x2);
554 }
555 ✗ BOOST_CHECK(data->Jc.block(nc, 0, nc_i, nv).isZero());
556 ✗ BOOST_CHECK(data->a0.segment(nc, nc_i).isZero());
557 ✗ BOOST_CHECK(data->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
558 ✗ nc += nc_i;
559 }
560 }
561
562 ✗ void test_updateForce() {
563 // Setup the test
564 ✗ StateModelFactory state_factory;
565 crocoddyl::ContactModelMultiple model(
566 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
567 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
568
569 // create the corresponding data object
570 const pinocchio::Model& pinocchio_model =
571 ✗ *model.get_state()->get_pinocchio().get();
572 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
573
574 // create and add some contact objects
575 ✗ for (unsigned i = 0; i < 5; ++i) {
576 ✗ std::ostringstream os;
577 ✗ os << "random_contact_" << i;
578 ✗ model.addContact(os.str(), create_random_contact());
579 }
580
581 // create the data of the multiple-contacts
582 std::shared_ptr<crocoddyl::ContactDataMultiple> data =
583 ✗ model.createData(&pinocchio_data);
584
585 // Compute the jacobian and check that the contact model fetch it.
586 Eigen::VectorXd q =
587 ✗ model.get_state()->rand().segment(0, model.get_state()->get_nq());
588 ✗ Eigen::VectorXd v = Eigen::VectorXd::Random(model.get_state()->get_nv());
589 ✗ Eigen::VectorXd a = Eigen::VectorXd::Random(model.get_state()->get_nv());
590 ✗ pinocchio::computeJointJacobians(pinocchio_model, pinocchio_data, q);
591 ✗ pinocchio::updateFramePlacements(pinocchio_model, pinocchio_data);
592 ✗ pinocchio::computeForwardKinematicsDerivatives(pinocchio_model,
593 pinocchio_data, q, v, a);
594
595 // create random forces
596 ✗ Eigen::VectorXd forces = Eigen::VectorXd::Random(model.get_nc());
597
598 // update forces
599 ✗ model.updateForce(data, forces);
600
601 // Check that nothing has been computed and that all value are initialized to
602 // 0
603 ✗ BOOST_CHECK(data->Jc.isZero());
604 ✗ BOOST_CHECK(data->a0.isZero());
605 ✗ BOOST_CHECK(data->da0_dx.isZero());
606 ✗ crocoddyl::ContactModelMultiple::ContactDataContainer::iterator it_d, end_d;
607 ✗ for (it_d = data->contacts.begin(), end_d = data->contacts.end();
608 ✗ it_d != end_d; ++it_d) {
609 ✗ BOOST_CHECK(!it_d->second->f.toVector().isZero());
610 }
611
612 // Checking that casted computation is the same
613 #ifdef NDEBUG // Run only in release mode
614 crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
615 pinocchio::ModelTpl<float>& casted_pinocchio_model =
616 *casted_model.get_state()->get_pinocchio().get();
617 pinocchio::DataTpl<float> casted_pinocchio_data(
618 *casted_model.get_state()->get_pinocchio().get());
619 std::shared_ptr<crocoddyl::ContactDataMultipleTpl<float>> casted_data =
620 casted_model.createData(&casted_pinocchio_data);
621 const Eigen::VectorXf q_f = casted_model.get_state()->rand().segment(
622 0, casted_model.get_state()->get_nq());
623 const Eigen::VectorXf v_f =
624 Eigen::VectorXf::Random(casted_model.get_state()->get_nv());
625 const Eigen::VectorXf a_f =
626 Eigen::VectorXf::Random(casted_model.get_state()->get_nv());
627 pinocchio::computeJointJacobians(casted_pinocchio_model,
628 casted_pinocchio_data, q_f);
629 pinocchio::updateFramePlacements(casted_pinocchio_model,
630 casted_pinocchio_data);
631 pinocchio::computeForwardKinematicsDerivatives(
632 casted_pinocchio_model, casted_pinocchio_data, q_f, v_f, a_f);
633 const Eigen::VectorXf forces_f = forces.cast<float>();
634 casted_model.updateForce(casted_data, forces_f);
635 BOOST_CHECK(casted_data->Jc.isZero());
636 BOOST_CHECK(casted_data->a0.isZero());
637 BOOST_CHECK(casted_data->da0_dx.isZero());
638 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
639 crocoddyl::ContactModelMultipleTpl<float>::ContactDataContainer::iterator
640 it_d_f,
641 end_d_f;
642 for (it_d_f = casted_data->contacts.begin(),
643 end_d_f = casted_data->contacts.end(), it_d = data->contacts.begin(),
644 end_d = data->contacts.end();
645 it_d_f != end_d_f || it_d != end_d; ++it_d_f, ++it_d) {
646 BOOST_CHECK(!it_d_f->second->f.toVector().isZero());
647 BOOST_CHECK((it_d->second->f.toVector().cast<float>() -
648 it_d_f->second->f.toVector())
649 .isZero(tol_f));
650 }
651 #endif
652 }
653
654 ✗ void test_updateAccelerationDiff() {
655 // Setup the test
656 ✗ StateModelFactory state_factory;
657 crocoddyl::ContactModelMultiple model(
658 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
659 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
660 // create the corresponding data object
661 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
662
663 // create and add some contact objects
664 ✗ for (unsigned i = 0; i < 5; ++i) {
665 ✗ std::ostringstream os;
666 ✗ os << "random_contact_" << i;
667 ✗ model.addContact(os.str(), create_random_contact());
668 }
669
670 // create the data of the multiple-contacts
671 std::shared_ptr<crocoddyl::ContactDataMultiple> data =
672 ✗ model.createData(&pinocchio_data);
673
674 // create the velocity diff
675 ✗ Eigen::MatrixXd ddv_dx = Eigen::MatrixXd::Random(
676 ✗ model.get_state()->get_nv(), model.get_state()->get_ndx());
677
678 // call the update
679 ✗ model.updateAccelerationDiff(data, ddv_dx);
680
681 // Test
682 ✗ BOOST_CHECK((data->ddv_dx - ddv_dx).isZero(1e-9));
683
684 // Checking that casted computation is the same
685 #ifdef NDEBUG // Run only in release mode
686 crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
687 pinocchio::DataTpl<float> casted_pinocchio_data(
688 *casted_model.get_state()->get_pinocchio().get());
689 std::shared_ptr<crocoddyl::ContactDataMultipleTpl<float>> casted_data =
690 casted_model.createData(&casted_pinocchio_data);
691 const Eigen::MatrixXf ddv_dx_f = ddv_dx.cast<float>();
692 casted_model.updateAccelerationDiff(casted_data, ddv_dx_f);
693 BOOST_CHECK((casted_data->ddv_dx - ddv_dx_f).isZero(1e-9f));
694 #endif
695 }
696
697 ✗ void test_updateForceDiff() {
698 // Setup the test
699 ✗ StateModelFactory state_factory;
700 crocoddyl::ContactModelMultiple model(
701 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
702 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
703 // create the corresponding data object
704 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
705
706 // create and add some contact objects
707 ✗ for (unsigned i = 0; i < 5; ++i) {
708 ✗ std::ostringstream os;
709 ✗ os << "random_contact_" << i;
710 ✗ model.addContact(os.str(), create_random_contact());
711 }
712
713 // create the data of the multiple-contacts
714 std::shared_ptr<crocoddyl::ContactDataMultiple> data =
715 ✗ model.createData(&pinocchio_data);
716
717 // create force diff
718 Eigen::MatrixXd df_dx =
719 ✗ Eigen::MatrixXd::Random(model.get_nc(), model.get_state()->get_nv());
720 Eigen::MatrixXd df_du =
721 ✗ Eigen::MatrixXd::Random(model.get_nc(), model.get_state()->get_nv());
722
723 // call update force diff
724 ✗ model.updateForceDiff(data, df_dx, df_du);
725
726 // Test
727 ✗ crocoddyl::ContactModelMultiple::ContactDataContainer::iterator it_d, end_d;
728 ✗ for (it_d = data->contacts.begin(), end_d = data->contacts.end();
729 ✗ it_d != end_d; ++it_d) {
730 ✗ BOOST_CHECK(!it_d->second->df_dx.isZero());
731 }
732
733 // Checking that casted computation is the same
734 #ifdef NDEBUG // Run only in release mode
735 crocoddyl::ContactModelMultipleTpl<float> casted_model = model.cast<float>();
736 pinocchio::DataTpl<float> casted_pinocchio_data(
737 *casted_model.get_state()->get_pinocchio().get());
738 std::shared_ptr<crocoddyl::ContactDataMultipleTpl<float>> casted_data =
739 casted_model.createData(&casted_pinocchio_data);
740 const Eigen::MatrixXf df_dx_f = df_dx.cast<float>();
741 const Eigen::MatrixXf df_du_f = df_du.cast<float>();
742 casted_model.updateForceDiff(casted_data, df_dx_f, df_du_f);
743 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
744 crocoddyl::ContactModelMultipleTpl<float>::ContactDataContainer::iterator
745 it_d_f,
746 end_d_f;
747 for (it_d_f = casted_data->contacts.begin(),
748 end_d_f = casted_data->contacts.end(), it_d = data->contacts.begin(),
749 end_d = data->contacts.end();
750 it_d_f != end_d_f || it_d != end_d; ++it_d_f, ++it_d) {
751 BOOST_CHECK(!it_d_f->second->df_dx.isZero());
752 BOOST_CHECK((it_d->second->df_dx.cast<float>() - it_d_f->second->df_dx)
753 .isZero(tol_f));
754 }
755 #endif
756 }
757
758 ✗ void test_assert_updateForceDiff_assert_mismatch_model_data() {
759 // Setup the test
760 ✗ StateModelFactory state_factory;
761 crocoddyl::ContactModelMultiple model1(
762 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
763 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
764 crocoddyl::ContactModelMultiple model2(
765 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
766 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
767 // create the corresponding data object
768 ✗ pinocchio::Data pinocchio_data(*model1.get_state()->get_pinocchio().get());
769
770 // create and add some contact objects
771 ✗ for (unsigned i = 0; i < 5; ++i) {
772 std::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact =
773 ✗ create_random_contact();
774 {
775 ✗ std::ostringstream os;
776 ✗ os << "random_contact1_" << i;
777 ✗ model1.addContact(os.str(), rand_contact);
778 }
779 {
780 ✗ std::ostringstream os;
781 ✗ os << "random_contact2_" << i;
782 ✗ model2.addContact(os.str(), rand_contact);
783 }
784 }
785
786 // create the data of the multiple-contacts
787 std::shared_ptr<crocoddyl::ContactDataMultiple> data1 =
788 ✗ model1.createData(&pinocchio_data);
789 std::shared_ptr<crocoddyl::ContactDataMultiple> data2 =
790 ✗ model2.createData(&pinocchio_data);
791
792 // create force diff
793 Eigen::MatrixXd df_dx =
794 ✗ Eigen::MatrixXd::Random(model1.get_nc(), model1.get_state()->get_nv());
795 Eigen::MatrixXd df_du =
796 ✗ Eigen::MatrixXd::Random(model1.get_nc(), model1.get_state()->get_nv());
797
798 // call that trigger assert
799 std::string error_message = GetErrorMessages(
800 ✗ boost::bind(&updateForceDiff, model1, data2, df_dx, df_du));
801
802 // expected error message content
803 std::string function_name =
804 "void crocoddyl::ContactModelMultiple::updateForceDiff("
805 "const std::shared_ptr<crocoddyl::ContactDataMultiple>&,"
806 ✗ " const MatrixXd&) const";
807 std::string assert_argument =
808 "it_m->first == it_d->first && \"it doesn't match"
809 ✗ " the contact name between data and model\"";
810
811 // Perform the checks
812 #ifndef __APPLE__
813 ✗ BOOST_CHECK(error_message.find(function_name) != std::string::npos);
814 #endif
815 ✗ BOOST_CHECK(error_message.find(assert_argument) != std::string::npos);
816 }
817
818 ✗ void test_get_contacts() {
819 // Setup the test
820 ✗ StateModelFactory state_factory;
821 crocoddyl::ContactModelMultiple model(
822 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
823 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
824 // create the corresponding data object
825 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
826
827 // create and add some contact objects
828 ✗ for (unsigned i = 0; i < 5; ++i) {
829 ✗ std::ostringstream os;
830 ✗ os << "random_contact_" << i;
831 ✗ model.addContact(os.str(), create_random_contact());
832 }
833
834 // get the contacts
835 const crocoddyl::ContactModelMultiple::ContactModelContainer& contacts =
836 ✗ model.get_contacts();
837
838 // test
839 ✗ crocoddyl::ContactModelMultiple::ContactModelContainer::const_iterator it_m,
840 ✗ end_m;
841 unsigned i;
842 ✗ for (i = 0, it_m = contacts.begin(), end_m = contacts.end(); it_m != end_m;
843 ✗ ++it_m, ++i) {
844 ✗ std::ostringstream os;
845 ✗ os << "random_contact_" << i;
846 ✗ BOOST_CHECK(it_m->first == os.str());
847 }
848 }
849
850 ✗ void test_get_nc() {
851 // Setup the test
852 ✗ StateModelFactory state_factory;
853 crocoddyl::ContactModelMultiple model(
854 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
855 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
856
857 // create the corresponding data object
858 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
859
860 // create and add some contact objects
861 ✗ for (unsigned i = 0; i < 5; ++i) {
862 ✗ std::ostringstream os;
863 ✗ os << "random_contact_" << i;
864 ✗ model.addContact(os.str(), create_random_contact());
865 }
866
867 // compute ni
868 ✗ std::size_t ni = 0;
869 ✗ crocoddyl::ContactModelMultiple::ContactModelContainer::const_iterator it_m,
870 ✗ end_m;
871 ✗ for (it_m = model.get_contacts().begin(), end_m = model.get_contacts().end();
872 ✗ it_m != end_m; ++it_m) {
873 ✗ ni += it_m->second->contact->get_nc();
874 }
875
876 ✗ BOOST_CHECK(ni == model.get_nc());
877 }
878
879 //----------------------------------------------------------------------------//
880
881 ✗ void register_unit_tests() {
882 ✗ framework::master_test_suite().add(
883 ✗ BOOST_TEST_CASE(boost::bind(&test_constructor)));
884 ✗ framework::master_test_suite().add(
885 ✗ BOOST_TEST_CASE(boost::bind(&test_addContact)));
886 ✗ framework::master_test_suite().add(
887 ✗ BOOST_TEST_CASE(boost::bind(&test_addContact_error_message)));
888 ✗ framework::master_test_suite().add(
889 ✗ BOOST_TEST_CASE(boost::bind(&test_removeContact)));
890 ✗ framework::master_test_suite().add(
891 ✗ BOOST_TEST_CASE(boost::bind(&test_removeContact_error_message)));
892 ✗ framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_calc)));
893 ✗ framework::master_test_suite().add(
894 ✗ BOOST_TEST_CASE(boost::bind(&test_calc_diff)));
895 ✗ framework::master_test_suite().add(
896 ✗ BOOST_TEST_CASE(boost::bind(&test_calc_diff_no_recalc)));
897 ✗ framework::master_test_suite().add(
898 ✗ BOOST_TEST_CASE(boost::bind(&test_updateForce)));
899 ✗ framework::master_test_suite().add(
900 ✗ BOOST_TEST_CASE(boost::bind(&test_updateAccelerationDiff)));
901 ✗ framework::master_test_suite().add(
902 ✗ BOOST_TEST_CASE(boost::bind(&test_get_contacts)));
903 ✗ framework::master_test_suite().add(
904 ✗ BOOST_TEST_CASE(boost::bind(&test_get_nc)));
905 }
906
907 ✗ bool init_function() {
908 ✗ register_unit_tests();
909 ✗ return true;
910 }
911
912 ✗ int main(int argc, char** argv) {
913 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
914 }
915