GCC Code Coverage Report

Directory: ./
File: unittest/test_actions.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 183 0.0%
Functions: 0 15 0.0%
Branches: 0 808 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, 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 "factory/action.hpp"
15 #include "factory/control.hpp"
16 #include "factory/diff_action.hpp"
17 #include "factory/integrator.hpp"
18 #include "unittest_common.hpp"
19
20 using namespace boost::unit_test;
21 using namespace crocoddyl::unittest;
22
23 //----------------------------------------------------------------------------//
24
25 ✗ void test_check_data(
26 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model) {
27 // Run the print function
28 ✗ std::ostringstream tmp;
29 ✗ tmp << *model;
30
31 // create the corresponding data object
32 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
33 ✗ model->createData();
34 ✗ BOOST_CHECK(model->checkData(data));
35
36 // Checking that casted computation is the same
37 #ifdef NDEBUG // Run only in release mode
38 std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>> casted_model =
39 model->cast<float>();
40 std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>> casted_data =
41 casted_model->createData();
42 BOOST_CHECK(casted_model->checkData(casted_data));
43 #endif
44 ✗ }
45
46 ✗ void test_calc(const std::shared_ptr<crocoddyl::ActionModelAbstract>& model) {
47 // create the corresponding data object
48 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
49 ✗ model->createData();
50 ✗ data->cost = nan("");
51
52 // Generating random state and control vectors
53 ✗ const Eigen::VectorXd x = model->get_state()->rand();
54 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
55
56 // Getting the state dimension from calc() call
57 ✗ model->calc(data, x, u);
58 ✗ BOOST_CHECK(static_cast<std::size_t>(data->xnext.size()) ==
59 model->get_state()->get_nx());
60
61 // Checking that calc returns a cost value
62 ✗ BOOST_CHECK(!std::isnan(data->cost));
63
64 // Checking the termninal state
65 ✗ double tol = std::sqrt(2.0 * std::numeric_limits<double>::epsilon());
66 ✗ model->calc(data, x);
67 ✗ BOOST_CHECK((data->xnext - x).head(model->get_state()->get_nq()).isZero(tol));
68
69 // Checking that casted computation is the same
70 #ifdef NDEBUG // Run only in release mode
71 std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>> casted_model =
72 model->cast<float>();
73 std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>> casted_data =
74 casted_model->createData();
75 const Eigen::VectorXf x_f = x.cast<float>();
76 const Eigen::VectorXf u_f = u.cast<float>();
77 model->calc(data, x, u);
78 casted_model->calc(casted_data, x_f, u_f);
79 BOOST_CHECK(static_cast<std::size_t>(casted_data->xnext.size()) ==
80 casted_model->get_state()->get_nx());
81 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
82 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
83 #endif
84 ✗ }
85
86 ✗ void test_partial_derivatives_against_numdiff(
87 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model) {
88 // create the corresponding data object and set the cost to nan
89 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
90 ✗ model->createData();
91
92 ✗ crocoddyl::ActionModelNumDiff model_num_diff(model);
93 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data_num_diff =
94 ✗ model_num_diff.createData();
95
96 // Generating random values for the state and control
97 ✗ Eigen::VectorXd x = model->get_state()->rand();
98 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
99
100 // Computing the action derivatives
101 ✗ model->calc(data, x, u);
102 ✗ model->calcDiff(data, x, u);
103 ✗ model_num_diff.calc(data_num_diff, x, u);
104 ✗ model_num_diff.calcDiff(data_num_diff, x, u);
105 // Tolerance defined as in
106 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
107 ✗ double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
108 ✗ BOOST_CHECK((data->h - data_num_diff->h).isZero(tol));
109 ✗ BOOST_CHECK((data->g - data_num_diff->g).isZero(tol));
110 ✗ BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(tol));
111 ✗ BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(tol));
112 ✗ BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
113 ✗ BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
114 ✗ if (model_num_diff.get_with_gauss_approx()) {
115 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
116 ✗ BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
117 ✗ BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
118 }
119 ✗ BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
120 ✗ BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
121 ✗ BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
122 ✗ BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
123
124 // Computing the action derivatives
125 ✗ x = model->get_state()->rand();
126 ✗ model->calc(data, x);
127 ✗ model->calcDiff(data, x);
128 ✗ model_num_diff.calc(data_num_diff, x);
129 ✗ model_num_diff.calcDiff(data_num_diff, x);
130 ✗ BOOST_CHECK((data->h - data_num_diff->h).isZero(tol));
131 ✗ BOOST_CHECK((data->g - data_num_diff->g).isZero(tol));
132 ✗ BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
133 ✗ if (model_num_diff.get_with_gauss_approx()) {
134 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
135 }
136 ✗ BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
137 ✗ BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
138
139 // Checking that casted computation is the same
140 #ifdef NDEBUG // Run only in release mode
141 std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>> casted_model =
142 model->cast<float>();
143 std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>> casted_data =
144 casted_model->createData();
145 const Eigen::VectorXf x_f = x.cast<float>();
146 const Eigen::VectorXf u_f = u.cast<float>();
147 model->calc(data, x, u);
148 model->calcDiff(data, x, u);
149 casted_model->calc(casted_data, x_f, u_f);
150 casted_model->calcDiff(casted_data, x_f, u_f);
151 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
152 BOOST_CHECK((data->h.cast<float>() - casted_data->h).isZero(tol_f));
153 BOOST_CHECK((data->g.cast<float>() - casted_data->g).isZero(tol_f));
154 BOOST_CHECK((data->Fx.cast<float>() - casted_data->Fx).isZero(tol_f));
155 BOOST_CHECK((data->Fu.cast<float>() - casted_data->Fu).isZero(tol_f));
156 BOOST_CHECK((data->Lx.cast<float>() - casted_data->Lx).isZero(tol_f));
157 BOOST_CHECK((data->Lu.cast<float>() - casted_data->Lu).isZero(tol_f));
158 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
159 BOOST_CHECK((data->Gu.cast<float>() - casted_data->Gu).isZero(tol_f));
160 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
161 BOOST_CHECK((data->Hu.cast<float>() - casted_data->Hu).isZero(tol_f));
162 crocoddyl::ActionModelNumDiffTpl<float> casted_model_num_diff =
163 model_num_diff.cast<float>();
164 std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>
165 casted_data_num_diff = casted_model_num_diff.createData();
166 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
167 casted_model_num_diff.calcDiff(casted_data_num_diff, x_f, u_f);
168 tol_f = 80.0f * sqrt(casted_model_num_diff.get_disturbance());
169 BOOST_CHECK((casted_data->Gx - casted_data_num_diff->Gx).isZero(tol_f));
170 BOOST_CHECK((casted_data->Gu - casted_data_num_diff->Gu).isZero(tol_f));
171 BOOST_CHECK((casted_data->Hx - casted_data_num_diff->Hx).isZero(tol_f));
172 BOOST_CHECK((casted_data->Hu - casted_data_num_diff->Hu).isZero(tol_f));
173 #endif
174 ✗ }
175
176 ✗ void test_check_action_data(ActionModelTypes::Type action_model_type) {
177 // create the model
178 ✗ ActionModelFactory factory;
179 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
180 ✗ factory.create(action_model_type);
181 ✗ test_check_data(model);
182 ✗ }
183
184 ✗ void test_check_integrated_action_data(
185 DifferentialActionModelTypes::Type dam_type,
186 IntegratorTypes::Type integrator_type, ControlTypes::Type control_type) {
187 // create the differential action model
188 ✗ DifferentialActionModelFactory factory_dam;
189 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& dam =
190 ✗ factory_dam.create(dam_type);
191 // create the control discretization
192 ✗ ControlFactory factory_ctrl;
193 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>& ctrl =
194 ✗ factory_ctrl.create(control_type, dam->get_nu());
195 // create the integrator
196 ✗ IntegratorFactory factory_int;
197 const std::shared_ptr<crocoddyl::IntegratedActionModelAbstract>& model =
198 ✗ factory_int.create(integrator_type, dam, ctrl);
199 ✗ test_check_data(model);
200 ✗ }
201
202 ✗ void test_calc_action_model(ActionModelTypes::Type action_model_type) {
203 // create the model
204 ✗ ActionModelFactory factory;
205 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
206 ✗ factory.create(action_model_type);
207 ✗ test_calc(model);
208 ✗ }
209
210 ✗ void test_calc_integrated_action_model(
211 DifferentialActionModelTypes::Type dam_type,
212 IntegratorTypes::Type integrator_type, ControlTypes::Type control_type) {
213 // create the differential action model
214 ✗ DifferentialActionModelFactory factory_dam;
215 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& dam =
216 ✗ factory_dam.create(dam_type);
217 // create the control discretization
218 ✗ ControlFactory factory_ctrl;
219 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>& ctrl =
220 ✗ factory_ctrl.create(control_type, dam->get_nu());
221 // create the integrator
222 ✗ IntegratorFactory factory_int;
223 const std::shared_ptr<crocoddyl::IntegratedActionModelAbstract>& model =
224 ✗ factory_int.create(integrator_type, dam, ctrl);
225 ✗ test_calc(model);
226 ✗ }
227
228 ✗ void test_partial_derivatives_action_model(
229 ActionModelTypes::Type action_model_type) {
230 // create the model
231 ✗ ActionModelFactory factory;
232 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
233 ✗ factory.create(action_model_type);
234 ✗ test_partial_derivatives_against_numdiff(model);
235 ✗ }
236
237 ✗ void test_partial_derivatives_integrated_action_model(
238 DifferentialActionModelTypes::Type dam_type,
239 IntegratorTypes::Type integrator_type, ControlTypes::Type control_type) {
240 // create the differential action model
241 ✗ DifferentialActionModelFactory factory_dam;
242 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& dam =
243 ✗ factory_dam.create(dam_type);
244 // create the control discretization
245 ✗ ControlFactory factory_ctrl;
246 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>& ctrl =
247 ✗ factory_ctrl.create(control_type, dam->get_nu());
248 // create the integrator
249 ✗ IntegratorFactory factory_int;
250 const std::shared_ptr<crocoddyl::IntegratedActionModelAbstract>& model =
251 ✗ factory_int.create(integrator_type, dam, ctrl);
252 ✗ test_partial_derivatives_against_numdiff(model);
253 ✗ }
254
255 /**
256 * Test two action models that should provide the same result when calling calc
257 * if the first part of the control input u of model2 is equal to the control
258 * input of model1. A typical case would be an integrated action model using an
259 * Euler integration scheme, which can be coupled either with a constant control
260 * parametrization (model1) or a linear control parametrization (model2), and
261 * should thus provide the same result as long as the control input at the
262 * beginning of the step has the same value.
263 */
264 ✗ void test_calc_against_calc(
265 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model1,
266 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model2) {
267 // create the corresponding data object and set the cost to nan
268 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data1 =
269 ✗ model1->createData();
270 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data2 =
271 ✗ model2->createData();
272
273 // Generating random values for the state and control
274 ✗ const Eigen::VectorXd x = model1->get_state()->rand();
275 ✗ Eigen::VectorXd u1 = Eigen::VectorXd::Random(model1->get_nu());
276 ✗ Eigen::VectorXd u2 = Eigen::VectorXd::Random(model2->get_nu());
277 // copy u1 to the first part of u2 (assuming u2 is larger than u1)
278 ✗ u2.head(u1.size()) = u1;
279
280 // Computing the action
281 ✗ model1->calc(data1, x, u1);
282 ✗ model2->calc(data2, x, u2);
283
284 // Checking the state and cost integration
285 ✗ BOOST_CHECK((data1->xnext - data2->xnext).isZero(1e-9));
286 ✗ BOOST_CHECK(abs(data1->cost - data2->cost) < 1e-9);
287 ✗ }
288
289 ✗ void register_test_calc_integrated_action_model(
290 DifferentialActionModelTypes::Type dam_type,
291 IntegratorTypes::Type integrator_type, ControlTypes::Type control_type1,
292 ControlTypes::Type control_type2) {
293 // create the differential action model
294 ✗ DifferentialActionModelFactory factory_dam;
295 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& dam =
296 ✗ factory_dam.create(dam_type);
297 // create the control discretization
298 ✗ ControlFactory factory_ctrl;
299 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>& ctrl1 =
300 ✗ factory_ctrl.create(control_type1, dam->get_nu());
301 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>& ctrl2 =
302 ✗ factory_ctrl.create(control_type2, dam->get_nu());
303 // create the integrator
304 ✗ IntegratorFactory factory_int;
305 const std::shared_ptr<crocoddyl::IntegratedActionModelAbstract>& model1 =
306 ✗ factory_int.create(integrator_type, dam, ctrl1);
307 const std::shared_ptr<crocoddyl::IntegratedActionModelAbstract>& model2 =
308 ✗ factory_int.create(integrator_type, dam, ctrl2);
309
310 ✗ boost::test_tools::output_test_stream test_name;
311 ✗ test_name << "test_calc_integrated_action_model_" << dam_type << "_"
312 ✗ << integrator_type << "_" << control_type1 << "_" << control_type2;
313 ✗ std::cout << "Running " << test_name.str() << std::endl;
314 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
315 ✗ ts->add(
316 ✗ BOOST_TEST_CASE(boost::bind(&test_calc_against_calc, model1, model2)));
317 ✗ framework::master_test_suite().add(ts);
318 ✗ }
319
320 //----------------------------------------------------------------------------//
321
322 ✗ void register_action_model_unit_tests(
323 ActionModelTypes::Type action_model_type) {
324 ✗ boost::test_tools::output_test_stream test_name;
325 ✗ test_name << "test_" << action_model_type;
326 ✗ std::cout << "Running " << test_name.str() << std::endl;
327 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
328 ✗ ts->add(
329 ✗ BOOST_TEST_CASE(boost::bind(&test_check_action_data, action_model_type)));
330 ✗ ts->add(
331 ✗ BOOST_TEST_CASE(boost::bind(&test_calc_action_model, action_model_type)));
332 ✗ ts->add(BOOST_TEST_CASE(
333 boost::bind(&test_partial_derivatives_action_model, action_model_type)));
334 ✗ framework::master_test_suite().add(ts);
335 ✗ }
336
337 ✗ void register_integrated_action_model_unit_tests(
338 DifferentialActionModelTypes::Type dam_type,
339 IntegratorTypes::Type integrator_type, ControlTypes::Type control_type) {
340 ✗ boost::test_tools::output_test_stream test_name;
341 ✗ test_name << "test_" << dam_type << "_" << integrator_type << "_"
342 ✗ << control_type;
343 ✗ std::cout << "Running " << test_name.str() << std::endl;
344 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
345 ✗ ts->add(
346 ✗ BOOST_TEST_CASE(boost::bind(&test_check_integrated_action_data, dam_type,
347 integrator_type, control_type)));
348 ✗ ts->add(
349 ✗ BOOST_TEST_CASE(boost::bind(&test_calc_integrated_action_model, dam_type,
350 integrator_type, control_type)));
351 ✗ ts->add(BOOST_TEST_CASE(
352 boost::bind(&test_partial_derivatives_integrated_action_model, dam_type,
353 integrator_type, control_type)));
354 ✗ framework::master_test_suite().add(ts);
355 ✗ }
356
357 ✗ bool init_function() {
358 ✗ for (size_t i = 0; i < ActionModelTypes::all.size(); ++i) {
359 ✗ register_action_model_unit_tests(ActionModelTypes::all[i]);
360 }
361
362 ✗ for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
363 ✗ register_integrated_action_model_unit_tests(
364 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorEuler,
365 ControlTypes::PolyZero);
366 ✗ register_integrated_action_model_unit_tests(
367 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK2,
368 ControlTypes::PolyZero);
369 ✗ register_integrated_action_model_unit_tests(
370 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK2,
371 ControlTypes::PolyOne);
372 ✗ register_integrated_action_model_unit_tests(
373 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK3,
374 ControlTypes::PolyZero);
375 ✗ register_integrated_action_model_unit_tests(
376 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK3,
377 ControlTypes::PolyOne);
378 ✗ register_integrated_action_model_unit_tests(
379 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK3,
380 ControlTypes::PolyTwoRK3);
381 ✗ register_integrated_action_model_unit_tests(
382 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK4,
383 ControlTypes::PolyZero);
384 ✗ register_integrated_action_model_unit_tests(
385 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK4,
386 ControlTypes::PolyOne);
387 ✗ register_integrated_action_model_unit_tests(
388 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorRK4,
389 ControlTypes::PolyTwoRK4);
390 }
391
392 ✗ for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
393 ✗ register_test_calc_integrated_action_model(
394 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorEuler,
395 ControlTypes::PolyZero, ControlTypes::PolyOne);
396 ✗ register_test_calc_integrated_action_model(
397 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::IntegratorEuler,
398 ControlTypes::PolyOne, ControlTypes::PolyTwoRK4);
399 }
400 ✗ return true;
401 }
402
403 ✗ int main(int argc, char** argv) {
404 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
405 }
406