GCC Code Coverage Report

Directory: ./
File: unittest/test_multiple_contacts.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 343 402 85.3%
Branches: 652 1408 46.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include <pinocchio/algorithm/frames.hpp>
13 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14
15 #include "factory/contact.hpp"
16 #include "unittest_common.hpp"
17
18 using namespace boost::unit_test;
19 using namespace crocoddyl::unittest;
20
21 //----------------------------------------------------------------------------//
22
23 /**
24 * These methods modify the return type of the model function in
25 * order to use the boost::execution_monitor::execute method which catch the
26 * assert signal
27 */
28 ✗ int calc(crocoddyl::ContactModelMultiple& model,
29 boost::shared_ptr<crocoddyl::ContactDataMultiple> data,
30 Eigen::VectorXd& dx) {
31 ✗ model.calc(data, dx);
32 ✗ return 0;
33 }
34
35 ✗ int calcDiff(crocoddyl::ContactModelMultiple& model,
36 boost::shared_ptr<crocoddyl::ContactDataMultiple> data,
37 Eigen::VectorXd& dx) {
38 ✗ model.calcDiff(data, dx);
39 ✗ return 0;
40 }
41
42 ✗ int updateForce(crocoddyl::ContactModelMultiple& model,
43 boost::shared_ptr<crocoddyl::ContactDataMultiple> data,
44 Eigen::VectorXd& dx) {
45 ✗ model.updateForce(data, dx);
46 ✗ return 0;
47 }
48
49 ✗ int updateAccelerationDiff(
50 crocoddyl::ContactModelMultiple& model,
51 boost::shared_ptr<crocoddyl::ContactDataMultiple> data,
52 const Eigen::MatrixXd& ddv_dx) {
53 ✗ model.updateAccelerationDiff(data, ddv_dx);
54 ✗ return 0;
55 }
56
57 ✗ int updateForceDiff(crocoddyl::ContactModelMultiple& model,
58 boost::shared_ptr<crocoddyl::ContactDataMultiple> data,
59 const Eigen::MatrixXd& df_dx,
60 const Eigen::MatrixXd& df_du) {
61 ✗ model.updateForceDiff(data, df_dx, df_du);
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::ContactModelMultiple model(
71 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
73
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
74
75 // Run the print function
76
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::ostringstream tmp;
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 tmp << model;
78
79 // Test the initial size of the map
80
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_contacts().size() == 0);
81 1 }
82
83 1 void test_addContact() {
84 // Setup the test
85
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
86 crocoddyl::ContactModelMultiple model(
87 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
89
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
90
91 // add an active contact
92 boost::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact_1 =
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_contact();
94
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addContact("random_contact_1", rand_contact_1);
95
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_contact_1->get_nc());
96
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_contact_1->get_nc());
97
98 // add an inactive contact
99 boost::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact_2 =
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_contact();
101
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addContact("random_contact_2", rand_contact_2, false);
102
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_contact_1->get_nc());
103
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() ==
104 rand_contact_1->get_nc() + rand_contact_2->get_nc());
105
106 // change the random contact 2 status
107
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("random_contact_2", true);
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() ==
109 rand_contact_1->get_nc() + rand_contact_2->get_nc());
110
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() ==
111 rand_contact_1->get_nc() + rand_contact_2->get_nc());
112
113 // change the random contact 1 status
114
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("random_contact_1", false);
115
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_contact_2->get_nc());
116
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() ==
117 rand_contact_1->get_nc() + rand_contact_2->get_nc());
118 1 }
119
120 1 void test_addContact_error_message() {
121 // Setup the test
122
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
123 crocoddyl::ContactModelMultiple model(
124 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
127
128 // create an contact object
129 boost::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact =
130
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_contact();
131
132 // add twice the same contact object to the container
133
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addContact("random_contact", rand_contact);
134
135 // test error message when we add a duplicate contact
136
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CaptureIOStream capture_ios;
137
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
138
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addContact("random_contact", rand_contact);
139
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
140
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream expected_buffer;
141 expected_buffer << "Warning: we couldn't add the random_contact contact "
142
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "item, it already existed."
143
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
144
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());
145
146 // test error message when we change the contact status of an inexistent
147 // contact
148
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
149
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("no_exist_contact", true);
150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
151
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 expected_buffer.clear();
152 expected_buffer << "Warning: we couldn't change the status of the "
153
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "no_exist_contact contact item, it doesn't exist."
154
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
155
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());
156 1 }
157
158 1 void test_removeContact() {
159 // Setup the test
160
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
161 crocoddyl::ContactModelMultiple model(
162 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
163
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
164
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
165
166 // add an active contact
167 boost::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact =
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 create_random_contact();
169
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 model.addContact("random_contact", rand_contact);
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 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_contact->get_nc());
171
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_contact->get_nc());
172
173 // remove the contact
174
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.removeContact("random_contact");
175
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);
176
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_total() == 0);
177 1 }
178
179 1 void test_removeContact_error_message() {
180 // Setup the test
181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
182 crocoddyl::ContactModelMultiple model(
183 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
186
187 // remove a none existing contact form the container, we expect a cout message
188 // here
189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CaptureIOStream capture_ios;
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.beginCapture();
191
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.removeContact("random_contact");
192
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 capture_ios.endCapture();
193
194 // Test that the error message is sent.
195
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::stringstream expected_buffer;
196 expected_buffer << "Warning: we couldn't remove the random_contact contact "
197
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 "item, it doesn't exist."
198
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 << std::endl;
199
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());
200 1 }
201
202 1 void test_calc() {
203 // Setup the test
204
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
205 crocoddyl::ContactModelMultiple model(
206 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
207
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
208
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
209 // create the corresponding data object
210 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
211
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
212
213 // create and add some contact objects
214 1 std::vector<boost::shared_ptr<crocoddyl::ContactModelAbstract> > models;
215 1 std::vector<boost::shared_ptr<crocoddyl::ContactDataAbstract> > datas;
216
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
217
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
218
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
219 const boost::shared_ptr<crocoddyl::ContactModelAbstract>& m =
220
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_contact();
221
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 model.addContact(os.str(), m);
222
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
223
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));
224 5 }
225
226 // create the data of the multiple-contacts
227 boost::shared_ptr<crocoddyl::ContactDataMultiple> data =
228
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
229
230 // compute the multiple contact data for the case when all contacts are
231 // defined as active
232
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
233
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,
234 x1);
235
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);
236
237 // check that only the Jacobian has been filled
238
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());
239
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->a0.isZero());
240
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->da0_dx.isZero());
241
242 // check Jc and a0 against single contact computations
243 1 std::size_t nc = 0;
244 1 const std::size_t nv = model.get_state()->get_nv();
245
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
246 5 const std::size_t nc_i = models[i]->get_nc();
247
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);
248
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(nc, 0, nc_i, nv) == datas[i]->Jc);
249
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->a0.segment(nc, nc_i) == datas[i]->a0);
250 5 nc += nc_i;
251 }
252 1 nc = 0;
253
254 // compute the multiple contact data for the case when the first three
255 // contacts are defined as active
256
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("random_contact_3", false);
257
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("random_contact_4", false);
258
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
259
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,
260 x2);
261
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);
262
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
263 5 const std::size_t nc_i = models[i]->get_nc();
264
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (i < 3) { // we need to update data because this contacts are active
265
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);
266 }
267
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(nc, 0, nc_i, nv) == datas[i]->Jc);
268
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->a0.segment(nc, nc_i) == datas[i]->a0);
269 5 nc += nc_i;
270 }
271 1 }
272
273 1 void test_calc_diff() {
274 // Setup the test
275
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
276 crocoddyl::ContactModelMultiple model(
277 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
278
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
280 // create the corresponding data object
281 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
282
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
283
284 // create and add some contact objects
285 1 std::vector<boost::shared_ptr<crocoddyl::ContactModelAbstract> > models;
286 1 std::vector<boost::shared_ptr<crocoddyl::ContactDataAbstract> > datas;
287
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
288
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
289
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
290 const boost::shared_ptr<crocoddyl::ContactModelAbstract>& m =
291
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_contact();
292
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 model.addContact(os.str(), m);
293
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
294
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));
295 5 }
296
297 // create the data of the multiple-contacts
298 boost::shared_ptr<crocoddyl::ContactDataMultiple> data =
299
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
300
301 // compute the multiple contact data for the case when all contacts are
302 // defined as active
303
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
304
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,
305 x1);
306
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);
307
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);
308
309 // check that nothing has been computed and that all value are initialized to
310 // 0
311
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());
312
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->a0.isZero());
313
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->da0_dx.isZero());
314
315 // check Jc and a0 against single contact computations
316 1 std::size_t nc = 0;
317 1 const std::size_t nv = model.get_state()->get_nv();
318 1 const std::size_t ndx = model.get_state()->get_ndx();
319
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
320 5 const std::size_t nc_i = models[i]->get_nc();
321
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);
322
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);
323
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(nc, 0, nc_i, nv) == datas[i]->Jc);
324
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->a0.segment(nc, nc_i) == datas[i]->a0);
325
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->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
326 5 nc += nc_i;
327 }
328 1 nc = 0;
329
330 // compute the multiple contact data for the case when the first three
331 // contacts are defined as active
332
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("random_contact_3", false);
333
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 model.changeContactStatus("random_contact_4", false);
334
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x2 = model.get_state()->rand();
335
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,
336 x2);
337
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);
338
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);
339
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
340 5 const std::size_t nc_i = models[i]->get_nc();
341
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (i < 3) { // we need to update data because this contacts are active
342
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);
343
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);
344 }
345
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(nc, 0, nc_i, nv) == datas[i]->Jc);
346
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->a0.segment(nc, nc_i) == datas[i]->a0);
347
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->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
348 5 nc += nc_i;
349 }
350 1 }
351
352 1 void test_calc_diff_no_recalc() {
353 // Setup the test
354
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
355 crocoddyl::ContactModelMultiple model(
356 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
357
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
358
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
359 // create the corresponding data object
360 1 pinocchio::Model& pinocchio_model = *model.get_state()->get_pinocchio().get();
361
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
362
363 // create and add some contact objects
364 1 std::vector<boost::shared_ptr<crocoddyl::ContactModelAbstract> > models;
365 1 std::vector<boost::shared_ptr<crocoddyl::ContactDataAbstract> > datas;
366
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
367
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
368
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
369 const boost::shared_ptr<crocoddyl::ContactModelAbstract>& m =
370
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 create_random_contact();
371
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 model.addContact(os.str(), m);
372
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 models.push_back(m);
373
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));
374 5 }
375
376 // create the data of the multiple-contacts
377 boost::shared_ptr<crocoddyl::ContactDataMultiple> data =
378
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
379
380 // compute the multiple contact data for the case when all contacts are
381 // defined as active
382
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x1 = model.get_state()->rand();
383
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,
384 x1);
385
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);
386
387 // check that nothing has been computed and that all value are initialized to
388 // 0
389
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());
390
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->a0.isZero());
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->da0_dx.isZero());
392
393 // check Jc and a0 against single contact computations
394 1 std::size_t nc = 0;
395 1 const std::size_t nv = model.get_state()->get_nv();
396 1 const std::size_t ndx = model.get_state()->get_ndx();
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 nc_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]->calcDiff(datas[i], x1);
400
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(nc, 0, nc_i, nv).isZero());
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 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->a0.segment(nc, nc_i).isZero());
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->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
403 5 nc += nc_i;
404 }
405 1 nc = 0;
406
407 // compute the multiple contact data for the case when the first three
408 // contacts 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.changeContactStatus("random_contact_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.changeContactStatus("random_contact_4", false);
411
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 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.calcDiff(data, x2);
415
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (std::size_t i = 0; i < 5; ++i) {
416 5 const std::size_t nc_i = models[i]->get_nc();
417
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 2 times.
 5 if (i < 3) { // we need to update data because this contacts are active
418
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);
419 }
420
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(nc, 0, nc_i, nv).isZero());
421
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->a0.segment(nc, nc_i).isZero());
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->da0_dx.block(nc, 0, nc_i, ndx) == datas[i]->da0_dx);
423 5 nc += nc_i;
424 }
425 1 }
426
427 1 void test_updateForce() {
428 // Setup the test
429
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
430 crocoddyl::ContactModelMultiple model(
431 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
432
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
433
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
434
435 // create the corresponding data object
436 const pinocchio::Model& pinocchio_model =
437 1 *model.get_state()->get_pinocchio().get();
438
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
439
440 // create and add some contact objects
441
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
442
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
443
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
444
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.addContact(os.str(), create_random_contact());
445 5 }
446
447 // create the data of the multiple-contacts
448 boost::shared_ptr<crocoddyl::ContactDataMultiple> data =
449
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
450
451 // Compute the jacobian and check that the contact model fetch it.
452 Eigen::VectorXd q =
453
3/6
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
 1 model.get_state()->rand().segment(0, model.get_state()->get_nq());
454
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Eigen::VectorXd v = Eigen::VectorXd::Random(model.get_state()->get_nv());
455
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 Eigen::VectorXd a = Eigen::VectorXd::Random(model.get_state()->get_nv());
456
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pinocchio::computeJointJacobians(pinocchio_model, pinocchio_data, q);
457
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pinocchio::updateFramePlacements(pinocchio_model, pinocchio_data);
458
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 pinocchio::computeForwardKinematicsDerivatives(pinocchio_model,
459 pinocchio_data, q, v, a);
460
461 // create random forces
462
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());
463
464 // update forces
465
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.updateForce(data, forces);
466
467 // Check that nothing has been computed and that all value are initialized to
468 // 0
469
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());
470
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->a0.isZero());
471
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->da0_dx.isZero());
472 1 crocoddyl::ContactModelMultiple::ContactDataContainer::iterator it_d, end_d;
473 1 for (it_d = data->contacts.begin(), end_d = data->contacts.end();
474
2/2
✓ Branch 2 taken 5 times.
✓ Branch 3 taken 1 times.
 6 it_d != end_d; ++it_d) {
475
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());
476 }
477 1 }
478
479 1 void test_updateAccelerationDiff() {
480 // Setup the test
481
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
482 crocoddyl::ContactModelMultiple model(
483 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
484
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
485
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
486 // create the corresponding data object
487
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
488
489 // create and add some contact objects
490
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned 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_contact_" << i;
493
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.addContact(os.str(), create_random_contact());
494 5 }
495
496 // create the data of the multiple-contacts
497 boost::shared_ptr<crocoddyl::ContactDataMultiple> data =
498
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.createData(&pinocchio_data);
499
500 // create the velocity diff
501
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::MatrixXd ddv_dx = Eigen::MatrixXd::Random(
502
1/2
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 model.get_state()->get_nv(), model.get_state()->get_ndx());
503
504 // call the update
505
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 model.updateAccelerationDiff(data, ddv_dx);
506
507 // Test
508
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->ddv_dx - ddv_dx).isZero(1e-9));
509 1 }
510
511 ✗ void test_updateForceDiff() {
512 // Setup the test
513 ✗ StateModelFactory state_factory;
514 crocoddyl::ContactModelMultiple model(
515 ✗ boost::static_pointer_cast<crocoddyl::StateMultibody>(
516 ✗ state_factory.create(
517 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
518 // create the corresponding data object
519 ✗ pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
520
521 // create and add some contact objects
522 ✗ for (unsigned i = 0; i < 5; ++i) {
523 ✗ std::ostringstream os;
524 ✗ os << "random_contact_" << i;
525 ✗ model.addContact(os.str(), create_random_contact());
526 }
527
528 // create the data of the multiple-contacts
529 boost::shared_ptr<crocoddyl::ContactDataMultiple> data =
530 ✗ model.createData(&pinocchio_data);
531
532 // create force diff
533 Eigen::MatrixXd df_dx =
534 ✗ Eigen::MatrixXd::Random(model.get_nc(), model.get_state()->get_nv());
535 Eigen::MatrixXd df_du =
536 ✗ Eigen::MatrixXd::Random(model.get_nc(), model.get_state()->get_nv());
537
538 // call update force diff
539 ✗ model.updateForceDiff(data, df_dx, df_du);
540
541 // Test
542 ✗ crocoddyl::ContactModelMultiple::ContactDataContainer::iterator it_d, end_d;
543 ✗ for (it_d = data->contacts.begin(), end_d = data->contacts.end();
544 ✗ it_d != end_d; ++it_d) {
545 ✗ BOOST_CHECK(!it_d->second->df_dx.isZero());
546 }
547 }
548
549 ✗ void test_assert_updateForceDiff_assert_mismatch_model_data() {
550 // Setup the test
551 ✗ StateModelFactory state_factory;
552 crocoddyl::ContactModelMultiple model1(
553 ✗ boost::static_pointer_cast<crocoddyl::StateMultibody>(
554 ✗ state_factory.create(
555 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
556 crocoddyl::ContactModelMultiple model2(
557 ✗ boost::static_pointer_cast<crocoddyl::StateMultibody>(
558 ✗ state_factory.create(
559 ✗ StateModelTypes::StateMultibody_RandomHumanoid)));
560 // create the corresponding data object
561 ✗ pinocchio::Data pinocchio_data(*model1.get_state()->get_pinocchio().get());
562
563 // create and add some contact objects
564 ✗ for (unsigned i = 0; i < 5; ++i) {
565 boost::shared_ptr<crocoddyl::ContactModelAbstract> rand_contact =
566 ✗ create_random_contact();
567 {
568 ✗ std::ostringstream os;
569 ✗ os << "random_contact1_" << i;
570 ✗ model1.addContact(os.str(), rand_contact);
571 }
572 {
573 ✗ std::ostringstream os;
574 ✗ os << "random_contact2_" << i;
575 ✗ model2.addContact(os.str(), rand_contact);
576 }
577 }
578
579 // create the data of the multiple-contacts
580 boost::shared_ptr<crocoddyl::ContactDataMultiple> data1 =
581 ✗ model1.createData(&pinocchio_data);
582 boost::shared_ptr<crocoddyl::ContactDataMultiple> data2 =
583 ✗ model2.createData(&pinocchio_data);
584
585 // create force diff
586 Eigen::MatrixXd df_dx =
587 ✗ Eigen::MatrixXd::Random(model1.get_nc(), model1.get_state()->get_nv());
588 Eigen::MatrixXd df_du =
589 ✗ Eigen::MatrixXd::Random(model1.get_nc(), model1.get_state()->get_nv());
590
591 // call that trigger assert
592 std::string error_message = GetErrorMessages(
593 ✗ boost::bind(&updateForceDiff, model1, data2, df_dx, df_du));
594
595 // expected error message content
596 std::string function_name =
597 "void crocoddyl::ContactModelMultiple::updateForceDiff("
598 "const boost::shared_ptr<crocoddyl::ContactDataMultiple>&,"
599 ✗ " const MatrixXd&) const";
600 std::string assert_argument =
601 "it_m->first == it_d->first && \"it doesn't match"
602 ✗ " the contact name between data and model\"";
603
604 // Perform the checks
605 #ifndef __APPLE__
606 ✗ BOOST_CHECK(error_message.find(function_name) != std::string::npos);
607 #endif
608 ✗ BOOST_CHECK(error_message.find(assert_argument) != std::string::npos);
609 }
610
611 1 void test_get_contacts() {
612 // Setup the test
613
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
614 crocoddyl::ContactModelMultiple model(
615 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
616
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
617
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
618 // create the corresponding data object
619
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
620
621 // create and add some contact objects
622
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
623
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
624
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
625
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.addContact(os.str(), create_random_contact());
626 5 }
627
628 // get the contacts
629 const crocoddyl::ContactModelMultiple::ContactModelContainer& contacts =
630 1 model.get_contacts();
631
632 // test
633 1 crocoddyl::ContactModelMultiple::ContactModelContainer::const_iterator it_m,
634 1 end_m;
635 unsigned i;
636
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 1 times.
 6 for (i = 0, it_m = contacts.begin(), end_m = contacts.end(); it_m != end_m;
637 5 ++it_m, ++i) {
638
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
639
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
640
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());
641 5 }
642 1 }
643
644 1 void test_get_nc() {
645 // Setup the test
646
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
647 crocoddyl::ContactModelMultiple model(
648 2 boost::static_pointer_cast<crocoddyl::StateMultibody>(
649
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(
650
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelTypes::StateMultibody_RandomHumanoid)));
651
652 // create the corresponding data object
653
1/2
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 pinocchio::Data pinocchio_data(*model.get_state()->get_pinocchio().get());
654
655 // create and add some contact objects
656
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 1 times.
 6 for (unsigned i = 0; i < 5; ++i) {
657
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 std::ostringstream os;
658
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 os << "random_contact_" << i;
659
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.addContact(os.str(), create_random_contact());
660 5 }
661
662 // compute ni
663 1 std::size_t ni = 0;
664 1 crocoddyl::ContactModelMultiple::ContactModelContainer::const_iterator it_m,
665 1 end_m;
666 1 for (it_m = model.get_contacts().begin(), end_m = model.get_contacts().end();
667
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 it_m != end_m; ++it_m) {
668 5 ni += it_m->second->contact->get_nc();
669 }
670
671
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());
672 1 }
673
674 //----------------------------------------------------------------------------//
675
676 1 void register_unit_tests() {
677 1 framework::master_test_suite().add(
678
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)));
679 1 framework::master_test_suite().add(
680
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_addContact)));
681 1 framework::master_test_suite().add(
682
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_addContact_error_message)));
683 1 framework::master_test_suite().add(
684
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_removeContact)));
685 1 framework::master_test_suite().add(
686
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_removeContact_error_message)));
687
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)));
688 1 framework::master_test_suite().add(
689
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)));
690 1 framework::master_test_suite().add(
691
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)));
692 1 framework::master_test_suite().add(
693
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)));
694 1 framework::master_test_suite().add(
695
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_updateAccelerationDiff)));
696 1 framework::master_test_suite().add(
697
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_contacts)));
698 1 framework::master_test_suite().add(
699
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)));
700 1 }
701
702 1 bool init_function() {
703 1 register_unit_tests();
704 1 return true;
705 }
706
707 1 int main(int argc, char** argv) {
708 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
709 }
710