GCC Code Coverage Report

Directory: ./
File: unittest/test_costs_collision.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 1 1 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #define BOOST_TEST_NO_MAIN
11 #define BOOST_TEST_ALTERNATIVE_INIT_API
12
13 #ifdef PINOCCHIO_WITH_HPP_FCL
14 #ifdef CROCODDYL_WITH_PAIR_COLLISION
15
16 #include "crocoddyl/multibody/data/multibody.hpp"
17 #include "factory/cost.hpp"
18 #include "unittest_common.hpp"
19
20 using namespace boost::unit_test;
21 using namespace crocoddyl::unittest;
22
23 //----------------------------------------------------------------------------//
24
25 void test_calc_returns_a_cost(CostModelCollisionTypes::Type cost_type,
26 StateModelTypes::Type state_type) {
27 // create the model
28 CostModelFactory factory;
29 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
30 factory.create(cost_type, state_type);
31
32 // Run the print function
33 std::ostringstream tmp;
34 tmp << *model;
35
36 // create the corresponding data object
37 const std::shared_ptr<crocoddyl::StateMultibody>& state =
38 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
39 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
40 pinocchio::Data pinocchio_data(pinocchio_model);
41 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
42 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
43 model->createData(&shared_data);
44 data->cost = nan("");
45
46 // Generating random values for the state and control
47 const Eigen::VectorXd x = model->get_state()->rand();
48 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
49
50 // Compute all the pinocchio function needed for the models.
51 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
52
53 // Getting the cost value computed by calc()
54 model->calc(data, x, u);
55
56 // Checking that calc returns a cost value
57 BOOST_CHECK(!std::isnan(data->cost));
58 }
59
60 void test_calc_against_numdiff(CostModelCollisionTypes::Type cost_type,
61 StateModelTypes::Type state_type) {
62 // create the model
63 CostModelFactory factory;
64 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
65 factory.create(cost_type, state_type);
66
67 // create the corresponding data object
68 const std::shared_ptr<crocoddyl::StateMultibody>& state =
69 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
71 pinocchio::Data pinocchio_data(pinocchio_model);
72 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
73 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
74 model->createData(&shared_data);
75
76 // Create the equivalent num diff model and data.
77 crocoddyl::CostModelNumDiff model_num_diff(model);
78 const std::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
79 model_num_diff.createData(&shared_data);
80
81 // Generating random values for the state and control
82 const Eigen::VectorXd x = model->get_state()->rand();
83 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
84
85 // Compute all the pinocchio function needed for the models.
86 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
87
88 // Computing the cost derivatives
89 model->calc(data, x, u);
90 model_num_diff.calc(data_num_diff, x, u);
91
92 // Checking the partial derivatives against NumDiff
93 BOOST_CHECK(data->cost == data_num_diff->cost);
94 }
95
96 void test_partial_derivatives_against_numdiff(
97 CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type) {
98 using namespace boost::placeholders;
99
100 // create the model
101 CostModelFactory factory;
102 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
103 factory.create(cost_type, state_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 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
111 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
112 model->createData(&shared_data);
113
114 // Create the equivalent num diff model and data.
115 crocoddyl::CostModelNumDiff model_num_diff(model);
116 const std::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
117 model_num_diff.createData(&shared_data);
118
119 // Generating random values for the state and control
120 const Eigen::VectorXd x = model->get_state()->rand();
121 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
122
123 // Compute all the pinocchio function needed for the models.
124 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
125
126 // set the function that needs to be called at every step of the numdiff
127 std::vector<crocoddyl::CostModelNumDiff::ReevaluationFunction> reevals;
128 reevals.push_back(
129 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
130 double, 0, pinocchio::JointCollectionDefaultTpl>,
131 &pinocchio_model, &pinocchio_data, _1, _2));
132 model_num_diff.set_reevals(reevals);
133
134 // Computing the cost derivatives
135 model->calc(data, x, u);
136 model->calcDiff(data, x, u);
137 model_num_diff.calc(data_num_diff, x, u);
138 model_num_diff.calcDiff(data_num_diff, x, u);
139 // Tolerance defined as in
140 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
141 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
142 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
143 BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
144 if (model_num_diff.get_with_gauss_approx()) {
145 // The num diff is not precise enough to be tested here.
146 BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
147 BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
148 BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
149 } else {
150 BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
151 BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
152 BOOST_CHECK((data_num_diff->Luu).isZero(tol));
153 }
154 }
155
156 void test_dimensions_in_cost_sum(CostModelCollisionTypes::Type cost_type,
157 StateModelTypes::Type state_type) {
158 // create the model
159 CostModelFactory factory;
160 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
161 factory.create(cost_type, state_type);
162
163 // create the corresponding data object
164 const std::shared_ptr<crocoddyl::StateMultibody>& state =
165 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
166 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
167 pinocchio::Data pinocchio_data(pinocchio_model);
168 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
169
170 // create the cost sum model
171 crocoddyl::CostModelSum cost_sum(state, model->get_nu());
172 cost_sum.addCost("myCost", model, 1.);
173
174 // Generating random values for the state and control
175 const Eigen::VectorXd x = state->rand();
176
177 // Compute all the pinocchio function needed for the models.
178 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
179
180 BOOST_CHECK(model->get_state()->get_nx() == cost_sum.get_state()->get_nx());
181 BOOST_CHECK(model->get_state()->get_ndx() == cost_sum.get_state()->get_ndx());
182 BOOST_CHECK(model->get_nu() == cost_sum.get_nu());
183 BOOST_CHECK(model->get_state()->get_nq() == cost_sum.get_state()->get_nq());
184 BOOST_CHECK(model->get_state()->get_nv() == cost_sum.get_state()->get_nv());
185 BOOST_CHECK(model->get_activation()->get_nr() == cost_sum.get_nr());
186 }
187
188 void test_partial_derivatives_in_cost_sum(
189 CostModelCollisionTypes::Type cost_type, StateModelTypes::Type state_type) {
190 // create the model
191 CostModelFactory factory;
192 const std::shared_ptr<crocoddyl::CostModelAbstract>& model =
193 factory.create(cost_type, state_type);
194
195 // create the corresponding data object
196 const std::shared_ptr<crocoddyl::StateMultibody>& state =
197 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
198 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
199 pinocchio::Data pinocchio_data(pinocchio_model);
200 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
201 const std::shared_ptr<crocoddyl::CostDataAbstract>& data =
202 model->createData(&shared_data);
203
204 // create the cost sum model
205 crocoddyl::CostModelSum cost_sum(state, model->get_nu());
206 cost_sum.addCost("myCost", model, 1.);
207 const std::shared_ptr<crocoddyl::CostDataSum>& data_sum =
208 cost_sum.createData(&shared_data);
209
210 // Generating random values for the state and control
211 const Eigen::VectorXd x = state->rand();
212 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
213
214 // Compute all the pinocchio function needed for the models.
215 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
216
217 // Computing the cost derivatives
218 model->calc(data, x, u);
219 model->calcDiff(data, x, u);
220 cost_sum.calc(data_sum, x, u);
221 cost_sum.calcDiff(data_sum, x, u);
222
223 BOOST_CHECK((data->Lx - data_sum->Lx).isZero());
224 BOOST_CHECK((data->Lu - data_sum->Lu).isZero());
225 BOOST_CHECK((data->Lxx - data_sum->Lxx).isZero());
226 BOOST_CHECK((data->Lxu - data_sum->Lxu).isZero());
227 BOOST_CHECK((data->Luu - data_sum->Luu).isZero());
228 }
229
230 //----------------------------------------------------------------------------//
231
232 void register_cost_model_unit_tests(CostModelCollisionTypes::Type cost_type,
233 StateModelTypes::Type state_type) {
234 boost::test_tools::output_test_stream test_name;
235 test_name << "test_" << cost_type << "_2norm_barrier_" << state_type;
236 std::cout << "Running " << test_name.str() << std::endl;
237 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
238 ts->add(BOOST_TEST_CASE(
239 boost::bind(&test_calc_returns_a_cost, cost_type, state_type)));
240 ts->add(BOOST_TEST_CASE(
241 boost::bind(&test_calc_against_numdiff, cost_type, state_type)));
242 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
243 cost_type, state_type)));
244 ts->add(BOOST_TEST_CASE(
245 boost::bind(&test_dimensions_in_cost_sum, cost_type, state_type)));
246 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_in_cost_sum,
247 cost_type, state_type)));
248 framework::master_test_suite().add(ts);
249 }
250
251 bool init_function() {
252 // Test all costs available with all the activation types with all available
253 // states types.
254 for (size_t cost_type = 0; cost_type < CostModelCollisionTypes::all.size();
255 ++cost_type) {
256 register_cost_model_unit_tests(CostModelCollisionTypes::all[cost_type],
257 StateModelTypes::StateMultibody_HyQ);
258 register_cost_model_unit_tests(
259 CostModelCollisionTypes::all[cost_type],
260 StateModelTypes::StateMultibody_RandomHumanoid);
261 }
262 return true;
263 }
264
265 int main(int argc, char** argv) {
266 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
267 }
268
269 #else
270
271 1 int main(int, char**) {}
272
273 #endif // CROCODDYL_WITH_PAIR_COLLISION
274 #endif // PINOCCHIO_WITH_HPP_FCL
275