GCC Code Coverage Report

Directory: ./
File: unittest/test_constraints.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 127 0.0%
Branches: 0 660 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-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/data/multibody.hpp"
13 #include "factory/constraint.hpp"
14 #include "unittest_common.hpp"
15
16 using namespace boost::unit_test;
17 using namespace crocoddyl::unittest;
18
19 //----------------------------------------------------------------------------//
20
21 ✗ void test_calc_returns_a_residual(ConstraintModelTypes::Type constraint_type,
22 StateModelTypes::Type state_type) {
23 // create the model
24 ✗ ConstraintModelFactory factory;
25 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
26 ✗ factory.create(constraint_type, state_type);
27
28 // create the corresponding data object
29 const std::shared_ptr<crocoddyl::StateMultibody>& state =
30 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
31 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
32 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
33 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
34 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
35 ✗ model->createData(&shared_data);
36 ✗ data->g *= nan("");
37 ✗ data->h *= nan("");
38
39 // Generating random values for the state and control
40 ✗ const Eigen::VectorXd x = model->get_state()->rand();
41 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
42
43 // Compute all the pinocchio function needed for the models.
44 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
45
46 // Getting the constraint residual computed by calc()
47 ✗ model->calc(data, x, u);
48
49 // Checking that calc returns a residual vector
50 ✗ BOOST_CHECK(!data->g.hasNaN());
51 ✗ BOOST_CHECK(!data->h.hasNaN());
52
53 // Checking that casted computation is the same
54 #ifdef NDEBUG // Run only in release mode
55 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
56 casted_model = model->cast<float>();
57 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
58 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
59 casted_model->get_state());
60 pinocchio::ModelTpl<float>& casted_pinocchio_model =
61 *casted_state->get_pinocchio().get();
62 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
63 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
64 &casted_pinocchio_data);
65 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
66 casted_data = casted_model->createData(&casted_shared_data);
67 casted_data->g *= float(nan(""));
68 casted_data->h *= float(nan(""));
69 const Eigen::VectorXf x_f = x.cast<float>();
70 const Eigen::VectorXf u_f = u.cast<float>();
71 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
72 &casted_pinocchio_data, x_f);
73 casted_model->calc(casted_data, x_f, u_f);
74 BOOST_CHECK(!casted_data->g.hasNaN());
75 BOOST_CHECK(!casted_data->h.hasNaN());
76 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
77 BOOST_CHECK((data->g.cast<float>() - casted_data->g).isZero(tol_f));
78 BOOST_CHECK((data->h.cast<float>() - casted_data->h).isZero(tol_f));
79 #endif
80 }
81
82 ✗ void test_calc_against_numdiff(ConstraintModelTypes::Type constraint_type,
83 StateModelTypes::Type state_type) {
84 // create the model
85 ✗ ConstraintModelFactory factory;
86 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
87 ✗ factory.create(constraint_type, state_type);
88
89 // create the corresponding data object
90 const std::shared_ptr<crocoddyl::StateMultibody>& state =
91 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
92 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
93 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
94 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
95 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
96 ✗ model->createData(&shared_data);
97
98 // Create the equivalent num diff model and data.
99 ✗ crocoddyl::ConstraintModelNumDiff model_num_diff(model);
100 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
101 ✗ model_num_diff.createData(&shared_data);
102
103 // Generating random values for the state and control
104 ✗ const Eigen::VectorXd x = model->get_state()->rand();
105 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
106
107 // Compute all the pinocchio function needed for the models.
108 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
109
110 // Computing the cost derivatives
111 ✗ model->calc(data, x, u);
112 ✗ model_num_diff.calc(data_num_diff, x, u);
113
114 // Checking the partial derivatives against NumDiff
115 ✗ BOOST_CHECK((data->g - data_num_diff->g).isZero());
116 ✗ BOOST_CHECK((data->h - data_num_diff->h).isZero());
117
118 // Checking that casted computation is the same
119 #ifdef NDEBUG // Run only in release mode
120 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
121 casted_model = model->cast<float>();
122 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
123 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
124 casted_model->get_state());
125 pinocchio::ModelTpl<float>& casted_pinocchio_model =
126 *casted_state->get_pinocchio().get();
127 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
128 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
129 &casted_pinocchio_data);
130 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
131 casted_data = casted_model->createData(&casted_shared_data);
132 crocoddyl::ConstraintModelNumDiffTpl<float> casted_model_num_diff =
133 model_num_diff.cast<float>();
134 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
135 casted_data_num_diff =
136 casted_model_num_diff.createData(&casted_shared_data);
137 const Eigen::VectorXf x_f = x.cast<float>();
138 const Eigen::VectorXf u_f = u.cast<float>();
139 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
140 &casted_pinocchio_data, x_f);
141 casted_model->calc(casted_data, x_f, u_f);
142 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
143 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
144 BOOST_CHECK((casted_data->g - casted_data_num_diff->g).isZero(tol_f));
145 BOOST_CHECK((casted_data->h - casted_data_num_diff->h).isZero(tol_f));
146 #endif
147 }
148
149 ✗ void test_partial_derivatives_against_numdiff(
150 ConstraintModelTypes::Type constraint_type,
151 StateModelTypes::Type state_type) {
152 // create the model
153 ✗ ConstraintModelFactory factory;
154 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
155 ✗ factory.create(constraint_type, state_type);
156
157 // create the corresponding data object
158 const std::shared_ptr<crocoddyl::StateMultibody>& state =
159 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
160 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
161 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
162 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
163 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
164 ✗ model->createData(&shared_data);
165
166 // Create the equivalent num diff model and data.
167 ✗ crocoddyl::ConstraintModelNumDiff model_num_diff(model);
168 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
169 ✗ model_num_diff.createData(&shared_data);
170
171 // Generating random values for the state and control
172 ✗ Eigen::VectorXd x = model->get_state()->rand();
173 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
174
175 // Compute all the pinocchio function needed for the models.
176 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
177
178 // set the function that needs to be called at every step of the numdiff
179 using namespace boost::placeholders;
180
181 ✗ std::vector<crocoddyl::ConstraintModelNumDiff::ReevaluationFunction> reevals;
182 ✗ reevals.push_back(
183 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
184 double, 0, pinocchio::JointCollectionDefaultTpl>,
185 &pinocchio_model, &pinocchio_data, _1, _2));
186 ✗ model_num_diff.set_reevals(reevals);
187
188 // Computing the cost derivatives
189 ✗ model->calc(data, x, u);
190 ✗ model->calcDiff(data, x, u);
191
192 // Computing the cost derivatives via numerical differentiation
193 ✗ model_num_diff.calc(data_num_diff, x, u);
194 ✗ model_num_diff.calcDiff(data_num_diff, x, u);
195
196 // Checking the partial derivatives against numdiff
197 ✗ double tol = sqrt(model_num_diff.get_disturbance());
198 ✗ BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
199 ✗ BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
200 ✗ BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
201 ✗ BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
202
203 // Computing the cost derivatives
204 ✗ x = model->get_state()->rand();
205 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
206 ✗ model->calc(data, x);
207 ✗ model->calcDiff(data, x);
208
209 // Computing the cost derivatives via numerical differentiation
210 ✗ model_num_diff.calc(data_num_diff, x);
211 ✗ model_num_diff.calcDiff(data_num_diff, x);
212
213 // Checking the partial derivatives against numdiff
214 ✗ BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
215 ✗ BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
216
217 // Checking that casted computation is the same
218 #ifdef NDEBUG // Run only in release mode
219 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
220 casted_model = model->cast<float>();
221 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
222 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
223 casted_model->get_state());
224 pinocchio::ModelTpl<float>& casted_pinocchio_model =
225 *casted_state->get_pinocchio().get();
226 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
227 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
228 &casted_pinocchio_data);
229 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
230 casted_data = casted_model->createData(&casted_shared_data);
231 crocoddyl::ConstraintModelNumDiffTpl<float> casted_model_num_diff =
232 model_num_diff.cast<float>();
233 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
234 casted_data_num_diff =
235 casted_model_num_diff.createData(&casted_shared_data);
236 std::vector<crocoddyl::ConstraintModelNumDiffTpl<float>::ReevaluationFunction>
237 casted_reevals;
238 casted_reevals.push_back(
239 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
240 float, 0, pinocchio::JointCollectionDefaultTpl>,
241 &casted_pinocchio_model, &casted_pinocchio_data, _1, _2));
242 casted_model_num_diff.set_reevals(casted_reevals);
243 const Eigen::VectorXf x_f = x.cast<float>();
244 const Eigen::VectorXf u_f = u.cast<float>();
245 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
246 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
247 &casted_pinocchio_data, x_f);
248 model->calc(data, x, u);
249 model->calcDiff(data, x, u);
250 casted_model->calc(casted_data, x_f, u_f);
251 casted_model->calcDiff(casted_data, x_f, u_f);
252 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
253 casted_model_num_diff.calcDiff(casted_data_num_diff, x_f, u_f);
254 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
255 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
256 BOOST_CHECK((data->Gu.cast<float>() - casted_data->Gu).isZero(tol_f));
257 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
258 BOOST_CHECK((data->Hu.cast<float>() - casted_data->Hu).isZero(tol_f));
259 tol_f = sqrt(casted_model_num_diff.get_disturbance());
260 BOOST_CHECK((casted_data->Gx - casted_data_num_diff->Gx).isZero(tol_f));
261 BOOST_CHECK((casted_data->Gu - casted_data_num_diff->Gu).isZero(tol_f));
262 BOOST_CHECK((casted_data->Hx - casted_data_num_diff->Hx).isZero(tol_f));
263 BOOST_CHECK((casted_data->Hu - casted_data_num_diff->Hu).isZero(tol_f));
264 #endif
265 }
266
267 ✗ void test_dimensions_in_constraint_manager(
268 ConstraintModelTypes::Type constraint_type,
269 StateModelTypes::Type state_type) {
270 // create the model
271 ✗ ConstraintModelFactory factory;
272 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
273 ✗ factory.create(constraint_type, state_type);
274
275 // create the corresponding data object
276 const std::shared_ptr<crocoddyl::StateMultibody>& state =
277 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
278 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
279 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
280 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
281
282 // create the constraint manager model
283 ✗ crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
284 ✗ constraint_man.addConstraint("myConstraint", model);
285
286 // Generating random values for the state and control
287 ✗ const Eigen::VectorXd x = state->rand();
288
289 // Compute all the pinocchio function needed for the models.
290 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
291
292 ✗ BOOST_CHECK(model->get_state()->get_nx() ==
293 constraint_man.get_state()->get_nx());
294 ✗ BOOST_CHECK(model->get_state()->get_ndx() ==
295 constraint_man.get_state()->get_ndx());
296 ✗ BOOST_CHECK(model->get_nu() == constraint_man.get_nu());
297 ✗ BOOST_CHECK(model->get_state()->get_nq() ==
298 constraint_man.get_state()->get_nq());
299 ✗ BOOST_CHECK(model->get_state()->get_nv() ==
300 constraint_man.get_state()->get_nv());
301 ✗ BOOST_CHECK(model->get_ng() == constraint_man.get_ng());
302 ✗ BOOST_CHECK(model->get_nh() == constraint_man.get_nh());
303
304 // Checking that casted computation is the same
305 #ifdef NDEBUG // Run only in release mode
306 crocoddyl::ConstraintModelManagerTpl<float> casted_constraint_man =
307 constraint_man.cast<float>();
308 BOOST_CHECK(model->get_state()->get_nx() ==
309 casted_constraint_man.get_state()->get_nx());
310 BOOST_CHECK(model->get_state()->get_ndx() ==
311 casted_constraint_man.get_state()->get_ndx());
312 BOOST_CHECK(model->get_nu() == casted_constraint_man.get_nu());
313 BOOST_CHECK(model->get_state()->get_nq() ==
314 casted_constraint_man.get_state()->get_nq());
315 BOOST_CHECK(model->get_state()->get_nv() ==
316 casted_constraint_man.get_state()->get_nv());
317 BOOST_CHECK(model->get_ng() == casted_constraint_man.get_ng());
318 BOOST_CHECK(model->get_nh() == casted_constraint_man.get_nh());
319 #endif
320 }
321
322 ✗ void test_partial_derivatives_in_constraint_manager(
323 ConstraintModelTypes::Type constraint_type,
324 StateModelTypes::Type state_type) {
325 // create the model
326 ✗ ConstraintModelFactory factory;
327 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
328 ✗ factory.create(constraint_type, state_type);
329
330 // create the corresponding data object
331 const std::shared_ptr<crocoddyl::StateMultibody>& state =
332 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
333 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
334 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
335 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
336 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
337 ✗ model->createData(&shared_data);
338
339 // create the constraint manager model
340 ✗ crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
341 ✗ constraint_man.addConstraint("myConstraint", model, 1.);
342 const std::shared_ptr<crocoddyl::ConstraintDataManager>& data_man =
343 ✗ constraint_man.createData(&shared_data);
344
345 // Generating random values for the state and control
346 ✗ const Eigen::VectorXd x = state->rand();
347 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
348
349 // Compute all the pinocchio function needed for the models.
350 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
351
352 // Computing the constraint derivatives
353 ✗ model->calc(data, x, u);
354 ✗ model->calcDiff(data, x, u);
355
356 // Computing the constraint-manager derivatives
357 ✗ constraint_man.calc(data_man, x, u);
358 ✗ constraint_man.calcDiff(data_man, x, u);
359
360 ✗ BOOST_CHECK((data->Hx - data_man->Hx).isZero());
361 ✗ BOOST_CHECK((data->Hu - data_man->Hu).isZero());
362
363 // Checking that casted computation is the same
364 #ifdef NDEBUG // Run only in release mode
365 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
366 casted_model = model->cast<float>();
367 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
368 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
369 casted_model->get_state());
370 pinocchio::ModelTpl<float>& casted_pinocchio_model =
371 *casted_state->get_pinocchio().get();
372 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
373 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
374 &casted_pinocchio_data);
375 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
376 casted_data = casted_model->createData(&casted_shared_data);
377 crocoddyl::ConstraintModelManagerTpl<float> casted_constraint_man =
378 constraint_man.cast<float>();
379 const std::shared_ptr<crocoddyl::ConstraintDataManagerTpl<float>>&
380 casted_data_man = casted_constraint_man.createData(&casted_shared_data);
381 const Eigen::VectorXf x_f = x.cast<float>();
382 const Eigen::VectorXf u_f = u.cast<float>();
383 casted_model->calc(casted_data, x_f, u_f);
384 casted_model->calcDiff(casted_data, x_f, u_f);
385 casted_constraint_man.calc(casted_data_man, x_f, u_f);
386 casted_constraint_man.calcDiff(casted_data_man, x_f, u_f);
387 BOOST_CHECK((casted_data->Hx - casted_data_man->Hx).isZero());
388 BOOST_CHECK((casted_data->Hu - casted_data_man->Hu).isZero());
389 #endif
390 }
391
392 //----------------------------------------------------------------------------//
393
394 ✗ void register_constraint_model_unit_tests(
395 ConstraintModelTypes::Type constraint_type,
396 StateModelTypes::Type state_type) {
397 ✗ boost::test_tools::output_test_stream test_name;
398 ✗ test_name << "test_" << constraint_type << "_" << state_type;
399 ✗ std::cout << "Running " << test_name.str() << std::endl;
400 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
401 ✗ ts->add(BOOST_TEST_CASE(
402 boost::bind(&test_calc_returns_a_residual, constraint_type, state_type)));
403 ✗ ts->add(BOOST_TEST_CASE(
404 boost::bind(&test_calc_against_numdiff, constraint_type, state_type)));
405 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
406 constraint_type, state_type)));
407 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_dimensions_in_constraint_manager,
408 constraint_type, state_type)));
409 ✗ ts->add(BOOST_TEST_CASE(
410 boost::bind(&test_partial_derivatives_in_constraint_manager,
411 constraint_type, state_type)));
412 ✗ framework::master_test_suite().add(ts);
413 }
414
415 ✗ bool init_function() {
416 // Test all constraints available with all available states types.
417 ✗ for (size_t constraint_type = 0;
418 ✗ constraint_type < ConstraintModelTypes::all.size(); ++constraint_type) {
419 ✗ for (size_t state_type =
420 ✗ StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
421 ✗ state_type < StateModelTypes::all.size(); ++state_type) {
422 ✗ register_constraint_model_unit_tests(
423 ✗ ConstraintModelTypes::all[constraint_type],
424 ✗ StateModelTypes::all[state_type]);
425 }
426 }
427 ✗ return true;
428 }
429
430 ✗ int main(int argc, char** argv) {
431 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
432 }
433