GCC Code Coverage Report

Directory: ./
File: unittest/test_impulses.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 82 0.0%
Branches: 0 458 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,
6 // INRIA, University of Edinburgh,
7 // Heriot-Watt University
8 // Copyright note valid unless otherwise stated in individual files.
9 // All rights reserved.
10 ///////////////////////////////////////////////////////////////////////////////
11
12 #define BOOST_TEST_NO_MAIN
13 #define BOOST_TEST_ALTERNATIVE_INIT_API
14
15 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
16 #include "crocoddyl/multibody/impulses/impulse-6d.hpp"
17 #include "factory/impulse.hpp"
18 #include "unittest_common.hpp"
19
20 using namespace boost::unit_test;
21 using namespace crocoddyl::unittest;
22
23 //----------------------------------------------------------------------------//
24
25 ✗ void test_construct_data(ImpulseModelTypes::Type impulse_type,
26 PinocchioModelTypes::Type model_type) {
27 // create the model
28 ✗ ImpulseModelFactory factory;
29 std::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
30 ✗ factory.create(impulse_type, model_type);
31
32 // Run the print function
33 ✗ std::ostringstream tmp;
34 ✗ tmp << *model;
35
36 // create the corresponding data object
37 ✗ pinocchio::Data pinocchio_data(*model->get_state()->get_pinocchio().get());
38 std::shared_ptr<crocoddyl::ImpulseDataAbstract> data =
39 ✗ model->createData(&pinocchio_data);
40 }
41
42 ✗ void test_calc_fetch_jacobians(ImpulseModelTypes::Type impulse_type,
43 PinocchioModelTypes::Type model_type) {
44 // create the model
45 ✗ ImpulseModelFactory factory;
46 std::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
47 ✗ factory.create(impulse_type, model_type);
48
49 // create the corresponding data object
50 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
51 ✗ model->get_state()->get_pinocchio();
52 ✗ pinocchio::Data pinocchio_data(*pinocchio_model.get());
53 const std::shared_ptr<crocoddyl::ImpulseDataAbstract>& data =
54 ✗ model->createData(&pinocchio_data);
55
56 // Compute the jacobian and check that the impulse model fetch it.
57 ✗ Eigen::VectorXd x = model->get_state()->rand();
58 ✗ crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
59 &pinocchio_data, x);
60
61 // Getting the jacobian from the model
62 ✗ Eigen::VectorXd dx;
63 ✗ model->calc(data, dx);
64
65 // Check that only the Jacobian has been filled
66 ✗ BOOST_CHECK(!data->Jc.isZero());
67 ✗ BOOST_CHECK(data->dv0_dq.isZero());
68 ✗ BOOST_CHECK(data->f.toVector().isZero());
69 ✗ BOOST_CHECK(data->df_dx.isZero());
70
71 // Checking that casted computation is the same
72 #ifdef NDEBUG // Run only in release mode
73 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>> casted_model =
74 model->cast<float>();
75 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
76 casted_model->get_state()->get_pinocchio();
77 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
78 const std::shared_ptr<crocoddyl::ImpulseDataAbstractTpl<float>>& casted_data =
79 casted_model->createData(&pinocchio_data_f);
80 Eigen::VectorXf x_f = x.cast<float>();
81 crocoddyl::unittest::updateAllPinocchio(pinocchio_model_f.get(),
82 &pinocchio_data_f, x_f);
83 Eigen::VectorXf dx_f;
84 casted_model->calc(casted_data, dx_f);
85 BOOST_CHECK(!casted_data->Jc.isZero());
86 BOOST_CHECK((data->Jc.cast<float>() - casted_data->Jc).isZero());
87 BOOST_CHECK(casted_data->dv0_dq.isZero());
88 BOOST_CHECK(casted_data->f.toVector().isZero());
89 BOOST_CHECK(casted_data->df_dx.isZero());
90 #endif
91 }
92
93 ✗ void test_calc_diff_fetch_derivatives(ImpulseModelTypes::Type impulse_type,
94 PinocchioModelTypes::Type model_type) {
95 // create the model
96 ✗ ImpulseModelFactory factory;
97 std::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
98 ✗ factory.create(impulse_type, model_type);
99
100 // create the corresponding data object
101 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
102 ✗ model->get_state()->get_pinocchio();
103 ✗ pinocchio::Data pinocchio_data(*pinocchio_model.get());
104 const std::shared_ptr<crocoddyl::ImpulseDataAbstract>& data =
105 ✗ model->createData(&pinocchio_data);
106
107 // Compute the jacobian and check that the impulse model fetch it.
108 ✗ Eigen::VectorXd x = model->get_state()->rand();
109 ✗ crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
110 &pinocchio_data, x);
111
112 // Getting the jacobian from the model
113 ✗ Eigen::VectorXd dx;
114 ✗ model->calc(data, dx);
115 ✗ model->calcDiff(data, dx);
116
117 // Check that nothing has been computed and that all value are initialized to
118 // 0
119 ✗ BOOST_CHECK(!data->Jc.isZero());
120 ✗ if (model_type == PinocchioModelTypes::Hector &&
121 ✗ (impulse_type == ImpulseModelTypes::ImpulseModel3D_LOCAL ||
122 impulse_type ==
123 ImpulseModelTypes::ImpulseModel6D_LOCAL)) { // this is due to Hector
124 // is a single rigid
125 // body system.
126 ✗ BOOST_CHECK(data->dv0_dq.isZero());
127 } else {
128 ✗ BOOST_CHECK(!data->dv0_dq.isZero());
129 }
130 ✗ BOOST_CHECK(data->f.toVector().isZero());
131 ✗ BOOST_CHECK(data->df_dx.isZero());
132
133 // Checking that casted computation is the same
134 #ifdef NDEBUG // Run only in release mode
135 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>> casted_model =
136 model->cast<float>();
137 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
138 casted_model->get_state()->get_pinocchio();
139 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
140 const std::shared_ptr<crocoddyl::ImpulseDataAbstractTpl<float>>& casted_data =
141 casted_model->createData(&pinocchio_data_f);
142 Eigen::VectorXf x_f = x.cast<float>();
143 crocoddyl::unittest::updateAllPinocchio(pinocchio_model_f.get(),
144 &pinocchio_data_f, x_f);
145 Eigen::VectorXf dx_f;
146 casted_model->calc(casted_data, dx_f);
147 casted_model->calcDiff(casted_data, dx_f);
148 BOOST_CHECK(!casted_data->Jc.isZero());
149 BOOST_CHECK((data->Jc.cast<float>() - casted_data->Jc).isZero());
150 if (model_type == PinocchioModelTypes::Hector &&
151 (impulse_type == ImpulseModelTypes::ImpulseModel3D_LOCAL ||
152 impulse_type ==
153 ImpulseModelTypes::ImpulseModel6D_LOCAL)) { // this is due to Hector
154 // is a single rigid
155 // body system.
156 BOOST_CHECK(casted_data->dv0_dq.isZero());
157 } else {
158 BOOST_CHECK(!casted_data->dv0_dq.isZero());
159 BOOST_CHECK((data->dv0_dq.cast<float>() - casted_data->dv0_dq).isZero());
160 }
161 BOOST_CHECK(casted_data->f.toVector().isZero());
162 BOOST_CHECK(casted_data->df_dx.isZero());
163 #endif
164 }
165
166 ✗ void test_update_force(ImpulseModelTypes::Type impulse_type,
167 PinocchioModelTypes::Type model_type) {
168 // create the model
169 ✗ ImpulseModelFactory factory;
170 std::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
171 ✗ factory.create(impulse_type, model_type);
172
173 // create the corresponding data object
174 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
175 ✗ model->get_state()->get_pinocchio();
176 ✗ pinocchio::Data pinocchio_data(*pinocchio_model.get());
177 const std::shared_ptr<crocoddyl::ImpulseDataAbstract>& data =
178 ✗ model->createData(&pinocchio_data);
179
180 // Create a random force and update it
181 ✗ Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
182 ✗ model->updateForce(data, f);
183
184 // Check that nothing has been computed and that all value are initialized to
185 // 0
186 ✗ BOOST_CHECK(data->Jc.isZero());
187 ✗ BOOST_CHECK(data->dv0_dq.isZero());
188 ✗ BOOST_CHECK(!data->f.toVector().isZero());
189 ✗ BOOST_CHECK(data->df_dx.isZero());
190
191 // Checking that casted computation is the same
192 #ifdef NDEBUG // Run only in release mode
193 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>> casted_model =
194 model->cast<float>();
195 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
196 casted_model->get_state()->get_pinocchio();
197 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
198 const std::shared_ptr<crocoddyl::ImpulseDataAbstractTpl<float>>& casted_data =
199 casted_model->createData(&pinocchio_data_f);
200 Eigen::VectorXf f_f = f.cast<float>();
201 casted_model->updateForce(casted_data, f_f);
202 BOOST_CHECK(casted_data->Jc.isZero());
203 BOOST_CHECK(casted_data->dv0_dq.isZero());
204 BOOST_CHECK(!casted_data->f.toVector().isZero());
205 BOOST_CHECK(
206 (data->f.toVector().cast<float>() - casted_data->f.toVector()).isZero());
207 BOOST_CHECK(casted_data->df_dx.isZero());
208 #endif
209 }
210
211 ✗ void test_update_force_diff(ImpulseModelTypes::Type impulse_type,
212 PinocchioModelTypes::Type model_type) {
213 // create the model
214 ✗ ImpulseModelFactory factory;
215 std::shared_ptr<crocoddyl::ImpulseModelAbstract> model =
216 ✗ factory.create(impulse_type, model_type);
217
218 // create the corresponding data object
219 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
220 ✗ model->get_state()->get_pinocchio();
221 ✗ pinocchio::Data pinocchio_data(*pinocchio_model.get());
222 const std::shared_ptr<crocoddyl::ImpulseDataAbstract>& data =
223 ✗ model->createData(&pinocchio_data);
224
225 // Create a random force and update it
226 Eigen::MatrixXd df_dx =
227 ✗ Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
228 ✗ model->updateForceDiff(data, df_dx);
229
230 // Check that nothing has been computed and that all value are initialized to
231 // 0
232 ✗ BOOST_CHECK(data->Jc.isZero());
233 ✗ BOOST_CHECK(data->dv0_dq.isZero());
234 ✗ BOOST_CHECK(data->f.toVector().isZero());
235 ✗ BOOST_CHECK(!data->df_dx.isZero());
236
237 // Checking that casted computation is the same
238 #ifdef NDEBUG // Run only in release mode
239 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>> casted_model =
240 model->cast<float>();
241 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
242 casted_model->get_state()->get_pinocchio();
243 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
244 const std::shared_ptr<crocoddyl::ImpulseDataAbstractTpl<float>>& casted_data =
245 casted_model->createData(&pinocchio_data_f);
246 Eigen::MatrixXf df_dx_f = df_dx.cast<float>();
247 casted_model->updateForceDiff(casted_data, df_dx_f);
248 BOOST_CHECK(casted_data->Jc.isZero());
249 BOOST_CHECK(casted_data->dv0_dq.isZero());
250 BOOST_CHECK(casted_data->f.toVector().isZero());
251 BOOST_CHECK(!casted_data->df_dx.isZero());
252 BOOST_CHECK((data->df_dx.cast<float>() - casted_data->df_dx).isZero());
253 #endif
254 }
255
256 //----------------------------------------------------------------------------//
257
258 ✗ void register_impulse_model_unit_tests(ImpulseModelTypes::Type impulse_type,
259 PinocchioModelTypes::Type model_type) {
260 ✗ boost::test_tools::output_test_stream test_name;
261 ✗ test_name << "test_" << impulse_type << "_" << model_type;
262 ✗ std::cout << "Running " << test_name.str() << std::endl;
263 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
264 ✗ ts->add(BOOST_TEST_CASE(
265 boost::bind(&test_construct_data, impulse_type, model_type)));
266 ✗ ts->add(BOOST_TEST_CASE(
267 boost::bind(&test_calc_fetch_jacobians, impulse_type, model_type)));
268 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
269 impulse_type, model_type)));
270 ✗ ts->add(BOOST_TEST_CASE(
271 boost::bind(&test_update_force, impulse_type, model_type)));
272 ✗ ts->add(BOOST_TEST_CASE(
273 boost::bind(&test_update_force_diff, impulse_type, model_type)));
274 ✗ framework::master_test_suite().add(ts);
275 }
276
277 ✗ bool init_function() {
278 ✗ for (size_t impulse_type = 0; impulse_type < ImpulseModelTypes::all.size();
279 ++impulse_type) {
280 ✗ for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
281 ++model_type) {
282 ✗ register_impulse_model_unit_tests(ImpulseModelTypes::all[impulse_type],
283 ✗ PinocchioModelTypes::all[model_type]);
284 }
285 }
286 ✗ return true;
287 }
288
289 ✗ int main(int argc, char** argv) {
290 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
291 }
292