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 "crocoddyl/multibody/contacts/contact-1d.hpp"
13			#include "crocoddyl/multibody/contacts/contact-2d.hpp"
14			#include "crocoddyl/multibody/contacts/contact-3d.hpp"
15			#include "crocoddyl/multibody/contacts/contact-6d.hpp"
16			#include "factory/contact.hpp"
17			#include "unittest_common.hpp"
18
19			using namespace crocoddyl::unittest;
20			using namespace boost::unit_test;
21
22			//----------------------------------------------------------------------------//
23
24		✗	void test_construct_data(ContactModelTypes::Type contact_type,
25			PinocchioModelTypes::Type model_type) {
26			// create the model
27		✗	ContactModelFactory factory;
28			std::shared_ptr<crocoddyl::ContactModelAbstract> model =
29		✗	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
30
31			// Run the print function
32		✗	std::ostringstream tmp;
33		✗	tmp << *model;
34
35			// create the corresponding data object
36			const std::shared_ptr<pinocchio::Model>& pinocchio_model =
37		✗	model->get_state()->get_pinocchio();
38		✗	pinocchio::Data pinocchio_data(*pinocchio_model.get());
39			std::shared_ptr<crocoddyl::ContactDataAbstract> data =
40		✗	model->createData(&pinocchio_data);
41		✗	}
42
43		✗	void test_calc_fetch_jacobians(ContactModelTypes::Type contact_type,
44			PinocchioModelTypes::Type model_type) {
45			// create the model
46		✗	ContactModelFactory factory;
47			std::shared_ptr<crocoddyl::ContactModelAbstract> model =
48		✗	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
49
50			// create the corresponding data object
51			const std::shared_ptr<pinocchio::Model>& pinocchio_model =
52		✗	model->get_state()->get_pinocchio();
53		✗	pinocchio::Data pinocchio_data(*pinocchio_model.get());
54			const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
55		✗	model->createData(&pinocchio_data);
56
57			// Compute the jacobian and check that the impulse model fetch it.
58		✗	Eigen::VectorXd x = model->get_state()->rand();
59		✗	crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
60			&pinocchio_data, x);
61
62			// Getting the jacobian from the model
63		✗	model->calc(data, x);
64
65			// Check that only the Jacobian has been filled
66		✗	BOOST_CHECK(!data->Jc.isZero());
67		✗	if (model_type !=
68			PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
69			// body system.
70		✗	BOOST_CHECK(!data->a0.isZero());
71			}
72		✗	BOOST_CHECK(data->da0_dx.isZero());
73		✗	BOOST_CHECK(data->f.toVector().isZero());
74		✗	BOOST_CHECK(data->df_dx.isZero());
75		✗	BOOST_CHECK(data->df_du.isZero());
76
77			// Checking that casted computation is the same
78			#ifdef NDEBUG // Run only in release mode
79			std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
80			model->cast<float>();
81			const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
82			casted_model->get_state()->get_pinocchio();
83			pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
84			const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
85			casted_model->createData(&pinocchio_data_f);
86			Eigen::VectorXf x_f = x.cast<float>();
87			crocoddyl::unittest::updateAllPinocchio(pinocchio_model_f.get(),
88			&pinocchio_data_f, x_f);
89			casted_model->calc(casted_data, x_f);
90			BOOST_CHECK(!casted_data->Jc.isZero());
91			if (model_type !=
92			PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
93			// body system.
94			BOOST_CHECK(!casted_data->a0.isZero());
95			}
96			BOOST_CHECK(casted_data->da0_dx.isZero());
97			BOOST_CHECK(casted_data->f.toVector().isZero());
98			BOOST_CHECK(casted_data->df_dx.isZero());
99			BOOST_CHECK(casted_data->df_du.isZero());
100			#endif
101		✗	}
102
103		✗	void test_calc_diff_fetch_derivatives(ContactModelTypes::Type contact_type,
104			PinocchioModelTypes::Type model_type) {
105			// create the model
106		✗	ContactModelFactory factory;
107			std::shared_ptr<crocoddyl::ContactModelAbstract> model =
108		✗	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
109
110			// create the corresponding data object
111			const std::shared_ptr<pinocchio::Model>& pinocchio_model =
112		✗	model->get_state()->get_pinocchio();
113		✗	pinocchio::Data pinocchio_data(*pinocchio_model.get());
114			const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
115		✗	model->createData(&pinocchio_data);
116
117			// Compute the jacobian and check that the impulse model fetch it.
118		✗	Eigen::VectorXd x = model->get_state()->rand();
119		✗	crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
120			&pinocchio_data, x);
121
122			// Getting the jacobian from the model
123		✗	model->calc(data, x);
124		✗	model->calcDiff(data, x);
125
126			// Check that nothing has been computed and that all value are initialized to
127			// 0
128		✗	BOOST_CHECK(!data->Jc.isZero());
129		✗	if (model_type !=
130			PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
131			// body system.
132		✗	BOOST_CHECK(!data->a0.isZero());
133		✗	BOOST_CHECK(!data->da0_dx.isZero());
134			}
135		✗	BOOST_CHECK(data->f.toVector().isZero());
136		✗	BOOST_CHECK(data->df_dx.isZero());
137		✗	BOOST_CHECK(data->df_du.isZero());
138
139			// Checking that casted computation is the same
140			#ifdef NDEBUG // Run only in release mode
141			std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
142			model->cast<float>();
143			const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
144			casted_model->get_state()->get_pinocchio();
145			pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
146			const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
147			casted_model->createData(&pinocchio_data_f);
148			Eigen::VectorXf x_f = x.cast<float>();
149			crocoddyl::unittest::updateAllPinocchio(pinocchio_model_f.get(),
150			&pinocchio_data_f, x_f);
151			casted_model->calc(casted_data, x_f);
152			casted_model->calcDiff(casted_data, x_f);
153			BOOST_CHECK(!casted_data->Jc.isZero());
154			if (model_type !=
155			PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
156			// body system.
157			float tol_f =
158			80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
159			BOOST_CHECK(!casted_data->a0.isZero());
160			BOOST_CHECK(!casted_data->da0_dx.isZero());
161			BOOST_CHECK((data->a0.cast<float>() - casted_data->a0).isZero());
162			BOOST_CHECK(
163			(data->da0_dx.cast<float>() - casted_data->da0_dx).isZero(tol_f));
164			}
165			BOOST_CHECK(casted_data->f.toVector().isZero());
166			BOOST_CHECK(casted_data->df_dx.isZero());
167			BOOST_CHECK(casted_data->df_du.isZero());
168			#endif
169		✗	}
170
171		✗	void test_update_force(ContactModelTypes::Type contact_type,
172			PinocchioModelTypes::Type model_type) {
173			// create the model
174		✗	ContactModelFactory factory;
175			std::shared_ptr<crocoddyl::ContactModelAbstract> model =
176		✗	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
177
178			// create the corresponding data object
179			const std::shared_ptr<pinocchio::Model>& pinocchio_model =
180		✗	model->get_state()->get_pinocchio();
181		✗	pinocchio::Data pinocchio_data(*pinocchio_model.get());
182			const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
183		✗	model->createData(&pinocchio_data);
184
185			// Create a random force and update it
186		✗	Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
187		✗	model->updateForce(data, f);
188
189			// Check that nothing has been computed and that all value are initialized to
190			// 0
191		✗	BOOST_CHECK(data->Jc.isZero());
192		✗	BOOST_CHECK(data->a0.isZero());
193		✗	BOOST_CHECK(data->da0_dx.isZero());
194		✗	BOOST_CHECK(!data->f.toVector().isZero());
195		✗	BOOST_CHECK(!data->fext.toVector().isZero());
196		✗	BOOST_CHECK(data->df_dx.isZero());
197		✗	BOOST_CHECK(data->df_du.isZero());
198
199			// Checking that casted computation is the same
200			#ifdef NDEBUG // Run only in release mode
201			std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
202			model->cast<float>();
203			const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
204			casted_model->get_state()->get_pinocchio();
205			pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
206			const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
207			casted_model->createData(&pinocchio_data_f);
208			Eigen::VectorXf f_f = f.cast<float>();
209			casted_model->updateForce(casted_data, f_f);
210			BOOST_CHECK(casted_data->Jc.isZero());
211			BOOST_CHECK(casted_data->a0.isZero());
212			BOOST_CHECK(casted_data->da0_dx.isZero());
213			BOOST_CHECK(!casted_data->f.toVector().isZero());
214			BOOST_CHECK(!casted_data->fext.toVector().isZero());
215			BOOST_CHECK(
216			(data->f.toVector().cast<float>() - casted_data->f.toVector()).isZero());
217			BOOST_CHECK(
218			(data->fext.toVector().cast<float>() - casted_data->fext.toVector())
219			.isZero());
220			BOOST_CHECK(casted_data->df_dx.isZero());
221			BOOST_CHECK(casted_data->df_du.isZero());
222			#endif
223		✗	}
224
225		✗	void test_update_force_diff(ContactModelTypes::Type contact_type,
226			PinocchioModelTypes::Type model_type) {
227			// create the model
228		✗	ContactModelFactory factory;
229			std::shared_ptr<crocoddyl::ContactModelAbstract> model =
230		✗	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
231
232			// create the corresponding data object
233			const std::shared_ptr<pinocchio::Model>& pinocchio_model =
234		✗	model->get_state()->get_pinocchio();
235		✗	pinocchio::Data pinocchio_data(*pinocchio_model.get());
236			const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
237		✗	model->createData(&pinocchio_data);
238
239			// Create a random force and update it
240			Eigen::MatrixXd df_dx =
241		✗	Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
242			Eigen::MatrixXd df_du =
243		✗	Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols());
244		✗	model->updateForceDiff(data, df_dx, df_du);
245
246			// Check that nothing has been computed and that all value are initialized to
247			// 0
248		✗	BOOST_CHECK(data->Jc.isZero());
249		✗	BOOST_CHECK(data->a0.isZero());
250		✗	BOOST_CHECK(data->da0_dx.isZero());
251		✗	BOOST_CHECK(data->f.toVector().isZero());
252		✗	BOOST_CHECK(data->fext.toVector().isZero());
253		✗	BOOST_CHECK(!data->df_dx.isZero());
254		✗	BOOST_CHECK(!data->df_du.isZero());
255
256			// Checking that casted computation is the same
257			#ifdef NDEBUG // Run only in release mode
258			std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
259			model->cast<float>();
260			const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
261			casted_model->get_state()->get_pinocchio();
262			pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
263			const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
264			casted_model->createData(&pinocchio_data_f);
265			Eigen::MatrixXf df_dx_f = df_dx.cast<float>();
266			Eigen::MatrixXf df_du_f = df_du.cast<float>();
267			casted_model->updateForceDiff(casted_data, df_dx_f, df_du_f);
268			BOOST_CHECK(casted_data->Jc.isZero());
269			BOOST_CHECK(casted_data->a0.isZero());
270			BOOST_CHECK(casted_data->da0_dx.isZero());
271			BOOST_CHECK(casted_data->f.toVector().isZero());
272			BOOST_CHECK(casted_data->fext.toVector().isZero());
273			BOOST_CHECK(!casted_data->df_dx.isZero());
274			BOOST_CHECK(!casted_data->df_du.isZero());
275			BOOST_CHECK((data->df_dx.cast<float>() - casted_data->df_dx).isZero());
276			BOOST_CHECK((data->df_du.cast<float>() - casted_data->df_du).isZero());
277			#endif
278		✗	}
279
280		✗	void test_partial_derivatives_against_numdiff(
281			ContactModelTypes::Type contact_type,
282			PinocchioModelTypes::Type model_type) {
283			using namespace boost::placeholders;
284
285			// create the model
286		✗	ContactModelFactory factory;
287			std::shared_ptr<crocoddyl::ContactModelAbstract> model =
288		✗	factory.create(contact_type, model_type, Eigen::Vector2d::Random());
289
290			// create the corresponding data object
291			pinocchio::Model& pinocchio_model =
292		✗	*model->get_state()->get_pinocchio().get();
293		✗	pinocchio::Data pinocchio_data(pinocchio_model);
294			const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
295		✗	model->createData(&pinocchio_data);
296
297			// Create the equivalent num diff model and data.
298		✗	crocoddyl::ContactModelNumDiff model_num_diff(model);
299			const std::shared_ptr<crocoddyl::ContactDataAbstract>& data_num_diff =
300		✗	model_num_diff.createData(&pinocchio_data);
301
302			// Generating random values for the state
303		✗	const Eigen::VectorXd x = model->get_state()->rand();
304
305			// Compute all the pinocchio function needed for the models.
306		✗	crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
307
308			// set the function that needs to be called at every step of the numdiff
309		✗	std::vector<crocoddyl::ContactModelNumDiff::ReevaluationFunction> reevals;
310		✗	reevals.push_back(
311			boost::bind(&crocoddyl::unittest::updateAllPinocchio<
312			double, 0, pinocchio::JointCollectionDefaultTpl>,
313			&pinocchio_model, &pinocchio_data, _1, _2));
314		✗	model_num_diff.set_reevals(reevals);
315
316			// Computing the contact derivatives
317		✗	model->calc(data, x);
318		✗	model->calcDiff(data, x);
319		✗	model_num_diff.calc(data_num_diff, x);
320		✗	model_num_diff.calcDiff(data_num_diff, x);
321			// Tolerance defined as in
322			// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
323		✗	double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
324		✗	BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isZero(tol));
325
326			// Checking that casted computation is the same
327			#ifdef NDEBUG // Run only in release mode
328			std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
329			model->cast<float>();
330			pinocchio::ModelTpl<float>& pinocchio_model_f =
331			*casted_model->get_state()->get_pinocchio().get();
332			pinocchio::DataTpl<float> pinocchio_data_f(pinocchio_model_f);
333			const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
334			casted_model->createData(&pinocchio_data_f);
335			const Eigen::VectorXf x_f = x.cast<float>();
336			crocoddyl::ContactModelNumDiffTpl<float> casted_model_num_diff =
337			model_num_diff.cast<float>();
338			const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>&
339			casted_data_num_diff =
340			casted_model_num_diff.createData(&pinocchio_data_f);
341			crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
342			crocoddyl::unittest::updateAllPinocchio(&pinocchio_model_f, &pinocchio_data_f,
343			x_f);
344			std::vector<crocoddyl::ContactModelNumDiffTpl<float>::ReevaluationFunction>
345			reevals_f;
346			reevals_f.push_back(
347			boost::bind(&crocoddyl::unittest::updateAllPinocchio<
348			float, 0, pinocchio::JointCollectionDefaultTpl>,
349			&pinocchio_model_f, &pinocchio_data_f, _1, _2));
350			casted_model_num_diff.set_reevals(reevals_f);
351			model->calc(data, x);
352			model->calcDiff(data, x);
353			casted_model->calc(casted_data, x_f);
354			casted_model->calcDiff(casted_data, x_f);
355			casted_model_num_diff.calc(casted_data_num_diff, x_f);
356			casted_model_num_diff.calcDiff(casted_data_num_diff, x_f);
357			float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
358			BOOST_CHECK((data->da0_dx.cast<float>() - casted_data->da0_dx).isZero(tol_f));
359			BOOST_CHECK((casted_data->da0_dx - casted_data_num_diff->da0_dx)
360			.isZero(30.f * tol_f));
361			#endif
362		✗	}
363
364			//----------------------------------------------------------------------------//
365
366		✗	void register_contact_model_unit_tests(ContactModelTypes::Type contact_type,
367			PinocchioModelTypes::Type model_type) {
368		✗	boost::test_tools::output_test_stream test_name;
369		✗	test_name << "test_" << contact_type << "_" << model_type;
370		✗	std::cout << "Running " << test_name.str() << std::endl;
371		✗	test_suite* ts = BOOST_TEST_SUITE(test_name.str());
372		✗	ts->add(BOOST_TEST_CASE(
373			boost::bind(&test_construct_data, contact_type, model_type)));
374		✗	ts->add(BOOST_TEST_CASE(
375			boost::bind(&test_calc_fetch_jacobians, contact_type, model_type)));
376		✗	ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
377			contact_type, model_type)));
378		✗	ts->add(BOOST_TEST_CASE(
379			boost::bind(&test_update_force, contact_type, model_type)));
380		✗	ts->add(BOOST_TEST_CASE(
381			boost::bind(&test_update_force_diff, contact_type, model_type)));
382		✗	ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
383			contact_type, model_type)));
384		✗	framework::master_test_suite().add(ts);
385		✗	}
386
387		✗	bool init_function() {
388		✗	for (size_t contact_type = 0; contact_type < ContactModelTypes::all.size();
389			++contact_type) {
390		✗	for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
391			++model_type) {
392		✗	register_contact_model_unit_tests(ContactModelTypes::all[contact_type],
393		✗	PinocchioModelTypes::all[model_type]);
394			}
395			}
396		✗	return true;
397			}
398
399		✗	int main(int argc, char** argv) {
400		✗	return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
401			}
402