GCC Code Coverage Report

Directory: ./
File: unittest/test_multiple_impulses.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 314 370 84.9%
Branches: 634 1382 45.9%
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, INRIA,
6 // Heriot-Watt University
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 <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
16
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 /**
26 * These methods modify the return type of the model function in
27 * order to use the boost::execution_monitor::execute method which catch the
28 * assert signal
29 */
30 ✗ int calc(crocoddyl::ImpulseModelMultiple& model,
31 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data,
32 Eigen::VectorXd& dx) {
33 ✗ model.calc(data, dx);
34 ✗ return 0;
35 }
36
37 ✗ int calcDiff(crocoddyl::ImpulseModelMultiple& model,
38 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data,
39 Eigen::VectorXd& dx) {
40 ✗ model.calcDiff(data, dx);
41 ✗ return 0;
42 }
43
44 ✗ int updateForce(crocoddyl::ImpulseModelMultiple& model,
45 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data,
46 Eigen::VectorXd& dx) {
47 ✗ model.updateForce(data, dx);
48 ✗ return 0;
49 }
50
51 ✗ int updateVelocityDiff(crocoddyl::ImpulseModelMultiple& model,
52 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data,
53 const Eigen::MatrixXd& dvnext_dx) {
54 ✗ model.updateVelocityDiff(data, dvnext_dx);
55 ✗ return 0;
56 }
57
58 ✗ int updateForceDiff(crocoddyl::ImpulseModelMultiple& model,
59 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data,
60 const Eigen::MatrixXd& df_dx) {
61 ✗ model.updateForceDiff(data, df_dx);
62 ✗ return 0;
63 }
64
65 //----------------------------------------------------------------------------//
66
67 1 void test_constructor() {
68 // Setup the test
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
70 crocoddyl::ImpulseModelMultiple model(
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
73
74 // Run the print function
75
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::ostringstream tmp;
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 tmp << model;
77
78 // Test the initial size of the map
79
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
 1 BOOST_CHECK(model.get_impulses().size() == 0);
80
81 // Checking that casted computation is the same
82 #ifdef NDEBUG // Run only in release mode
83 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
84 BOOST_CHECK(casted_model.get_impulses().size() == 0);
85 #endif
86 1 }
87
88 1 void test_addImpulse() {
89 // Setup the test
90
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
91 crocoddyl::ImpulseModelMultiple model(
92
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
94
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
95
96 // add an active impulse
97 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse_1 =
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
99
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addImpulse("random_impulse_1", rand_impulse_1);
100
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 1 BOOST_CHECK(model.get_nc() == rand_impulse_1->get_nc());
101
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 1 BOOST_CHECK(model.get_nc_total() == rand_impulse_1->get_nc());
102
103 // add an inactive impulse
104 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse_2 =
105
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
106
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addImpulse("random_impulse_2", rand_impulse_2, false);
107
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 1 BOOST_CHECK(model.get_nc() == rand_impulse_1->get_nc());
108
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 1 BOOST_CHECK(model.get_nc_total() ==
109 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
110
111 // change the random impulse 2 status
112
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_2", true);
113
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 1 BOOST_CHECK(model.get_nc() ==
114 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
115
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 1 BOOST_CHECK(model.get_nc_total() ==
116 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
117
118 // change the random impulse 1 status
119
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_1", false);
120
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 1 BOOST_CHECK(model.get_nc() == rand_impulse_2->get_nc());
121
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
 1 BOOST_CHECK(model.get_nc_total() ==
122 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
123
124 // Checking that casted computation is the same
125 #ifdef NDEBUG // Run only in release mode
126 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>>
127 casted_rand_impulse_1 = rand_impulse_1->cast<float>();
128 casted_model.addImpulse("random_impulse_1", casted_rand_impulse_1);
129 BOOST_CHECK(casted_model.get_nc() == casted_rand_impulse_1->get_nc());
130 BOOST_CHECK(casted_model.get_nc_total() == casted_rand_impulse_1->get_nc());
131 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>>
132 casted_rand_impulse_2 = rand_impulse_2->cast<float>();
133 casted_model.addImpulse("random_impulse_2", casted_rand_impulse_2, false);
134 BOOST_CHECK(casted_model.get_nc() == casted_rand_impulse_1->get_nc());
135 BOOST_CHECK(casted_model.get_nc_total() ==
136 casted_rand_impulse_1->get_nc() +
137 casted_rand_impulse_2->get_nc());
138 casted_model.changeImpulseStatus("random_impulse_2", true);
139 BOOST_CHECK(casted_model.get_nc() == casted_rand_impulse_1->get_nc() +
140 casted_rand_impulse_2->get_nc());
141 BOOST_CHECK(casted_model.get_nc_total() ==
142 casted_rand_impulse_1->get_nc() +
143 casted_rand_impulse_2->get_nc());
144 casted_model.changeImpulseStatus("random_impulse_1", false);
145 BOOST_CHECK(casted_model.get_nc() == casted_rand_impulse_2->get_nc());
146 BOOST_CHECK(casted_model.get_nc_total() ==
147 casted_rand_impulse_1->get_nc() +
148 casted_rand_impulse_2->get_nc());
149 #endif
150 1 }
151
152 1 void test_addImpulse_error_message() {
153 // Setup the test
154
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
155 crocoddyl::ImpulseModelMultiple model(
156
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
157
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
158
159 // create an impulse object
160 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse =
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
162
163 // add twice the same impulse object to the container
164
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addImpulse("random_impulse", rand_impulse);
165
166 // test error message when we add a duplicate impulse
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CaptureIOStream capture_ios;
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
169
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addImpulse("random_impulse", rand_impulse);
170
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
171
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream expected_buffer;
172 expected_buffer << "Warning: we couldn't add the random_impulse impulse "
173
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "item, it already existed."
174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
175
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 1 BOOST_CHECK(capture_ios.str() == expected_buffer.str());
176
177 // test error message when we change the impulse status of an inexistent
178 // impulse
179
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
180
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("no_exist_impulse", true);
181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 expected_buffer.clear();
183 expected_buffer << "Warning: we couldn't change the status of the "
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "no_exist_impulse impulse item, it doesn't exist."
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
186
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 1 BOOST_CHECK(capture_ios.str() == expected_buffer.str());
187 1 }
188
189 1 void test_removeImpulse() {
190 // Setup the test
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
192 crocoddyl::ImpulseModelMultiple model(
193
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
194
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
195
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
196
197 // add an active impulse
198 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse =
199
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
200
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addImpulse("random_impulse", rand_impulse);
201
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 1 BOOST_CHECK(model.get_nc() == rand_impulse->get_nc());
202
203 // remove the impulse
204
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.removeImpulse("random_impulse");
205
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 1 BOOST_CHECK(model.get_nc() == 0);
206
207 // Checking that casted computation is the same
208 #ifdef NDEBUG // Run only in release mode
209 std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>>
210 casted_rand_impulse = rand_impulse->cast<float>();
211 casted_model.addImpulse("random_impulse", casted_rand_impulse);
212 BOOST_CHECK(casted_model.get_nc() == casted_rand_impulse->get_nc());
213 BOOST_CHECK(casted_model.get_nc_total() == casted_rand_impulse->get_nc());
214 casted_model.removeImpulse("random_impulse");
215 BOOST_CHECK(casted_model.get_nc() == 0);
216 BOOST_CHECK(casted_model.get_nc_total() == 0);
217 #endif
218 1 }
219
220 1 void test_removeImpulse_error_message() {
221 // Setup the test
222
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
223 crocoddyl::ImpulseModelMultiple model(
224
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
225
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
226
227 // remove a none existing impulse form the container, we expect a cout message
228 // here
229
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CaptureIOStream capture_ios;
230
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
231
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.removeImpulse("random_impulse");
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
233
234 // Test that the error message is sent.
235
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream expected_buffer;
236 expected_buffer << "Warning: we couldn't remove the random_impulse impulse "
237
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "item, it doesn't exist."
238
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
239
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
 1 BOOST_CHECK(capture_ios.str() == expected_buffer.str());
240 1 }
241
242 1 void test_calc() {
243 // Setup the test
244
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
245 crocoddyl::ImpulseModelMultiple model(
246
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
247
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
248 // create the corresponding data object
249
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
250
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
251
252 // create and add some impulse objects
253 1 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstract>> models;
254 1 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstract>> datas;
255
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
256
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
257
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
258 const std::shared_ptr<crocoddyl::ImpulseModelAbstract>& m =
259
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_impulse();
260
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 model.addImpulse(os.str(), m);
261
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
262
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 datas.push_back(m->createData(&pinocchio_data));
263 5 }
264
265 // create the data of the multiple-impulses
266 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
267
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
268
269 // compute the multiple contact data for the case when all impulses are
270 // defined as active
271
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
272
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
273 x1);
274
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calc(data, x1);
275
276 // check that only the Jacobian has been filled
277
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(!data->Jc.isZero());
278
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(data->dv0_dq.isZero());
279
280 // check Jc against single impulse computations
281 1 std::size_t ni = 0;
282
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 std::size_t nv = model.get_state()->get_nv();
283
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
284
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 const std::size_t ni_i = models[i]->get_nc();
285
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 models[i]->calc(datas[i], x1);
286
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->Jc.block(ni, 0, ni_i, nv) == datas[i]->Jc);
287 5 ni += ni_i;
288 }
289 1 ni = 0;
290
291 // compute the multiple impulse data for the case when the first three
292 // impulses are defined as active
293
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_3", false);
294
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_4", false);
295
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
296
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
297 x2);
298
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calc(data, x2);
299
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
300
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 const std::size_t ni_i = models[i]->get_nc();
301
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (i < 3) { // we need to update data because this impulses are active
302
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 models[i]->calc(datas[i], x2);
303 }
304
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->Jc.block(ni, 0, ni_i, nv) == datas[i]->Jc);
305 5 ni += ni_i;
306 }
307
308 // Checking that casted computation is the same
309 #ifdef NDEBUG // Run only in release mode
310 model.changeImpulseStatus("random_impulse_3", true);
311 model.changeImpulseStatus("random_impulse_4", true);
312 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
313 pinocchio::ModelTpl<float>& casted_pinocchio_model =
314 *casted_model.get_state()->get_pinocchio().get();
315 pinocchio::DataTpl<float> casted_pinocchio_data(
316 *casted_model.get_state()->get_pinocchio().get());
317 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>>>
318 casted_models;
319 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstractTpl<float>>>
320 casted_datas;
321 for (std::size_t i = 0; i < 5; ++i) {
322 casted_models.push_back(models[i]->cast<float>());
323 casted_datas.push_back(
324 casted_models[i]->createData(&casted_pinocchio_data));
325 }
326 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<float>> casted_data =
327 casted_model.createData(&casted_pinocchio_data);
328 const Eigen::VectorXf x1_f = x1.cast<float>();
329 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
330 x1);
331 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
332 &casted_pinocchio_data, x1_f);
333 model.calc(data, x1);
334 casted_model.calc(casted_data, x1_f);
335 BOOST_CHECK(!casted_data->Jc.isZero());
336 BOOST_CHECK(casted_data->dv0_dq.isZero());
337 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
338 BOOST_CHECK((data->Jc.cast<float>() - casted_data->Jc).isZero(tol_f));
339 BOOST_CHECK((data->dv0_dq.cast<float>() - casted_data->dv0_dq).isZero(tol_f));
340 BOOST_CHECK(casted_data->dv0_dq.isZero());
341 ni = 0;
342 nv = casted_model.get_state()->get_nv();
343 for (std::size_t i = 0; i < 5; ++i) {
344 const std::size_t ni_i = casted_models[i]->get_nc();
345 casted_models[i]->calc(casted_datas[i], x1_f);
346 BOOST_CHECK(casted_data->Jc.block(ni, 0, ni_i, nv) == casted_datas[i]->Jc);
347 ni += ni_i;
348 }
349 ni = 0;
350 #endif
351 1 }
352
353 1 void test_calc_diff() {
354 // Setup the test
355
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
356 crocoddyl::ImpulseModelMultiple model(
357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
358
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
359
360 // create the corresponding data object
361
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
362
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
363
364 // create and add some impulse objects
365 1 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstract>> models;
366 1 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstract>> datas;
367
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
368
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
369
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
370 const std::shared_ptr<crocoddyl::ImpulseModelAbstract>& m =
371
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_impulse();
372
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 model.addImpulse(os.str(), m);
373
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
374
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 datas.push_back(m->createData(&pinocchio_data));
375 5 }
376
377 // create the data of the multiple-impulses
378 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
379
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
380
381 // compute the multiple contact data for the case when all impulses are
382 // defined as active
383
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
384
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
385 x1);
386
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calc(data, x1);
387
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calcDiff(data, x1);
388
389 // Check that nothing has been computed and that all value are initialized to
390 // 0
391
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(!data->Jc.isZero());
392
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(!data->dv0_dq.isZero());
393
394 // check Jc against single impulse computations
395 1 std::size_t ni = 0;
396
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 std::size_t nv = model.get_state()->get_nv();
397
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
398
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 const std::size_t ni_i = models[i]->get_nc();
399
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 models[i]->calc(datas[i], x1);
400
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 models[i]->calcDiff(datas[i], x1);
401
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->Jc.block(ni, 0, ni_i, nv) == datas[i]->Jc);
402
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->dv0_dq.block(ni, 0, ni_i, nv) == datas[i]->dv0_dq);
403 5 ni += ni_i;
404 }
405 1 ni = 0;
406
407 // compute the multiple impulse data for the case when the first three
408 // impulses are defined as active
409
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_3", false);
410
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_4", false);
411
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
412
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
413 x2);
414
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calc(data, x2);
415
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calcDiff(data, x2);
416
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
417
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 const std::size_t ni_i = models[i]->get_nc();
418
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (i < 3) { // we need to update data because this impulses are active
419
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 models[i]->calc(datas[i], x2);
420
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 models[i]->calcDiff(datas[i], x2);
421 }
422
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->Jc.block(ni, 0, ni_i, nv) == datas[i]->Jc);
423
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->dv0_dq.block(ni, 0, ni_i, nv) == datas[i]->dv0_dq);
424 5 ni += ni_i;
425 }
426
427 // Checking that casted computation is the same
428 #ifdef NDEBUG // Run only in release mode
429 model.changeImpulseStatus("random_impulse_3", true);
430 model.changeImpulseStatus("random_impulse_4", true);
431 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
432 pinocchio::ModelTpl<float>& casted_pinocchio_model =
433 *casted_model.get_state()->get_pinocchio().get();
434 pinocchio::DataTpl<float> casted_pinocchio_data(
435 *casted_model.get_state()->get_pinocchio().get());
436 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstractTpl<float>>>
437 casted_models;
438 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstractTpl<float>>>
439 casted_datas;
440 for (std::size_t i = 0; i < 5; ++i) {
441 casted_models.push_back(models[i]->cast<float>());
442 casted_datas.push_back(
443 casted_models[i]->createData(&casted_pinocchio_data));
444 }
445 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<float>> casted_data =
446 casted_model.createData(&casted_pinocchio_data);
447 const Eigen::VectorXf x1_f = x1.cast<float>();
448 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
449 x1);
450 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
451 &casted_pinocchio_data, x1_f);
452 model.calc(data, x1);
453 model.calcDiff(data, x1);
454 casted_model.calc(casted_data, x1_f);
455 casted_model.calcDiff(casted_data, x1_f);
456 BOOST_CHECK(!casted_data->Jc.isZero());
457 BOOST_CHECK(!casted_data->dv0_dq.isZero());
458 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
459 BOOST_CHECK((data->Jc.cast<float>() - casted_data->Jc).isZero(tol_f));
460 BOOST_CHECK((data->dv0_dq.cast<float>() - casted_data->dv0_dq).isZero(tol_f));
461 ni = 0;
462 nv = casted_model.get_state()->get_nv();
463 for (std::size_t i = 0; i < 5; ++i) {
464 const std::size_t ni_i = casted_models[i]->get_nc();
465 casted_models[i]->calc(casted_datas[i], x1_f);
466 casted_models[i]->calcDiff(casted_datas[i], x1_f);
467 BOOST_CHECK(casted_data->Jc.block(ni, 0, ni_i, nv) == casted_datas[i]->Jc);
468 BOOST_CHECK(casted_data->dv0_dq.block(ni, 0, ni_i, nv) ==
469 casted_datas[i]->dv0_dq);
470 ni += ni_i;
471 }
472 ni = 0;
473 #endif
474 1 }
475
476 1 void test_calc_diff_no_recalc() {
477 // Setup the test
478
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
479 crocoddyl::ImpulseModelMultiple model(
480
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
481
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
482
483 // create the corresponding data object
484
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
485
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
486
487 // create and add some impulse objects
488 1 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstract>> models;
489 1 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstract>> datas;
490
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
491
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
492
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
493 const std::shared_ptr<crocoddyl::ImpulseModelAbstract>& m =
494
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_impulse();
495
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 model.addImpulse(os.str(), m);
496
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
497
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 datas.push_back(m->createData(&pinocchio_data));
498 5 }
499
500 // create the data of the multiple-impulses
501 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
502
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
503
504 // compute the multiple contact data for the case when all impulses are
505 // defined as active
506
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
507
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
508 x1);
509
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calcDiff(data, x1);
510
511 // Check that nothing has been computed and that all value are initialized to
512 // 0
513
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(data->Jc.isZero());
514
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(!data->dv0_dq.isZero());
515
516 // check Jc against single impulse computations
517 1 std::size_t ni = 0;
518
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 const std::size_t nv = model.get_state()->get_nv();
519
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
520
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 const std::size_t ni_i = models[i]->get_nc();
521
2/4
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 models[i]->calcDiff(datas[i], x1);
522
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 5 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 5 times.
 5 BOOST_CHECK(data->Jc.block(ni, 0, ni_i, nv).isZero());
523
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->dv0_dq.block(ni, 0, ni_i, nv) == datas[i]->dv0_dq);
524 5 ni += ni_i;
525 }
526 1 ni = 0;
527
528 // compute the multiple impulse data for the case when the first three
529 // impulses are defined as active
530
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_3", false);
531
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeImpulseStatus("random_impulse_4", false);
532
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
533
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
534 x2);
535
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 model.calcDiff(data, x2);
536
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
537
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 const std::size_t ni_i = models[i]->get_nc();
538
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (i < 3) { // we need to update data because this impulses are active
539
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 models[i]->calcDiff(datas[i], x2);
540 }
541
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 5 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 5 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 5 times.
 5 BOOST_CHECK(data->Jc.block(ni, 0, ni_i, nv).isZero());
542
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 5 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(data->dv0_dq.block(ni, 0, ni_i, nv) == datas[i]->dv0_dq);
543 5 ni += ni_i;
544 }
545 1 }
546
547 1 void test_updateForce() {
548 // Setup the test
549
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
550 crocoddyl::ImpulseModelMultiple model(
551
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
552
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
553
554 // create the corresponding data object
555
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
556
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
557
558 // create and add some impulse objects
559
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
560
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
561
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
562
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 model.addImpulse(os.str(), create_random_impulse());
563 5 }
564
565 // create the data of the multiple-impulses
566 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
567
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
568
569 // Compute the jacobian and check that the impulse model fetch it.
570
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd x = model.get_state()->rand();
571
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
572
573 // create random forces
574
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Eigen::VectorXd forces = Eigen::VectorXd::Random(model.get_nc());
575
576 // update forces
577
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.updateForce(data, forces);
578
579 // Check that nothing has been computed and that all value are initialized to
580 // 0
581
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(data->Jc.isZero());
582
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 1 times.
 1 BOOST_CHECK(data->dv0_dq.isZero());
583 1 crocoddyl::ImpulseModelMultiple::ImpulseDataContainer::iterator it_d, end_d;
584 1 for (it_d = data->impulses.begin(), end_d = data->impulses.end();
585
2/2
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 1 times.
 6 it_d != end_d; ++it_d) {
586
8/16
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 5 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 5 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 5 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 5 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 5 times.
 5 BOOST_CHECK(!it_d->second->f.toVector().isZero());
587 }
588
589 // Checking that casted computation is the same
590 #ifdef NDEBUG // Run only in release mode
591 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
592 pinocchio::ModelTpl<float>& casted_pinocchio_model =
593 *casted_model.get_state()->get_pinocchio().get();
594 pinocchio::DataTpl<float> casted_pinocchio_data(
595 *casted_model.get_state()->get_pinocchio().get());
596 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<float>> casted_data =
597 casted_model.createData(&casted_pinocchio_data);
598 const Eigen::VectorXf x_f = x.cast<float>();
599 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
600 &casted_pinocchio_data, x_f);
601 const Eigen::VectorXf forces_f = forces.cast<float>();
602 casted_model.updateForce(casted_data, forces_f);
603 BOOST_CHECK(casted_data->Jc.isZero());
604 BOOST_CHECK(casted_data->dv0_dq.isZero());
605 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
606 crocoddyl::ImpulseModelMultipleTpl<float>::ImpulseDataContainer::iterator
607 it_d_f,
608 end_d_f;
609 for (it_d_f = casted_data->impulses.begin(),
610 end_d_f = casted_data->impulses.end(), it_d = data->impulses.begin(),
611 end_d = data->impulses.end();
612 it_d_f != end_d_f || it_d != end_d; ++it_d_f, ++it_d) {
613 BOOST_CHECK(!it_d_f->second->f.toVector().isZero());
614 BOOST_CHECK((it_d->second->f.toVector().cast<float>() -
615 it_d_f->second->f.toVector())
616 .isZero(tol_f));
617 }
618 #endif
619 1 }
620
621 1 void test_updateVelocityDiff() {
622 // Setup the test
623
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
624 crocoddyl::ImpulseModelMultiple model(
625
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
626
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
627
628 // create the corresponding data object
629
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
630
631 // create and add some impulse objects
632
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
633
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
634
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
635
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 model.addImpulse(os.str(), create_random_impulse());
636 5 }
637
638 // create the data of the multiple-impulses
639 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
640
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
641
642 // create the velocity diff
643 ✗ Eigen::MatrixXd dvnext_dx = Eigen::MatrixXd::Random(
644
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
 1 model.get_state()->get_nv(), model.get_state()->get_ndx());
645
646 // call the update
647
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.updateVelocityDiff(data, dvnext_dx);
648
649 // Test
650
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
 1 BOOST_CHECK((data->dvnext_dx - dvnext_dx).isZero(1e-9));
651
652 // Checking that casted computation is the same
653 #ifdef NDEBUG // Run only in release mode
654 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
655 pinocchio::DataTpl<float> casted_pinocchio_data(
656 *casted_model.get_state()->get_pinocchio().get());
657 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<float>> casted_data =
658 casted_model.createData(&casted_pinocchio_data);
659 const Eigen::MatrixXf dvnext_dx_f = dvnext_dx.cast<float>();
660 casted_model.updateVelocityDiff(casted_data, dvnext_dx_f);
661 BOOST_CHECK((casted_data->dvnext_dx - dvnext_dx_f).isZero(1e-9f));
662 #endif
663 1 }
664
665 ✗ void test_updateForceDiff() {
666 // Setup the test
667 ✗ StateModelFactory state_factory;
668 crocoddyl::ImpulseModelMultiple model(
669 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
670 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
671
672 // create the corresponding data object
673 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
674
675 // create and add some impulse objects
676 ✗ for (unsigned i = 0; i < 5; ++i) {
677 ✗ std::ostringstream os;
678 ✗ os << "random_impulse_" << i;
679 ✗ model.addImpulse(os.str(), create_random_impulse());
680 }
681
682 // create the data of the multiple-impulses
683 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
684 ✗ model.createData(&pinocchio_data);
685
686 // create force diff
687 Eigen::MatrixXd df_dx =
688 ✗ Eigen::MatrixXd::Random(model.get_nc(), model.get_state()->get_nv());
689
690 // call update force diff
691 ✗ model.updateForceDiff(data, df_dx);
692
693 // Test
694 ✗ crocoddyl::ImpulseModelMultiple::ImpulseDataContainer::iterator it_d, end_d;
695 ✗ for (it_d = data->impulses.begin(), end_d = data->impulses.end();
696 ✗ it_d != end_d; ++it_d) {
697 ✗ BOOST_CHECK(!it_d->second->df_dx.isZero());
698 }
699
700 // Checking that casted computation is the same
701 #ifdef NDEBUG // Run only in release mode
702 crocoddyl::ImpulseModelMultipleTpl<float> casted_model = model.cast<float>();
703 pinocchio::DataTpl<float> casted_pinocchio_data(
704 *casted_model.get_state()->get_pinocchio().get());
705 std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<float>> casted_data =
706 casted_model.createData(&casted_pinocchio_data);
707 const Eigen::MatrixXf df_dx_f = df_dx.cast<float>();
708 casted_model.updateForceDiff(casted_data, df_dx_f);
709 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
710 crocoddyl::ImpulseModelMultipleTpl<float>::ImpulseDataContainer::iterator
711 it_d_f,
712 end_d_f;
713 for (it_d_f = casted_data->impulses.begin(),
714 end_d_f = casted_data->impulses.end(), it_d = data->impulses.begin(),
715 end_d = data->impulses.end();
716 it_d_f != end_d_f || it_d != end_d; ++it_d_f, ++it_d) {
717 BOOST_CHECK(!it_d_f->second->df_dx.isZero());
718 BOOST_CHECK((it_d->second->df_dx.cast<float>() - it_d_f->second->df_dx)
719 .isZero(tol_f));
720 }
721 #endif
722 }
723
724 ✗ void test_assert_updateForceDiff_assert_mismatch_model_data() {
725 // Setup the test
726 ✗ StateModelFactory state_factory;
727 crocoddyl::ImpulseModelMultiple model1(
728 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
729 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
730 crocoddyl::ImpulseModelMultiple model2(
731 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
732 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
733
734 // create the corresponding data object
735 ✗ pinocchio::Data pinocchio_data(*model1.get_state()->get_pinocchio().get());
736
737 // create and add some impulse objects
738 ✗ std::vector<std::shared_ptr<ImpulseModelFactory>> impulse_factories;
739 ✗ for (unsigned i = 0; i < 5; ++i) {
740 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse =
741 ✗ create_random_impulse();
742 {
743 ✗ std::ostringstream os;
744 ✗ os << "random_impulse1_" << i;
745 ✗ model1.addImpulse(os.str(), rand_impulse);
746 }
747 {
748 ✗ std::ostringstream os;
749 ✗ os << "random_impulse2_" << i;
750 ✗ model2.addImpulse(os.str(), rand_impulse);
751 }
752 }
753
754 // create the data of the multiple-impulses
755 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data1 =
756 ✗ model1.createData(&pinocchio_data);
757 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data2 =
758 ✗ model2.createData(&pinocchio_data);
759
760 // create force diff
761 Eigen::MatrixXd df_dx =
762 ✗ Eigen::MatrixXd::Random(model1.get_nc(), model1.get_state()->get_nv());
763
764 // call that trigger assert
765 std::string error_message =
766 ✗ GetErrorMessages(boost::bind(&updateForceDiff, model1, data2, df_dx));
767
768 // expected error message content
769 std::string function_name =
770 "void crocoddyl::ImpulseModelMultiple::updateForceDiff("
771 "const std::shared_ptr<crocoddyl::ImpulseDataMultiple>&,"
772 ✗ " const MatrixXd&) const";
773 std::string assert_argument =
774 "it_m->first == it_d->first && \"it doesn't match"
775 ✗ " the impulse name between data and model\"";
776
777 // Perform the checks
778 #ifndef __APPLE__
779 ✗ BOOST_CHECK(error_message.find(function_name) != std::string::npos);
780 #endif
781 ✗ BOOST_CHECK(error_message.find(assert_argument) != std::string::npos);
782 }
783
784 1 void test_get_impulses() {
785 // Setup the test
786
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
787 crocoddyl::ImpulseModelMultiple model(
788
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
789
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
790
791 // create the corresponding data object
792
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
793
794 // create and add some impulse objects
795
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
796
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
797
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
798
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 model.addImpulse(os.str(), create_random_impulse());
799 5 }
800
801 // get the impulses
802 const crocoddyl::ImpulseModelMultiple::ImpulseModelContainer& impulses =
803
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.get_impulses();
804
805 // test
806 1 crocoddyl::ImpulseModelMultiple::ImpulseModelContainer::const_iterator it_m,
807 1 end_m;
808 unsigned i;
809
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 1 times.
 6 for (i = 0, it_m = impulses.begin(), end_m = impulses.end(); it_m != end_m;
810 5 ++it_m, ++i) {
811
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
812
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
813
7/14
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 5 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 5 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 5 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 5 times.
✗ Branch 23 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 5 times.
 5 BOOST_CHECK(it_m->first == os.str());
814 5 }
815 1 }
816
817 1 void test_get_nc() {
818 // Setup the test
819
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
820 crocoddyl::ImpulseModelMultiple model(
821
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
822
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
823
824 // create the corresponding data object
825
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
826
827 // create and add some impulse objects
828
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
829
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
830
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_impulse_" << i;
831
3/6
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
 5 model.addImpulse(os.str(), create_random_impulse());
832 5 }
833
834 // compute ni
835 1 std::size_t ni = 0;
836 1 crocoddyl::ImpulseModelMultiple::ImpulseModelContainer::const_iterator it_m,
837 1 end_m;
838
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 for (it_m = model.get_impulses().begin(), end_m = model.get_impulses().end();
839
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 it_m != end_m; ++it_m) {
840
1/2
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 ni += it_m->second->impulse->get_nc();
841 }
842
843
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 1 BOOST_CHECK(ni == model.get_nc());
844 1 }
845
846 //----------------------------------------------------------------------------//
847
848 1 void register_unit_tests() {
849 1 framework::master_test_suite().add(
850
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_constructor)));
851 1 framework::master_test_suite().add(
852
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_addImpulse)));
853 1 framework::master_test_suite().add(
854
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_addImpulse_error_message)));
855 1 framework::master_test_suite().add(
856
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_removeImpulse)));
857 1 framework::master_test_suite().add(
858
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_removeImpulse_error_message)));
859
4/8
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 framework::master_test_suite().add(BOOST_TEST_CASE(boost::bind(&test_calc)));
860 1 framework::master_test_suite().add(
861
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_calc_diff)));
862 1 framework::master_test_suite().add(
863
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_calc_diff_no_recalc)));
864 1 framework::master_test_suite().add(
865
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_updateForce)));
866 1 framework::master_test_suite().add(
867
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_updateVelocityDiff)));
868 1 framework::master_test_suite().add(
869
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_get_impulses)));
870 1 framework::master_test_suite().add(
871
4/8
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 BOOST_TEST_CASE(boost::bind(&test_get_nc)));
872 1 }
873
874 1 bool init_function() {
875 1 register_unit_tests();
876 1 return true;
877 }
878
879 1 int main(int argc, char** argv) {
880 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
881 }
882