GCC Code Coverage Report

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