GCC Code Coverage Report


Directory: ./
File: unittest/test_costs.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 138 0.0%
Branches: 0 748 0.0%

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, University of Edinburgh,
6 // INRIA
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 "crocoddyl/multibody/data/multibody.hpp"
15 #include "factory/cost.hpp"
16 #include "unittest_common.hpp"
17
18 using namespace boost::unit_test;
19 using namespace crocoddyl::unittest;
20
21 //----------------------------------------------------------------------------//
22
23 void test_calc_returns_a_cost(CostModelTypes::Type cost_type,
24 StateModelTypes::Type state_type,
25 ActivationModelTypes::Type activation_type) {
26 // create the model
27 CostModelFactory factory;
28 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
29 factory.create(cost_type, state_type, activation_type);
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<crocoddyl::StateMultibody>& state =
37 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
38 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
39 pinocchio::Data pinocchio_data(pinocchio_model);
40 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
41 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
42 model->createData(&shared_data);
43 data->cost = nan("");
44
45 // Generating random values for the state and control
46 const Eigen::VectorXd x = model->get_state()->rand();
47 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
48
49 // Compute all the pinocchio function needed for the models.
50 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
51
52 // Getting the cost value computed by calc()
53 model->calc(data, x, u);
54
55 // Checking that calc returns a cost value
56 BOOST_CHECK(!std::isnan(data->cost));
57
58 // Checking that casted computation is the same
59 #ifdef NDEBUG // Run only in release mode
60 const std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>>& casted_model =
61 model->cast<float>();
62 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
63 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
64 casted_model->get_state());
65 pinocchio::ModelTpl<float>& pinocchio_model_f =
66 *casted_state->get_pinocchio().get();
67 pinocchio::DataTpl<float> pinocchio_data_f(pinocchio_model_f);
68 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
69 &pinocchio_data_f);
70 const std::shared_ptr<crocoddyl::CostDataAbstractTpl<float>>& casted_data =
71 casted_model->createData(&casted_shared_data);
72 casted_data->cost = float(nan(""));
73 const Eigen::VectorXf x_f = x.cast<float>();
74 const Eigen::VectorXf u_f = u.cast<float>();
75 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model_f, &pinocchio_data_f,
76 x_f);
77 casted_model->calc(casted_data, x_f, u_f);
78 BOOST_CHECK(!std::isnan(casted_data->cost));
79 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
80 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
81 #endif
82 }
83
84 void test_calc_against_numdiff(CostModelTypes::Type cost_type,
85 StateModelTypes::Type state_type,
86 ActivationModelTypes::Type activation_type) {
87 // create the model
88 CostModelFactory factory;
89 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
90 factory.create(cost_type, state_type, activation_type);
91
92 // create the corresponding data object
93 const std::shared_ptr<crocoddyl::StateMultibody>& state =
94 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
95 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
96 pinocchio::Data pinocchio_data(pinocchio_model);
97 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
98 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
99 model->createData(&shared_data);
100
101 // Create the equivalent num diff model and data.
102 crocoddyl::CostModelNumDiff model_num_diff(model);
103 const std::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
104 model_num_diff.createData(&shared_data);
105
106 // Generating random values for the state and control
107 const Eigen::VectorXd x = model->get_state()->rand();
108 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
109
110 // Compute all the pinocchio function needed for the models.
111 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
112
113 // Computing the cost derivatives
114 model->calc(data, x, u);
115
116 model_num_diff.calc(data_num_diff, x, u);
117
118 // Checking the partial derivatives against NumDiff
119 BOOST_CHECK(data->cost == data_num_diff->cost);
120
121 // Checking that casted computation is the same
122 #ifdef NDEBUG // Run only in release mode
123 const std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>>& casted_model =
124 model->cast<float>();
125 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
126 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
127 casted_model->get_state());
128 pinocchio::ModelTpl<float>& casted_pinocchio_model =
129 *casted_state->get_pinocchio().get();
130 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
131 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
132 &casted_pinocchio_data);
133 const std::shared_ptr<crocoddyl::CostDataAbstractTpl<float>>& casted_data =
134 casted_model->createData(&casted_shared_data);
135 const Eigen::VectorXf x_f = x.cast<float>();
136 const Eigen::VectorXf u_f = u.cast<float>();
137 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
138 &casted_pinocchio_data, x_f);
139 casted_model->calc(casted_data, x_f, u_f);
140 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
141 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
142 #endif
143 }
144
145 void test_partial_derivatives_against_numdiff(
146 CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
147 ActivationModelTypes::Type activation_type) {
148 using namespace boost::placeholders;
149
150 // create the model
151 CostModelFactory factory;
152 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
153 factory.create(cost_type, state_type, activation_type);
154
155 // create the corresponding data object
156 const std::shared_ptr<crocoddyl::StateMultibody>& state =
157 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
158 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
159 pinocchio::Data pinocchio_data(pinocchio_model);
160 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
161 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
162 model->createData(&shared_data);
163
164 // Create the equivalent num diff model and data.
165 crocoddyl::CostModelNumDiff model_num_diff(model);
166 const std::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
167 model_num_diff.createData(&shared_data);
168
169 // Generating random values for the state and control
170 Eigen::VectorXd x = model->get_state()->rand();
171 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
172
173 // Compute all the pinocchio function needed for the models.
174 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
175
176 // set the function that needs to be called at every step of the numdiff
177 std::vector<crocoddyl::CostModelNumDiff::ReevaluationFunction> reevals;
178 reevals.push_back(
179 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
180 double, 0, pinocchio::JointCollectionDefaultTpl>,
181 &pinocchio_model, &pinocchio_data, _1, _2));
182 model_num_diff.set_reevals(reevals);
183
184 // Computing the cost derivatives
185 model->calc(data, x, u);
186 model->calcDiff(data, x, u);
187 model_num_diff.calc(data_num_diff, x, u);
188 model_num_diff.calcDiff(data_num_diff, x, u);
189 // Tolerance defined as in
190 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
191 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
192 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
193 BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
194 if (model_num_diff.get_with_gauss_approx()) {
195 // The num diff is not precise enough to be tested here.
196 BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
197 BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
198 BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
199 } else {
200 BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
201 BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
202 BOOST_CHECK((data_num_diff->Luu).isZero(tol));
203 }
204
205 // Computing the cost derivatives
206 x = model->get_state()->rand();
207 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
208 model->calc(data, x);
209 model->calcDiff(data, x);
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 tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
215 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
216 if (model_num_diff.get_with_gauss_approx()) {
217 // The num diff is not precise enough to be tested here.
218 BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
219 } else {
220 BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
221 }
222
223 // Checking that casted computation is the same
224 #ifdef NDEBUG // Run only in release mode
225 const std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>>& casted_model =
226 model->cast<float>();
227 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
228 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
229 casted_model->get_state());
230 pinocchio::ModelTpl<float>& casted_pinocchio_model =
231 *casted_state->get_pinocchio().get();
232 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
233 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
234 &casted_pinocchio_data);
235 const std::shared_ptr<crocoddyl::CostDataAbstractTpl<float>>& casted_data =
236 casted_model->createData(&casted_shared_data);
237 const Eigen::VectorXf x_f = x.cast<float>();
238 const Eigen::VectorXf u_f = u.cast<float>();
239 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
240 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
241 &casted_pinocchio_data, x_f);
242 model->calc(data, x, u);
243 model->calcDiff(data, x, u);
244 casted_model->calc(casted_data, x_f, u_f);
245 casted_model->calcDiff(casted_data, x_f, u_f);
246 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
247 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
248 BOOST_CHECK((data->Lx.cast<float>() - casted_data->Lx).isZero(tol_f));
249 BOOST_CHECK((data->Lu.cast<float>() - casted_data->Lu).isZero(tol_f));
250 BOOST_CHECK((data->Lxx.cast<float>() - casted_data->Lxx).isZero(tol_f));
251 BOOST_CHECK((data->Lxu.cast<float>() - casted_data->Lxu).isZero(tol_f));
252 BOOST_CHECK((data->Luu.cast<float>() - casted_data->Luu).isZero(tol_f));
253 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
254 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
255 &casted_pinocchio_data, x_f);
256 model->calc(data, x);
257 model->calcDiff(data, x);
258 casted_model->calc(casted_data, x_f);
259 casted_model->calcDiff(casted_data, x_f);
260 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
261 BOOST_CHECK((data->Lx.cast<float>() - casted_data->Lx).isZero(tol_f));
262 BOOST_CHECK((data->Lxx.cast<float>() - casted_data->Lxx).isZero(tol_f));
263 #endif
264 }
265
266 void test_dimensions_in_cost_sum(CostModelTypes::Type cost_type,
267 StateModelTypes::Type state_type,
268 ActivationModelTypes::Type activation_type) {
269 // create the model
270 CostModelFactory factory;
271 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
272 factory.create(cost_type, state_type, activation_type);
273
274 // create the corresponding data object
275 const std::shared_ptr<crocoddyl::StateMultibody>& state =
276 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
277 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
278 pinocchio::Data pinocchio_data(pinocchio_model);
279 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
280
281 // create the cost sum model
282 crocoddyl::CostModelSum cost_sum(state, model->get_nu());
283 cost_sum.addCost("myCost", model, 1.);
284
285 // Generating random values for the state and control
286 const Eigen::VectorXd x = state->rand();
287
288 // Compute all the pinocchio function needed for the models.
289 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
290
291 BOOST_CHECK(model->get_state()->get_nx() == cost_sum.get_state()->get_nx());
292 BOOST_CHECK(model->get_state()->get_ndx() == cost_sum.get_state()->get_ndx());
293 BOOST_CHECK(model->get_nu() == cost_sum.get_nu());
294 BOOST_CHECK(model->get_state()->get_nq() == cost_sum.get_state()->get_nq());
295 BOOST_CHECK(model->get_state()->get_nv() == cost_sum.get_state()->get_nv());
296 BOOST_CHECK(model->get_activation()->get_nr() == cost_sum.get_nr());
297
298 // Checking that casted computation is the same
299 #ifdef NDEBUG // Run only in release mode
300 crocoddyl::CostModelSumTpl<float> casted_cost_sum = cost_sum.cast<float>();
301 BOOST_CHECK(model->get_state()->get_nx() ==
302 casted_cost_sum.get_state()->get_nx());
303 BOOST_CHECK(model->get_state()->get_ndx() ==
304 casted_cost_sum.get_state()->get_ndx());
305 BOOST_CHECK(model->get_nu() == casted_cost_sum.get_nu());
306 BOOST_CHECK(model->get_state()->get_nq() ==
307 casted_cost_sum.get_state()->get_nq());
308 BOOST_CHECK(model->get_state()->get_nv() ==
309 casted_cost_sum.get_state()->get_nv());
310 BOOST_CHECK(model->get_activation()->get_nr() == casted_cost_sum.get_nr());
311 #endif
312 }
313
314 void test_partial_derivatives_in_cost_sum(
315 CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
316 ActivationModelTypes::Type activation_type) {
317 // create the model
318 CostModelFactory factory;
319 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
320 factory.create(cost_type, state_type, activation_type);
321
322 // create the corresponding data object
323 const std::shared_ptr<crocoddyl::StateMultibody>& state =
324 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
325 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
326 pinocchio::Data pinocchio_data(pinocchio_model);
327 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
328 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
329 model->createData(&shared_data);
330
331 // create the cost sum model
332 crocoddyl::CostModelSum cost_sum(state, model->get_nu());
333 cost_sum.addCost("myCost", model, 1.);
334 const std::shared_ptr<crocoddyl::CostDataSum>& data_sum =
335 cost_sum.createData(&shared_data);
336
337 // Generating random values for the state and control
338 const Eigen::VectorXd x = state->rand();
339 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
340
341 // Compute all the pinocchio function needed for the models.
342 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
343
344 // Computing the cost derivatives
345 model->calc(data, x, u);
346 model->calcDiff(data, x, u);
347 cost_sum.calc(data_sum, x, u);
348 cost_sum.calcDiff(data_sum, x, u);
349
350 BOOST_CHECK((data->Lx - data_sum->Lx).isZero());
351 BOOST_CHECK((data->Lu - data_sum->Lu).isZero());
352 BOOST_CHECK((data->Lxx - data_sum->Lxx).isZero());
353 BOOST_CHECK((data->Lxu - data_sum->Lxu).isZero());
354 BOOST_CHECK((data->Luu - data_sum->Luu).isZero());
355
356 // Checking that casted computation is the same
357 #ifdef NDEBUG // Run only in release mode
358 const std::shared_ptr<crocoddyl::CostModelAbstractTpl<float>>& casted_model =
359 model->cast<float>();
360 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
361 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
362 casted_model->get_state());
363 pinocchio::ModelTpl<float>& casted_pinocchio_model =
364 *casted_state->get_pinocchio().get();
365 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
366 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
367 &casted_pinocchio_data);
368 const std::shared_ptr<crocoddyl::CostDataAbstractTpl<float>>& casted_data =
369 casted_model->createData(&casted_shared_data);
370 crocoddyl::CostModelSumTpl<float> casted_cost_sum = cost_sum.cast<float>();
371 const std::shared_ptr<crocoddyl::CostDataSumTpl<float>>& casted_data_sum =
372 casted_cost_sum.createData(&casted_shared_data);
373 const Eigen::VectorXf x_f = x.cast<float>();
374 const Eigen::VectorXf u_f = u.cast<float>();
375 casted_model->calc(casted_data, x_f, u_f);
376 casted_model->calcDiff(casted_data, x_f, u_f);
377 casted_cost_sum.calc(casted_data_sum, x_f, u_f);
378 casted_cost_sum.calcDiff(casted_data_sum, x_f, u_f);
379 BOOST_CHECK((casted_data->Lx - casted_data_sum->Lx).isZero());
380 BOOST_CHECK((casted_data->Lu - casted_data_sum->Lu).isZero());
381 BOOST_CHECK((casted_data->Lxx - casted_data_sum->Lxx).isZero());
382 BOOST_CHECK((casted_data->Lxu - casted_data_sum->Lxu).isZero());
383 BOOST_CHECK((casted_data->Luu - casted_data_sum->Luu).isZero());
384 #endif
385 }
386
387 //----------------------------------------------------------------------------//
388
389 void register_cost_model_unit_tests(
390 CostModelTypes::Type cost_type, StateModelTypes::Type state_type,
391 ActivationModelTypes::Type activation_type) {
392 boost::test_tools::output_test_stream test_name;
393 test_name << "test_" << cost_type << "_" << activation_type << "_"
394 << state_type;
395 std::cout << "Running " << test_name.str() << std::endl;
396 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
397 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, cost_type,
398 state_type, activation_type)));
399 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_against_numdiff, cost_type,
400 state_type, activation_type)));
401 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
402 cost_type, state_type, activation_type)));
403 ts->add(BOOST_TEST_CASE(boost::bind(&test_dimensions_in_cost_sum, cost_type,
404 state_type, activation_type)));
405 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_in_cost_sum,
406 cost_type, state_type, activation_type)));
407 framework::master_test_suite().add(ts);
408 }
409
410 bool init_function() {
411 // Test all costs available with all the activation types with all available
412 // states types.
413 for (size_t cost_type = 0; cost_type < CostModelTypes::all.size();
414 ++cost_type) {
415 for (size_t state_type =
416 StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
417 state_type < StateModelTypes::all.size(); ++state_type) {
418 for (size_t activation_type = 0;
419 activation_type < ActivationModelTypes::all.size();
420 ++activation_type) {
421 register_cost_model_unit_tests(
422 CostModelTypes::all[cost_type], StateModelTypes::all[state_type],
423 ActivationModelTypes::all[activation_type]);
424 }
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