GCC Code Coverage Report

Directory: ./
File: unittest/test_residuals.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 159 0.0%
Branches: 0 624 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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/core/residuals/control.hpp"
13 #include "crocoddyl/core/residuals/joint-acceleration.hpp"
14 #include "crocoddyl/core/residuals/joint-effort.hpp"
15 #include "crocoddyl/multibody/data/multibody.hpp"
16 #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
17 #include "crocoddyl/multibody/residuals/com-position.hpp"
18 #include "crocoddyl/multibody/residuals/state.hpp"
19 #include "factory/actuation.hpp"
20 #include "factory/residual.hpp"
21 #include "unittest_common.hpp"
22
23 using namespace boost::unit_test;
24 using namespace crocoddyl::unittest;
25
26 //----------------------------------------------------------------------------//
27
28 ✗ void test_calc_returns_a_residual(ResidualModelTypes::Type residual_type,
29 StateModelTypes::Type state_type,
30 ActuationModelTypes::Type actuation_type) {
31 // Create the model
32 ✗ ResidualModelFactory residual_factory;
33 ✗ ActuationModelFactory actuation_factory;
34 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
35 ✗ actuation_factory.create(actuation_type, state_type);
36 const std::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
37 residual_factory.create(residual_type, state_type,
38 ✗ actuation_model->get_nu());
39
40 // Run the print function
41 ✗ std::ostringstream tmp;
42 ✗ tmp << *model;
43
44 // Create the corresponding shared data
45 const std::shared_ptr<crocoddyl::StateMultibody>& state =
46 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
47 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
48 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
49 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
50 ✗ actuation_model->createData();
51 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
52 ✗ actuation_data);
53
54 // create the residual data
55 const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
56 ✗ model->createData(&shared_data);
57
58 // Generating random values for the state and control
59 ✗ const Eigen::VectorXd x = model->get_state()->rand();
60 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
61
62 // Compute all the pinocchio function needed for the models.
63 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
64 ✗ crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u);
65
66 // Getting the residual value computed by calc()
67 ✗ data->r *= nan("");
68 ✗ model->calc(data, x, u);
69
70 // Checking that calc returns a residual value
71 ✗ for (std::size_t i = 0; i < model->get_nr(); ++i)
72 ✗ BOOST_CHECK(!std::isnan(data->r(i)));
73
74 // Checking that casted computation is the same
75 #ifdef NDEBUG // Run only in release mode
76 const std::shared_ptr<crocoddyl::ResidualModelAbstractTpl<float>>&
77 casted_model = model->cast<float>();
78 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
79 casted_actuation_model = actuation_model->cast<float>();
80 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
81 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
82 casted_model->get_state());
83 pinocchio::ModelTpl<float>& pinocchio_model_f =
84 *casted_state->get_pinocchio().get();
85 pinocchio::DataTpl<float> pinocchio_data_f(pinocchio_model_f);
86 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
87 casted_actuation_data = casted_actuation_model->createData();
88 crocoddyl::DataCollectorActMultibodyTpl<float> casted_shared_data(
89 &pinocchio_data_f, casted_actuation_data);
90 const std::shared_ptr<crocoddyl::ResidualDataAbstractTpl<float>>&
91 casted_data = casted_model->createData(&casted_shared_data);
92 const Eigen::VectorXf x_f = x.cast<float>();
93 const Eigen::VectorXf u_f = u.cast<float>();
94 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model_f, &pinocchio_data_f,
95 x_f);
96 crocoddyl::unittest::updateActuation(casted_actuation_model,
97 casted_actuation_data, x_f, u_f);
98 casted_data->r *= float(nan(""));
99 casted_model->calc(casted_data, x_f, u_f);
100 for (std::size_t i = 0; i < casted_model->get_nr(); ++i)
101 BOOST_CHECK(!std::isnan(casted_data->r(i)));
102 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
103 BOOST_CHECK((data->r.cast<float>() - casted_data->r).isZero(tol_f));
104 #endif
105 }
106
107 ✗ void test_calc_against_numdiff(ResidualModelTypes::Type residual_type,
108 StateModelTypes::Type state_type,
109 ActuationModelTypes::Type actuation_type) {
110 using namespace boost::placeholders;
111
112 // Create the model
113 ✗ ResidualModelFactory residual_factory;
114 ✗ ActuationModelFactory actuation_factory;
115 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
116 ✗ actuation_factory.create(actuation_type, state_type);
117 const std::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
118 residual_factory.create(residual_type, state_type,
119 ✗ actuation_model->get_nu());
120
121 // Create the corresponding shared data
122 const std::shared_ptr<crocoddyl::StateMultibody>& state =
123 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
124 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
125 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
126 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
127 ✗ actuation_model->createData();
128 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
129 ✗ actuation_data);
130
131 // Create the residual data
132 const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
133 ✗ model->createData(&shared_data);
134
135 // Create the equivalent num diff model and data.
136 ✗ crocoddyl::ResidualModelNumDiff model_num_diff(model);
137 const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff =
138 ✗ model_num_diff.createData(&shared_data);
139
140 // Generating random values for the state and control
141 ✗ const Eigen::VectorXd x = model->get_state()->rand();
142 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
143
144 // Computing the residual
145 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
146 ✗ actuation_model->calc(actuation_data, x, u);
147 ✗ model->calc(data, x, u);
148
149 // Computing the residual from num diff
150 ✗ std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals;
151 ✗ reevals.push_back(
152 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
153 double, 0, pinocchio::JointCollectionDefaultTpl>,
154 &pinocchio_model, &pinocchio_data, _1, _2));
155 ✗ reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation<double>,
156 actuation_model, actuation_data, _1, _2));
157 ✗ model_num_diff.set_reevals(reevals);
158 ✗ model_num_diff.calc(data_num_diff, x, u);
159
160 // Checking the partial derivatives against NumDiff
161 ✗ BOOST_CHECK(data->r == data_num_diff->r);
162
163 // Checking that casted computation is the same
164 #ifdef NDEBUG // Run only in release mode
165 const std::shared_ptr<crocoddyl::ResidualModelAbstractTpl<float>>&
166 casted_model = model->cast<float>();
167 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
168 casted_actuation_model = actuation_model->cast<float>();
169 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
170 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
171 casted_model->get_state());
172 pinocchio::ModelTpl<float>& casted_pinocchio_model =
173 *casted_state->get_pinocchio().get();
174 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
175 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
176 casted_actuation_data = casted_actuation_model->createData();
177 crocoddyl::DataCollectorActMultibodyTpl<float> casted_shared_data(
178 &casted_pinocchio_data, casted_actuation_data);
179 const std::shared_ptr<crocoddyl::ResidualDataAbstractTpl<float>>&
180 casted_data = casted_model->createData(&casted_shared_data);
181 const Eigen::VectorXf x_f = x.cast<float>();
182 const Eigen::VectorXf u_f = u.cast<float>();
183 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
184 &casted_pinocchio_data, x_f);
185 casted_actuation_model->calc(casted_actuation_data, x_f, u_f);
186 casted_model->calc(casted_data, x_f, u_f);
187 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
188 BOOST_CHECK((data->r.cast<float>() - casted_data->r).isZero(tol_f));
189 #endif
190 }
191
192 ✗ void test_partial_derivatives_against_numdiff(
193 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
194 ActuationModelTypes::Type actuation_type) {
195 using namespace boost::placeholders;
196
197 // Create the model
198 ✗ ResidualModelFactory residual_factory;
199 ✗ ActuationModelFactory actuation_factory;
200 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
201 ✗ actuation_factory.create(actuation_type, state_type);
202 const std::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
203 residual_factory.create(residual_type, state_type,
204 ✗ actuation_model->get_nu());
205
206 // Create the corresponding shared data
207 const std::shared_ptr<crocoddyl::StateMultibody>& state =
208 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
209 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
210 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
211 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
212 ✗ actuation_model->createData();
213 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
214 ✗ actuation_data);
215
216 // Create the residual data
217 const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
218 ✗ model->createData(&shared_data);
219
220 // Create the equivalent num diff model and data.
221 ✗ crocoddyl::ResidualModelNumDiff model_num_diff(model);
222 const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff =
223 ✗ model_num_diff.createData(&shared_data);
224
225 // Generating random values for the state and control
226 ✗ Eigen::VectorXd x = model->get_state()->rand();
227 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
228
229 // Computing the residual derivatives
230 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
231 ✗ actuation_model->calc(actuation_data, x, u);
232 ✗ actuation_model->calcDiff(actuation_data, x, u);
233 ✗ model->calc(data, x, u);
234 ✗ model->calcDiff(data, x, u);
235
236 // Computing the residual derivatives via numerical differentiation
237 ✗ std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals;
238 ✗ reevals.push_back(
239 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
240 double, 0, pinocchio::JointCollectionDefaultTpl>,
241 &pinocchio_model, &pinocchio_data, _1, _2));
242 ✗ reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation<double>,
243 actuation_model, actuation_data, _1, _2));
244 ✗ model_num_diff.set_reevals(reevals);
245 ✗ model_num_diff.calc(data_num_diff, x, u);
246 ✗ model_num_diff.calcDiff(data_num_diff, x, u);
247
248 // Checking the partial derivatives against numdiff
249 // Tolerance defined as in
250 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
251 ✗ double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
252 ✗ BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol));
253 ✗ BOOST_CHECK((data->Ru - data_num_diff->Ru).isZero(tol));
254
255 // Computing the residual derivatives
256 ✗ x = model->get_state()->rand();
257 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
258 ✗ actuation_model->calc(actuation_data, x);
259 ✗ actuation_model->calcDiff(actuation_data, x);
260
261 // Computing the residual derivatives via numerical differentiation
262 ✗ model->calc(data, x);
263 ✗ model->calcDiff(data, x);
264 ✗ model_num_diff.calc(data_num_diff, x);
265 ✗ model_num_diff.calcDiff(data_num_diff, x);
266
267 // Checking the partial derivatives against numdiff
268 ✗ BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol));
269
270 // Checking that casted computation is the same
271 #ifdef NDEBUG // Run only in release mode
272 const std::shared_ptr<crocoddyl::ResidualModelAbstractTpl<float>>&
273 casted_model = model->cast<float>();
274 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
275 casted_actuation_model = actuation_model->cast<float>();
276 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
277 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
278 casted_model->get_state());
279 pinocchio::ModelTpl<float>& casted_pinocchio_model =
280 *casted_state->get_pinocchio().get();
281 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
282 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
283 casted_actuation_data = casted_actuation_model->createData();
284 crocoddyl::DataCollectorActMultibodyTpl<float> casted_shared_data(
285 &casted_pinocchio_data, casted_actuation_data);
286 const std::shared_ptr<crocoddyl::ResidualDataAbstractTpl<float>>&
287 casted_data = casted_model->createData(&casted_shared_data);
288 const Eigen::VectorXf x_f = x.cast<float>();
289 const Eigen::VectorXf u_f = u.cast<float>();
290 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
291 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
292 &casted_pinocchio_data, x_f);
293 actuation_model->calc(actuation_data, x, u);
294 actuation_model->calcDiff(actuation_data, x, u);
295 model->calc(data, x, u);
296 model->calcDiff(data, x, u);
297 casted_actuation_model->calc(casted_actuation_data, x_f, u_f);
298 casted_actuation_model->calcDiff(casted_actuation_data, x_f, u_f);
299 casted_model->calc(casted_data, x_f, u_f);
300 casted_model->calcDiff(casted_data, x_f, u_f);
301 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
302 BOOST_CHECK((data->Rx.cast<float>() - casted_data->Rx).isZero(tol_f));
303 BOOST_CHECK((data->Ru.cast<float>() - casted_data->Ru).isZero(tol_f));
304 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
305 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
306 &casted_pinocchio_data, x_f);
307 actuation_model->calc(actuation_data, x);
308 actuation_model->calcDiff(actuation_data, x);
309 model->calc(data, x);
310 model->calcDiff(data, x);
311 casted_actuation_model->calc(casted_actuation_data, x_f);
312 casted_actuation_model->calcDiff(casted_actuation_data, x_f);
313 casted_model->calc(casted_data, x_f);
314 casted_model->calcDiff(casted_data, x_f);
315 BOOST_CHECK((data->Rx.cast<float>() - casted_data->Rx).isZero(tol_f));
316 #endif
317 }
318
319 ✗ void test_reference() {
320 ✗ ResidualModelFactory factory;
321 ✗ StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos;
322 ✗ ActuationModelTypes::Type actuation_type =
323 ActuationModelTypes::ActuationModelFloatingBase;
324 ✗ StateModelFactory state_factory;
325 ✗ ActuationModelFactory actuation_factory;
326 std::shared_ptr<crocoddyl::StateMultibody> state =
327 std::static_pointer_cast<crocoddyl::StateMultibody>(
328 ✗ state_factory.create(state_type));
329 std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
330 ✗ actuation_factory.create(actuation_type, state_type);
331
332 ✗ const std::size_t nu = actuation->get_nu();
333 ✗ const std::size_t nv = state->get_nv();
334
335 // Test reference in state residual
336 ✗ crocoddyl::ResidualModelState state_residual(state, state->rand(), nu);
337 ✗ Eigen::VectorXd x_ref = state_residual.get_state()->rand();
338 ✗ state_residual.set_reference(x_ref);
339 ✗ BOOST_CHECK((x_ref - state_residual.get_reference()).isZero());
340 // Checking that casted computation is the same
341 #ifdef NDEBUG // Run only in release mode
342 crocoddyl::ResidualModelStateTpl<float> casted_state_residual =
343 state_residual.cast<float>();
344 Eigen::VectorXf x_ref_f = casted_state_residual.get_state()->rand();
345 casted_state_residual.set_reference(x_ref_f);
346 BOOST_CHECK((x_ref_f - casted_state_residual.get_reference()).isZero());
347 #endif
348 // Test reference in control residual
349 ✗ crocoddyl::ResidualModelControl control_residual(state, nu);
350 ✗ Eigen::VectorXd u_ref = Eigen::VectorXd::Random(nu);
351 ✗ control_residual.set_reference(u_ref);
352 ✗ BOOST_CHECK((u_ref - control_residual.get_reference()).isZero());
353 // Checking that casted computation is the same
354 #ifdef NDEBUG // Run only in release mode
355 crocoddyl::ResidualModelControlTpl<float> casted_control_residual =
356 control_residual.cast<float>();
357 Eigen::VectorXf u_ref_f = Eigen::VectorXf::Random(nu);
358 casted_control_residual.set_reference(u_ref_f);
359 BOOST_CHECK((u_ref_f - casted_control_residual.get_reference()).isZero());
360 #endif
361 // Test reference in joint-acceleration residual
362 ✗ crocoddyl::ResidualModelJointAcceleration jacc_residual(state, nu);
363 ✗ Eigen::VectorXd a_ref = Eigen::VectorXd::Random(nv);
364 ✗ jacc_residual.set_reference(a_ref);
365 ✗ BOOST_CHECK((a_ref - jacc_residual.get_reference()).isZero());
366 // Checking that casted computation is the same
367 #ifdef NDEBUG // Run only in release mode
368 crocoddyl::ResidualModelJointAccelerationTpl<float> casted_jacc_residual =
369 jacc_residual.cast<float>();
370 Eigen::VectorXf a_ref_f = Eigen::VectorXf::Random(nv);
371 casted_jacc_residual.set_reference(a_ref_f);
372 BOOST_CHECK((a_ref_f - casted_jacc_residual.get_reference()).isZero());
373 #endif
374 // Test reference in joint-effort residual
375 ✗ crocoddyl::ResidualModelJointEffort jeff_residual(state, actuation, nu);
376 ✗ Eigen::VectorXd tau_ref = Eigen::VectorXd::Random(nu);
377 ✗ jeff_residual.set_reference(tau_ref);
378 ✗ BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero());
379 // Checking that casted computation is the same
380 #ifdef NDEBUG // Run only in release mode
381 crocoddyl::ResidualModelJointEffortTpl<float> casted_jeff_residual =
382 jeff_residual.cast<float>();
383 Eigen::VectorXf tau_ref_f = Eigen::VectorXf::Random(nu);
384 casted_jeff_residual.set_reference(tau_ref_f);
385 BOOST_CHECK((tau_ref_f - casted_jeff_residual.get_reference()).isZero());
386 #endif
387 // Test reference in centroidal-momentum residual
388 crocoddyl::ResidualModelCentroidalMomentum cmon_residual(
389 ✗ state, Eigen::Matrix<double, 6, 1>::Zero());
390 ✗ Eigen::Matrix<double, 6, 1> h_ref = Eigen::Matrix<double, 6, 1>::Random();
391 ✗ cmon_residual.set_reference(h_ref);
392 ✗ BOOST_CHECK((h_ref - cmon_residual.get_reference()).isZero());
393 // Checking that casted computation is the same
394 #ifdef NDEBUG // Run only in release mode
395 crocoddyl::ResidualModelCentroidalMomentumTpl<float> casted_cmon_residual =
396 cmon_residual.cast<float>();
397 Eigen::Matrix<float, 6, 1> h_ref_f = Eigen::Matrix<float, 6, 1>::Random();
398 casted_cmon_residual.set_reference(h_ref_f);
399 BOOST_CHECK((h_ref_f - casted_cmon_residual.get_reference()).isZero());
400 #endif
401 // Test reference in com-position residual
402 crocoddyl::ResidualModelCoMPosition c_residual(state,
403 ✗ Eigen::Vector3d::Zero());
404 ✗ Eigen::Vector3d c_ref = Eigen::Vector3d::Random();
405 ✗ c_residual.set_reference(c_ref);
406 ✗ BOOST_CHECK((c_ref - c_residual.get_reference()).isZero());
407 // Checking that casted computation is the same
408 #ifdef NDEBUG // Run only in release mode
409 crocoddyl::ResidualModelCoMPositionTpl<float> casted_c_residual =
410 c_residual.cast<float>();
411 Eigen::Vector3f c_ref_f = Eigen::Vector3f::Random();
412 casted_c_residual.set_reference(c_ref_f);
413 BOOST_CHECK((c_ref_f - casted_c_residual.get_reference()).isZero());
414 #endif
415 }
416
417 //----------------------------------------------------------------------------//
418
419 ✗ void register_residual_model_unit_tests(
420 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
421 ActuationModelTypes::Type actuation_type) {
422 ✗ boost::test_tools::output_test_stream test_name;
423 ✗ test_name << "test_" << residual_type << "_" << state_type << "_"
424 ✗ << actuation_type;
425 ✗ std::cout << "Running " << test_name.str() << std::endl;
426 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
427 ✗ ts->add(
428 ✗ BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_residual, residual_type,
429 state_type, actuation_type)));
430 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_against_numdiff, residual_type,
431 state_type, actuation_type)));
432 ✗ ts->add(
433 ✗ BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
434 residual_type, state_type, actuation_type)));
435 ✗ framework::master_test_suite().add(ts);
436 }
437
438 ✗ void regiter_residual_reference_unit_tests() {
439 ✗ boost::test_tools::output_test_stream test_name;
440 ✗ test_name << "test_reference";
441 ✗ std::cout << "Running " << test_name.str() << std::endl;
442 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
443 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_reference)));
444 ✗ framework::master_test_suite().add(ts);
445 }
446
447 ✗ bool init_function() {
448 // Test all residuals available with all the activation types with all
449 // available states types.
450 ✗ for (size_t residual_type = 0; residual_type < ResidualModelTypes::all.size();
451 ++residual_type) {
452 // size_t residual_type = ResidualModelTypes::ResidualModelCoMPosition;
453 ✗ for (size_t state_type =
454 ✗ StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
455 ✗ state_type < StateModelTypes::all.size(); ++state_type) {
456 ✗ for (size_t actuation_type = 0;
457 ✗ actuation_type < ActuationModelTypes::all.size(); ++actuation_type) {
458 ✗ if (ActuationModelTypes::all[actuation_type] !=
459 ActuationModelTypes::ActuationModelFloatingBaseThrusters) {
460 ✗ register_residual_model_unit_tests(
461 ✗ ResidualModelTypes::all[residual_type],
462 ✗ StateModelTypes::all[state_type],
463 ✗ ActuationModelTypes::all[actuation_type]);
464 ✗ } else if (StateModelTypes::all[state_type] !=
465 ✗ StateModelTypes::StateMultibody_TalosArm &&
466 ✗ StateModelTypes::all[state_type] !=
467 StateModelTypes::StateMultibodyContact2D_TalosArm) {
468 ✗ register_residual_model_unit_tests(
469 ✗ ResidualModelTypes::all[residual_type],
470 ✗ StateModelTypes::all[state_type],
471 ✗ ActuationModelTypes::all[actuation_type]);
472 }
473 }
474 }
475 }
476 ✗ regiter_residual_reference_unit_tests();
477 ✗ return true;
478 }
479
480 ✗ int main(int argc, char** argv) {
481 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
482 }
483