GCC Code Coverage Report

Directory: ./
File: unittest/test_diff_actions.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 111 0.0%
Functions: 0 8 0.0%
Branches: 0 644 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, INRIA, University of
6 // Oxford, 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/diff_action.hpp"
15 #include "unittest_common.hpp"
16
17 using namespace boost::unit_test;
18 using namespace crocoddyl::unittest;
19
20 //----------------------------------------------------------------------------//
21
22 ✗ void test_check_data(DifferentialActionModelTypes::Type action_type) {
23 // create the model
24 ✗ DifferentialActionModelFactory factory;
25 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
26 ✗ factory.create(action_type);
27
28 // Run the print function
29 ✗ std::ostringstream tmp;
30 ✗ tmp << *model;
31
32 // create the corresponding data object
33 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
34 ✗ model->createData();
35
36 ✗ BOOST_CHECK(model->checkData(data));
37
38 // Checking that casted computation is the same
39 #ifdef NDEBUG // Run only in release mode
40 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
41 casted_model = model->cast<float>();
42 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
43 casted_data = casted_model->createData();
44 BOOST_CHECK(casted_model->checkData(casted_data));
45 #endif
46 ✗ }
47
48 ✗ void test_calc_returns_state(DifferentialActionModelTypes::Type action_type) {
49 // create the model
50 ✗ DifferentialActionModelFactory factory;
51 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
52 ✗ factory.create(action_type);
53
54 // create the corresponding data object
55 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
56 ✗ model->createData();
57
58 // Generating random state and control vectors
59 ✗ const Eigen::VectorXd x = model->get_state()->rand();
60 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
61
62 // Getting the state dimension from calc() call
63 ✗ model->calc(data, x, u);
64
65 ✗ BOOST_CHECK(static_cast<std::size_t>(data->xout.size()) ==
66 model->get_state()->get_nv());
67
68 // Checking that casted computation is the same
69 #ifdef NDEBUG // Run only in release mode
70 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
71 casted_model = model->cast<float>();
72 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
73 casted_data = casted_model->createData();
74 const Eigen::VectorXf x_f = x.cast<float>();
75 const Eigen::VectorXf u_f = u.cast<float>();
76 casted_model->calc(casted_data, x_f, u_f);
77 BOOST_CHECK(static_cast<std::size_t>(casted_data->xout.size()) ==
78 casted_model->get_state()->get_nv());
79 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
80 BOOST_CHECK((data->xout.cast<float>() - casted_data->xout).isZero(tol_f));
81 #endif
82 ✗ }
83
84 ✗ void test_calc_returns_a_cost(DifferentialActionModelTypes::Type action_type) {
85 // create the model
86 ✗ DifferentialActionModelFactory factory;
87 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
88 ✗ factory.create(action_type);
89
90 // create the corresponding data object and set the cost to nan
91 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
92 ✗ model->createData();
93 ✗ data->cost = nan("");
94
95 // Getting the cost value computed by calc()
96 ✗ const Eigen::VectorXd x = model->get_state()->rand();
97 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
98 ✗ model->calc(data, x, u);
99
100 // Checking that calc returns a cost value
101 ✗ BOOST_CHECK(!std::isnan(data->cost));
102
103 // Checking that casted computation is the same
104 #ifdef NDEBUG // Run only in release mode
105 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
106 casted_model = model->cast<float>();
107 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
108 casted_data = casted_model->createData();
109 casted_data->cost = float(nan(""));
110 const Eigen::VectorXf x_f = x.cast<float>();
111 const Eigen::VectorXf u_f = u.cast<float>();
112 casted_model->calc(casted_data, x_f, u_f);
113 BOOST_CHECK(!std::isnan(casted_data->cost));
114 float tol_f = 50.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
115 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
116 #endif
117 ✗ }
118
119 ✗ void test_quasi_static(DifferentialActionModelTypes::Type action_type) {
120 ✗ if (action_type ==
121 DifferentialActionModelTypes::
122 DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed)
123 ✗ return;
124 // create the model
125 ✗ DifferentialActionModelFactory factory;
126 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
127 ✗ factory.create(action_type, false);
128
129 // create the corresponding data object and set the cost to nan
130 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
131 ✗ model->createData();
132
133 // Getting the cost value computed by calc()
134 ✗ Eigen::VectorXd x = model->get_state()->rand();
135 ✗ x.tail(model->get_state()->get_nv()).setZero();
136 ✗ Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
137 ✗ model->quasiStatic(data, u, x);
138 ✗ model->calc(data, x, u);
139
140 // Check for inactive contacts
141 ✗ if (action_type == DifferentialActionModelTypes::
142 ✗ DifferentialActionModelContactFwdDynamics_HyQ ||
143 action_type ==
144 DifferentialActionModelTypes::
145 ✗ DifferentialActionModelContactFwdDynamicsWithFriction_HyQ ||
146 action_type == DifferentialActionModelTypes::
147 ✗ DifferentialActionModelContactFwdDynamics_Talos ||
148 action_type ==
149 DifferentialActionModelTypes::
150 ✗ DifferentialActionModelContactFwdDynamicsWithFriction_Talos ||
151 action_type == DifferentialActionModelTypes::
152 ✗ DifferentialActionModelContactInvDynamics_HyQ ||
153 action_type ==
154 DifferentialActionModelTypes::
155 ✗ DifferentialActionModelContactInvDynamicsWithFriction_HyQ ||
156 action_type == DifferentialActionModelTypes::
157 ✗ DifferentialActionModelContactInvDynamics_Talos ||
158 action_type ==
159 DifferentialActionModelTypes::
160 DifferentialActionModelContactInvDynamicsWithFriction_Talos) {
161 std::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> m =
162 std::static_pointer_cast<
163 ✗ crocoddyl::DifferentialActionModelContactFwdDynamics>(model);
164 ✗ m->get_contacts()->changeContactStatus("lf", false);
165
166 ✗ model->quasiStatic(data, u, x);
167 ✗ model->calc(data, x, u);
168
169 // Checking that the acceleration is zero as supposed to be in a quasi
170 // static condition
171 ✗ BOOST_CHECK(data->xout.norm() <= 1e-8);
172
173 // Checking that casted computation is the same
174 #ifdef NDEBUG // Run only in release mode
175 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
176 casted_model = model->cast<float>();
177 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
178 casted_data = casted_model->createData();
179 Eigen::VectorXf x_f = x.cast<float>();
180 x_f.tail(casted_model->get_state()->get_nv()).setZero();
181 Eigen::VectorXf u_f = Eigen::VectorXf::Zero(casted_model->get_nu());
182 casted_model->quasiStatic(casted_data, u_f, x_f);
183 casted_model->calc(casted_data, x_f, u_f);
184 float tol_f =
185 50.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
186 BOOST_CHECK(casted_data->xout.norm() <= tol_f);
187 BOOST_CHECK((data->xout.cast<float>() - casted_data->xout).isZero(tol_f));
188 #endif
189 ✗ }
190 ✗ }
191
192 ✗ void test_partial_derivatives_against_numdiff(
193 DifferentialActionModelTypes::Type action_type) {
194 // create the model
195 ✗ DifferentialActionModelFactory factory;
196 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
197 ✗ factory.create(action_type);
198
199 // create the corresponding data object and set the cost to nan
200 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
201 ✗ model->createData();
202
203 ✗ crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
204 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data_num_diff =
205 ✗ model_num_diff.createData();
206
207 // Generating random values for the state and control
208 ✗ Eigen::VectorXd x = model->get_state()->rand();
209 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
210
211 // Computing the action derivatives
212 ✗ model->calc(data, x, u);
213 ✗ model->calcDiff(data, x, u);
214 ✗ model_num_diff.calc(data_num_diff, x, u);
215 ✗ model_num_diff.calcDiff(data_num_diff, x, u);
216 // Tolerance defined as in
217 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
218 ✗ double tol = 2. * std::pow(model_num_diff.get_disturbance(), 1. / 3.);
219 ✗ BOOST_CHECK((data->h - data_num_diff->h).isZero(tol));
220 ✗ BOOST_CHECK((data->g - data_num_diff->g).isZero(tol));
221 ✗ BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(tol));
222 ✗ BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(tol));
223 ✗ BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
224 ✗ BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
225 ✗ if (model_num_diff.get_with_gauss_approx()) {
226 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
227 ✗ BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
228 ✗ BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
229 }
230 ✗ BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
231 ✗ BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
232 ✗ BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
233 ✗ BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
234
235 // Computing the action derivatives
236 ✗ x = model->get_state()->rand();
237 ✗ model->calc(data, x);
238 ✗ model->calcDiff(data, x);
239 ✗ model_num_diff.calc(data_num_diff, x);
240 ✗ model_num_diff.calcDiff(data_num_diff, x);
241 ✗ BOOST_CHECK((data->h - data_num_diff->h).isZero(tol));
242 ✗ BOOST_CHECK((data->g - data_num_diff->g).isZero(tol));
243 ✗ BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
244 ✗ if (model_num_diff.get_with_gauss_approx()) {
245 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
246 }
247 ✗ BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
248 ✗ BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
249
250 // Checking that casted computation is the same
251 #ifdef NDEBUG // Run only in release mode
252 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
253 casted_model = model->cast<float>();
254 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
255 casted_data = casted_model->createData();
256 const Eigen::VectorXf x_f = x.cast<float>();
257 const Eigen::VectorXf u_f = u.cast<float>();
258 model->calc(data, x, u);
259 model->calcDiff(data, x, u);
260 casted_model->calc(casted_data, x_f, u_f);
261 casted_model->calcDiff(casted_data, x_f, u_f);
262 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
263 BOOST_CHECK((data->h.cast<float>() - casted_data->h).isZero(tol_f));
264 BOOST_CHECK((data->g.cast<float>() - casted_data->g).isZero(tol_f));
265 BOOST_CHECK((data->Fx.cast<float>() - casted_data->Fx).isZero(tol_f));
266 BOOST_CHECK((data->Fu.cast<float>() - casted_data->Fu).isZero(tol_f));
267 BOOST_CHECK((data->Lx.cast<float>() - casted_data->Lx).isZero(tol_f));
268 BOOST_CHECK((data->Lu.cast<float>() - casted_data->Lu).isZero(tol_f));
269 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
270 BOOST_CHECK((data->Gu.cast<float>() - casted_data->Gu).isZero(tol_f));
271 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
272 BOOST_CHECK((data->Hu.cast<float>() - casted_data->Hu).isZero(tol_f));
273 crocoddyl::DifferentialActionModelNumDiffTpl<float> casted_model_num_diff =
274 model_num_diff.cast<float>();
275 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
276 casted_data_num_diff = casted_model_num_diff.createData();
277 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
278 casted_model_num_diff.calcDiff(casted_data_num_diff, x_f, u_f);
279 tol_f = 80.0f * sqrt(casted_model_num_diff.get_disturbance());
280 BOOST_CHECK((casted_data->Gx - casted_data_num_diff->Gx).isZero(tol_f));
281 BOOST_CHECK((casted_data->Gu - casted_data_num_diff->Gu).isZero(tol_f));
282 BOOST_CHECK((casted_data->Hx - casted_data_num_diff->Hx).isZero(tol_f));
283 BOOST_CHECK((casted_data->Hu - casted_data_num_diff->Hu).isZero(tol_f));
284 #endif
285 ✗ }
286
287 //----------------------------------------------------------------------------//
288
289 ✗ void register_action_model_unit_tests(
290 DifferentialActionModelTypes::Type action_type) {
291 ✗ boost::test_tools::output_test_stream test_name;
292 ✗ test_name << "test_" << action_type;
293 ✗ std::cout << "Running " << test_name.str() << std::endl;
294 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
295 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_check_data, action_type)));
296 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, action_type)));
297 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, action_type)));
298 ✗ ts->add(BOOST_TEST_CASE(
299 boost::bind(&test_partial_derivatives_against_numdiff, action_type)));
300 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_quasi_static, action_type)));
301 ✗ framework::master_test_suite().add(ts);
302 ✗ }
303
304 ✗ bool init_function() {
305 ✗ for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
306 ✗ register_action_model_unit_tests(DifferentialActionModelTypes::all[i]);
307 }
308 // register_action_model_unit_tests(DifferentialActionModelTypes::DifferentialActionModelContactInvDynamicsWithFriction_Talos);
309 // register_action_model_unit_tests(DifferentialActionModelTypes::DifferentialActionModelContactInvDynamics_TalosArm);
310 // register_action_model_unit_tests(DifferentialActionModelTypes::DifferentialActionModelContactInvDynamics_HyQ);
311 ✗ return true;
312 }
313
314 ✗ int main(int argc, char** argv) {
315 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
316 }
317