GCC Code Coverage Report

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