GCC Code Coverage Report

Directory: ./
File: unittest/test_multiple_impulses.cpp
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 313 368 85.1%
Branches: 545 1178 46.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, New York University, Max Planck
5 // Gesellschaft,
6 // INRIA
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #define BOOST_TEST_NO_MAIN
12 #define BOOST_TEST_ALTERNATIVE_INIT_API
13
14 #include <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 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
6/12
✓ 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 24 not taken.
✓ Branch 25 taken 1 times.
 1 BOOST_CHECK(model.get_impulses().size() == 0);
80 1 }
81
82 1 void test_addImpulse() {
83 // Setup the test
84
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
85 crocoddyl::ImpulseModelMultiple model(
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
87 1 StateModelTypes::StateMultibody_RandomHumanoid)));
88
89 // add an active impulse
90 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse_1 =
91
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
92
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);
93
6/12
✓ 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 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() == rand_impulse_1->get_nc());
94
6/12
✓ 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 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_total() == rand_impulse_1->get_nc());
95
96 // add an inactive impulse
97 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse_2 =
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_2", rand_impulse_2, false);
100
6/12
✓ 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 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() == rand_impulse_1->get_nc());
101
6/12
✓ 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 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(model.get_nc_total() ==
102 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
103
104 // change the random impulse 2 status
105
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);
106
6/12
✓ 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 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(model.get_nc() ==
107 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
108
6/12
✓ 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 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(model.get_nc_total() ==
109 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
110
111 // change the random impulse 1 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_1", false);
113
6/12
✓ 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 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() == rand_impulse_2->get_nc());
114
6/12
✓ 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 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(model.get_nc_total() ==
115 rand_impulse_1->get_nc() + rand_impulse_2->get_nc());
116 1 }
117
118 1 void test_addImpulse_error_message() {
119 // Setup the test
120
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
121 crocoddyl::ImpulseModelMultiple model(
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
123 1 StateModelTypes::StateMultibody_RandomHumanoid)));
124
125 // create an impulse object
126 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse =
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
128
129 // add twice the same impulse object to the container
130
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);
131
132 // test error message when we add a duplicate impulse
133
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CaptureIOStream capture_ios;
134
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
135
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);
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
137
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream expected_buffer;
138 expected_buffer << "Warning: we couldn't add the random_impulse impulse "
139
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "item, it already existed."
140
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
141
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());
142
143 // test error message when we change the impulse status of an inexistent
144 // impulse
145
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
146
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);
147
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
148
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 expected_buffer.clear();
149 expected_buffer << "Warning: we couldn't change the status of the "
150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "no_exist_impulse impulse item, it doesn't exist."
151
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
152
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());
153 1 }
154
155 1 void test_removeImpulse() {
156 // Setup the test
157
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
158 crocoddyl::ImpulseModelMultiple model(
159
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
160 1 StateModelTypes::StateMultibody_RandomHumanoid)));
161
162 // add an active impulse
163 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse =
164
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_impulse();
165
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);
166
6/12
✓ 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 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() == rand_impulse->get_nc());
167
168 // remove the impulse
169
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");
170
6/12
✓ 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 23 not taken.
✓ Branch 24 taken 1 times.
 1 BOOST_CHECK(model.get_nc() == 0);
171 1 }
172
173 1 void test_removeImpulse_error_message() {
174 // Setup the test
175
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
176 crocoddyl::ImpulseModelMultiple model(
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
178 1 StateModelTypes::StateMultibody_RandomHumanoid)));
179
180 // remove a none existing impulse form the container, we expect a cout message
181 // here
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CaptureIOStream capture_ios;
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
184
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");
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
186
187 // Test that the error message is sent.
188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream expected_buffer;
189 expected_buffer << "Warning: we couldn't remove the random_impulse impulse "
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "item, it doesn't exist."
191
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
192
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());
193 1 }
194
195 1 void test_calc() {
196 // Setup the test
197
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
198 crocoddyl::ImpulseModelMultiple model(
199
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
200 1 StateModelTypes::StateMultibody_RandomHumanoid)));
201 // create the corresponding data object
202 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
203
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
204
205 // create and add some impulse objects
206 1 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstract> > models;
207 1 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstract> > datas;
208
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
209
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
210
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;
211 const std::shared_ptr<crocoddyl::ImpulseModelAbstract>& m =
212
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_impulse();
213
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);
214
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
215
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));
216 5 }
217
218 // create the data of the multiple-impulses
219 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
220
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
221
222 // compute the multiple contact data for the case when all impulses are
223 // defined as active
224
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
225
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,
226 x1);
227
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);
228
229 // check that only the Jacobian has been filled
230
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());
231
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());
232
233 // check Jc against single impulse computations
234 1 std::size_t ni = 0;
235 1 const std::size_t nv = model.get_state()->get_nv();
236
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
237 5 const std::size_t ni_i = models[i]->get_nc();
238
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);
239
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);
240 5 ni += ni_i;
241 }
242 1 ni = 0;
243
244 // compute the multiple impulse data for the case when the first three
245 // impulses are defined as active
246
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);
247
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);
248
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
249
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,
250 x2);
251
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);
252
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
253 5 const std::size_t ni_i = models[i]->get_nc();
254
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
255
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);
256 }
257
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);
258 5 ni += ni_i;
259 }
260 1 }
261
262 1 void test_calc_diff() {
263 // Setup the test
264
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
265 crocoddyl::ImpulseModelMultiple model(
266
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
267 1 StateModelTypes::StateMultibody_RandomHumanoid)));
268
269 // create the corresponding data object
270 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
271
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
272
273 // create and add some impulse objects
274 1 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstract> > models;
275 1 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstract> > datas;
276
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
277
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
278
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;
279 const std::shared_ptr<crocoddyl::ImpulseModelAbstract>& m =
280
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_impulse();
281
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);
282
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
283
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));
284 5 }
285
286 // create the data of the multiple-impulses
287 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
288
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
289
290 // compute the multiple contact data for the case when all impulses are
291 // defined as active
292
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
293
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,
294 x1);
295
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);
296
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);
297
298 // Check that nothing has been computed and that all value are initialized to
299 // 0
300
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());
301
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());
302
303 // check Jc against single impulse computations
304 1 std::size_t ni = 0;
305 1 const std::size_t nv = model.get_state()->get_nv();
306
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
307 5 const std::size_t ni_i = models[i]->get_nc();
308
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);
309
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);
310
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);
311
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);
312 5 ni += ni_i;
313 }
314 1 ni = 0;
315
316 // compute the multiple impulse data for the case when the first three
317 // impulses are defined as active
318
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);
319
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);
320
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
321
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,
322 x2);
323
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);
324
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);
325
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
326 5 const std::size_t ni_i = models[i]->get_nc();
327
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
328
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);
329
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);
330 }
331
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);
332
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);
333 5 ni += ni_i;
334 }
335 1 }
336
337 1 void test_calc_diff_no_recalc() {
338 // Setup the test
339
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
340 crocoddyl::ImpulseModelMultiple model(
341
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
342 1 StateModelTypes::StateMultibody_RandomHumanoid)));
343
344 // create the corresponding data object
345 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
346
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
347
348 // create and add some impulse objects
349 1 std::vector<std::shared_ptr<crocoddyl::ImpulseModelAbstract> > models;
350 1 std::vector<std::shared_ptr<crocoddyl::ImpulseDataAbstract> > datas;
351
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
352
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
353
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;
354 const std::shared_ptr<crocoddyl::ImpulseModelAbstract>& m =
355
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_impulse();
356
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);
357
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
358
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));
359 5 }
360
361 // create the data of the multiple-impulses
362 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
363
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
364
365 // compute the multiple contact data for the case when all impulses are
366 // defined as active
367
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
368
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,
369 x1);
370
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);
371
372 // Check that nothing has been computed and that all value are initialized to
373 // 0
374
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());
375
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());
376
377 // check Jc against single impulse computations
378 1 std::size_t ni = 0;
379 1 const std::size_t nv = model.get_state()->get_nv();
380
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
381 5 const std::size_t ni_i = models[i]->get_nc();
382
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);
383
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());
384
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);
385 5 ni += ni_i;
386 }
387 1 ni = 0;
388
389 // compute the multiple impulse data for the case when the first three
390 // impulses are defined as active
391
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);
392
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);
393
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
394
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,
395 x2);
396
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);
397
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
398 5 const std::size_t ni_i = models[i]->get_nc();
399
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
400
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);
401 }
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 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());
403
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);
404 5 ni += ni_i;
405 }
406 1 }
407
408 1 void test_updateForce() {
409 // Setup the test
410
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
411 crocoddyl::ImpulseModelMultiple model(
412
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
413 1 StateModelTypes::StateMultibody_RandomHumanoid)));
414
415 // create the corresponding data object
416 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
417
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
418
419 // create and add some impulse objects
420
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
421
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
422
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;
423
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());
424 5 }
425
426 // create the data of the multiple-impulses
427 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
428
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
429
430 // Compute the jacobian and check that the impulse model fetch it.
431
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x = model.get_state()->rand();
432
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);
433
434 // create random forces
435
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Eigen::VectorXd forces = Eigen::VectorXd::Random(model.get_nc());
436
437 // update forces
438
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.updateForce(data, forces);
439
440 // Check that nothing has been computed and that all value are initialized to
441 // 0
442
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());
443
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());
444 1 crocoddyl::ImpulseModelMultiple::ImpulseDataContainer::iterator it_d, end_d;
445 1 for (it_d = data->impulses.begin(), end_d = data->impulses.end();
446
2/2
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 1 times.
 6 it_d != end_d; ++it_d) {
447
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());
448 }
449 1 }
450
451 1 void test_updateVelocityDiff() {
452 // Setup the test
453
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
454 crocoddyl::ImpulseModelMultiple model(
455
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
456 1 StateModelTypes::StateMultibody_RandomHumanoid)));
457
458 // create the corresponding data object
459
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
460
461 // create and add some impulse objects
462
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
463
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
464
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;
465
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());
466 5 }
467
468 // create the data of the multiple-impulses
469 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
470
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
471
472 // create the velocity diff
473
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::MatrixXd dvnext_dx = Eigen::MatrixXd::Random(
474
1/2
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 model.get_state()->get_nv(), model.get_state()->get_ndx());
475
476 // call the update
477
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.updateVelocityDiff(data, dvnext_dx);
478
479 // Test
480
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));
481 1 }
482
483 ✗ void test_updateForceDiff() {
484 // Setup the test
485 ✗ StateModelFactory state_factory;
486 crocoddyl::ImpulseModelMultiple model(
487 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
488 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
489
490 // create the corresponding data object
491 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
492
493 // create and add some impulse objects
494 ✗ for (unsigned i = 0; i < 5; ++i) {
495 ✗ std::ostringstream os;
496 ✗ os << "random_impulse_" << i;
497 ✗ model.addImpulse(os.str(), create_random_impulse());
498 }
499
500 // create the data of the multiple-impulses
501 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data =
502 ✗ model.createData(&pinocchio_data);
503
504 // create force diff
505 Eigen::MatrixXd df_dx =
506 ✗ Eigen::MatrixXd::Random(model.get_nc(), model.get_state()->get_nv());
507
508 // call update force diff
509 ✗ model.updateForceDiff(data, df_dx);
510
511 // Test
512 ✗ crocoddyl::ImpulseModelMultiple::ImpulseDataContainer::iterator it_d, end_d;
513 ✗ for (it_d = data->impulses.begin(), end_d = data->impulses.end();
514 ✗ it_d != end_d; ++it_d) {
515 ✗ BOOST_CHECK(!it_d->second->df_dx.isZero());
516 }
517 }
518
519 ✗ void test_assert_updateForceDiff_assert_mismatch_model_data() {
520 // Setup the test
521 ✗ StateModelFactory state_factory;
522 crocoddyl::ImpulseModelMultiple model1(
523 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
524 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
525 crocoddyl::ImpulseModelMultiple model2(
526 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
527 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
528
529 // create the corresponding data object
530 ✗ pinocchio::Data pinocchio_data(*model1.get_state()->get_pinocchio().get());
531
532 // create and add some impulse objects
533 ✗ std::vector<std::shared_ptr<ImpulseModelFactory> > impulse_factories;
534 ✗ for (unsigned i = 0; i < 5; ++i) {
535 std::shared_ptr<crocoddyl::ImpulseModelAbstract> rand_impulse =
536 ✗ create_random_impulse();
537 {
538 ✗ std::ostringstream os;
539 ✗ os << "random_impulse1_" << i;
540 ✗ model1.addImpulse(os.str(), rand_impulse);
541 }
542 {
543 ✗ std::ostringstream os;
544 ✗ os << "random_impulse2_" << i;
545 ✗ model2.addImpulse(os.str(), rand_impulse);
546 }
547 }
548
549 // create the data of the multiple-impulses
550 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data1 =
551 ✗ model1.createData(&pinocchio_data);
552 std::shared_ptr<crocoddyl::ImpulseDataMultiple> data2 =
553 ✗ model2.createData(&pinocchio_data);
554
555 // create force diff
556 Eigen::MatrixXd df_dx =
557 ✗ Eigen::MatrixXd::Random(model1.get_nc(), model1.get_state()->get_nv());
558
559 // call that trigger assert
560 std::string error_message =
561 ✗ GetErrorMessages(boost::bind(&updateForceDiff, model1, data2, df_dx));
562
563 // expected error message content
564 std::string function_name =
565 "void crocoddyl::ImpulseModelMultiple::updateForceDiff("
566 "const std::shared_ptr<crocoddyl::ImpulseDataMultiple>&,"
567 ✗ " const MatrixXd&) const";
568 std::string assert_argument =
569 "it_m->first == it_d->first && \"it doesn't match"
570 ✗ " the impulse name between data and model\"";
571
572 // Perform the checks
573 #ifndef __APPLE__
574 ✗ BOOST_CHECK(error_message.find(function_name) != std::string::npos);
575 #endif
576 ✗ BOOST_CHECK(error_message.find(assert_argument) != std::string::npos);
577 }
578
579 1 void test_get_impulses() {
580 // Setup the test
581
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
582 crocoddyl::ImpulseModelMultiple model(
583
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
584 1 StateModelTypes::StateMultibody_RandomHumanoid)));
585
586 // create the corresponding data object
587
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
588
589 // create and add some impulse objects
590
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
591
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
592
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;
593
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());
594 5 }
595
596 // get the impulses
597 const crocoddyl::ImpulseModelMultiple::ImpulseModelContainer& impulses =
598 1 model.get_impulses();
599
600 // test
601 1 crocoddyl::ImpulseModelMultiple::ImpulseModelContainer::const_iterator it_m,
602 1 end_m;
603 unsigned i;
604
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;
605 5 ++it_m, ++i) {
606
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
607
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;
608
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());
609 5 }
610 1 }
611
612 1 void test_get_nc() {
613 // Setup the test
614
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
615 crocoddyl::ImpulseModelMultiple model(
616
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 std::static_pointer_cast<crocoddyl::StateMultibody>(state_factory.create(
617 1 StateModelTypes::StateMultibody_RandomHumanoid)));
618
619 // create the corresponding data object
620
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
621
622 // create and add some impulse objects
623
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
624
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
625
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;
626
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());
627 5 }
628
629 // compute ni
630 1 std::size_t ni = 0;
631 1 crocoddyl::ImpulseModelMultiple::ImpulseModelContainer::const_iterator it_m,
632 1 end_m;
633 1 for (it_m = model.get_impulses().begin(), end_m = model.get_impulses().end();
634
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 it_m != end_m; ++it_m) {
635 5 ni += it_m->second->impulse->get_nc();
636 }
637
638
6/12
✓ 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 23 not taken.
✓ Branch 24 taken 1 times.
 1 BOOST_CHECK(ni == model.get_nc());
639 1 }
640
641 //----------------------------------------------------------------------------//
642
643 1 void register_unit_tests() {
644 1 framework::master_test_suite().add(
645
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)));
646 1 framework::master_test_suite().add(
647
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)));
648 1 framework::master_test_suite().add(
649
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)));
650 1 framework::master_test_suite().add(
651
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)));
652 1 framework::master_test_suite().add(
653
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)));
654
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)));
655 1 framework::master_test_suite().add(
656
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)));
657 1 framework::master_test_suite().add(
658
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)));
659 1 framework::master_test_suite().add(
660
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)));
661 1 framework::master_test_suite().add(
662
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)));
663 1 framework::master_test_suite().add(
664
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)));
665 1 framework::master_test_suite().add(
666
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)));
667 1 }
668
669 1 bool init_function() {
670 1 register_unit_tests();
671 1 return true;
672 }
673
674 1 int main(int argc, char** argv) {
675 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
676 }
677