GCC Code Coverage Report

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