GCC Code Coverage Report

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