GCC Code Coverage Report

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