GCC Code Coverage Report

Directory: ./
File: unittest/test_constraint_manager.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 306 0.0%
Functions: 0 12 0.0%
Branches: 0 1438 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
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 "crocoddyl/core/actions/lqr.hpp"
13 #include "crocoddyl/multibody/data/multibody.hpp"
14 #include "factory/constraint.hpp"
15 #include "unittest_common.hpp"
16
17 using namespace boost::unit_test;
18 using namespace crocoddyl::unittest;
19
20 //----------------------------------------------------------------------------//
21
22 ✗ void test_constructor(StateModelTypes::Type state_type) {
23 // Setup the test
24 ✗ StateModelFactory state_factory;
25 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
26
27 // Run the print function
28 ✗ std::ostringstream tmp;
29 ✗ tmp << model;
30
31 // Test the initial size of the map
32 ✗ BOOST_CHECK(model.get_constraints().size() == 0);
33
34 // Checking that casted computation is the same
35 #ifdef NDEBUG // Run only in release mode
36 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
37 model.cast<float>();
38 BOOST_CHECK(casted_model.get_constraints().size() == 0);
39 #endif
40 ✗ }
41
42 ✗ void test_addConstraint(StateModelTypes::Type state_type) {
43 // Setup the test
44 ✗ StateModelFactory state_factory;
45 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
46 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
47 ✗ model.cast<float>();
48
49 // add an active constraint
50 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint_1 =
51 ✗ create_random_constraint(state_type);
52 ✗ model.addConstraint("random_constraint_1", rand_constraint_1);
53 ✗ std::size_t ng = rand_constraint_1->get_ng();
54 ✗ std::size_t nh = rand_constraint_1->get_nh();
55 ✗ std::size_t ng_T = rand_constraint_1->get_T_constraint() ? ng : 0;
56 ✗ std::size_t nh_T = rand_constraint_1->get_T_constraint() ? nh : 0;
57 ✗ BOOST_CHECK(model.get_ng() == ng);
58 ✗ BOOST_CHECK(model.get_nh() == nh);
59 ✗ BOOST_CHECK(model.get_ng_T() == ng_T);
60 ✗ BOOST_CHECK(model.get_nh_T() == nh_T);
61
62 // add an inactive constraint
63 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint_2 =
64 ✗ create_random_constraint(state_type);
65 ✗ model.addConstraint("random_constraint_2", rand_constraint_2, false);
66 ✗ BOOST_CHECK(model.get_ng() == ng);
67 ✗ BOOST_CHECK(model.get_nh() == nh);
68 ✗ BOOST_CHECK(model.get_ng_T() == ng_T);
69 ✗ BOOST_CHECK(model.get_nh_T() == nh_T);
70
71 // change the random constraint 2 status
72 ✗ model.changeConstraintStatus("random_constraint_2", true);
73 ✗ ng += rand_constraint_2->get_ng();
74 ✗ nh += rand_constraint_2->get_nh();
75 ✗ if (rand_constraint_2->get_T_constraint()) {
76 ✗ ng_T += rand_constraint_2->get_ng();
77 ✗ nh_T += rand_constraint_2->get_nh();
78 }
79 ✗ BOOST_CHECK(model.get_ng() == ng);
80 ✗ BOOST_CHECK(model.get_nh() == nh);
81 ✗ BOOST_CHECK(model.get_ng_T() == ng_T);
82 ✗ BOOST_CHECK(model.get_nh_T() == nh_T);
83
84 // change the random constraint 1 status
85 ✗ model.changeConstraintStatus("random_constraint_1", false);
86 ✗ ng -= rand_constraint_1->get_ng();
87 ✗ nh -= rand_constraint_1->get_nh();
88 ✗ if (rand_constraint_1->get_T_constraint()) {
89 ✗ ng_T -= rand_constraint_1->get_ng();
90 ✗ nh_T -= rand_constraint_1->get_nh();
91 }
92 ✗ BOOST_CHECK(model.get_ng() == ng);
93 ✗ BOOST_CHECK(model.get_nh() == nh);
94 ✗ BOOST_CHECK(model.get_ng_T() == ng_T);
95 ✗ BOOST_CHECK(model.get_nh_T() == nh_T);
96
97 // Checking that casted computation is the same
98 #ifdef NDEBUG // Run only in release mode
99 std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>
100 casted_rand_constraint_1 = rand_constraint_1->cast<float>();
101 casted_model.addConstraint("random_constraint_1", casted_rand_constraint_1);
102 ng = casted_rand_constraint_1->get_ng();
103 nh = casted_rand_constraint_1->get_nh();
104 ng_T = casted_rand_constraint_1->get_T_constraint() ? ng : 0;
105 nh_T = casted_rand_constraint_1->get_T_constraint() ? nh : 0;
106 BOOST_CHECK(casted_model.get_ng() == ng);
107 BOOST_CHECK(casted_model.get_nh() == nh);
108 BOOST_CHECK(casted_model.get_ng_T() == ng_T);
109 BOOST_CHECK(casted_model.get_nh_T() == nh_T);
110 std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>
111 casted_rand_constraint_2 = rand_constraint_2->cast<float>();
112 casted_model.addConstraint("random_constraint_2", casted_rand_constraint_2,
113 false);
114 BOOST_CHECK(casted_model.get_ng() == ng);
115 BOOST_CHECK(casted_model.get_nh() == nh);
116 BOOST_CHECK(casted_model.get_ng_T() == ng_T);
117 BOOST_CHECK(casted_model.get_nh_T() == nh_T);
118 casted_model.changeConstraintStatus("random_constraint_2", true);
119 ng += casted_rand_constraint_2->get_ng();
120 nh += casted_rand_constraint_2->get_nh();
121 if (casted_rand_constraint_2->get_T_constraint()) {
122 ng_T += casted_rand_constraint_2->get_ng();
123 nh_T += casted_rand_constraint_2->get_nh();
124 }
125 BOOST_CHECK(casted_model.get_ng() == ng);
126 BOOST_CHECK(casted_model.get_nh() == nh);
127 BOOST_CHECK(casted_model.get_ng_T() == ng_T);
128 BOOST_CHECK(casted_model.get_nh_T() == nh_T);
129 casted_model.changeConstraintStatus("random_constraint_1", false);
130 ng -= casted_rand_constraint_1->get_ng();
131 nh -= casted_rand_constraint_1->get_nh();
132 if (casted_rand_constraint_1->get_T_constraint()) {
133 ng_T -= casted_rand_constraint_1->get_ng();
134 nh_T -= casted_rand_constraint_1->get_nh();
135 }
136 BOOST_CHECK(casted_model.get_ng() == ng);
137 BOOST_CHECK(casted_model.get_nh() == nh);
138 BOOST_CHECK(casted_model.get_ng_T() == ng_T);
139 BOOST_CHECK(casted_model.get_nh_T() == nh_T);
140 #endif
141 ✗ }
142
143 ✗ void test_addConstraint_error_message(StateModelTypes::Type state_type) {
144 // Setup the test
145 ✗ StateModelFactory state_factory;
146 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
147
148 // create an constraint object
149 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint =
150 ✗ create_random_constraint(state_type);
151
152 // add twice the same constraint object to the container
153 ✗ model.addConstraint("random_constraint", rand_constraint);
154
155 // test error message when we add a duplicate constraint
156 ✗ CaptureIOStream capture_ios;
157 ✗ capture_ios.beginCapture();
158 ✗ model.addConstraint("random_constraint", rand_constraint);
159 ✗ capture_ios.endCapture();
160 ✗ std::stringstream expected_buffer;
161 expected_buffer << "Warning: we couldn't add the random_constraint "
162 ✗ "constraint item, it already existed."
163 ✗ << std::endl;
164 ✗ BOOST_CHECK(capture_ios.str() == expected_buffer.str());
165
166 // test error message when we change the constraint status of an inexistent
167 // constraint
168 ✗ capture_ios.beginCapture();
169 ✗ model.changeConstraintStatus("no_exist_constraint", true);
170 ✗ capture_ios.endCapture();
171 ✗ expected_buffer.clear();
172 expected_buffer << "Warning: we couldn't change the status of the "
173 ✗ "no_exist_constraint constraint item, it doesn't exist."
174 ✗ << std::endl;
175 ✗ BOOST_CHECK(capture_ios.str() == expected_buffer.str());
176 ✗ }
177
178 ✗ void test_removeConstraint(StateModelTypes::Type state_type) {
179 // Setup the test
180 ✗ StateModelFactory state_factory;
181 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
182 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
183 ✗ model.cast<float>();
184
185 // add an active constraint
186 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint =
187 ✗ create_random_constraint(state_type);
188 ✗ model.addConstraint("random_constraint", rand_constraint);
189 ✗ std::size_t ng = rand_constraint->get_ng();
190 ✗ std::size_t nh = rand_constraint->get_nh();
191 ✗ std::size_t ng_T = rand_constraint->get_T_constraint() ? ng : 0;
192 ✗ std::size_t nh_T = rand_constraint->get_T_constraint() ? nh : 0;
193 ✗ BOOST_CHECK(model.get_ng() == ng);
194 ✗ BOOST_CHECK(model.get_nh() == nh);
195 ✗ BOOST_CHECK(model.get_ng_T() == ng_T);
196 ✗ BOOST_CHECK(model.get_nh_T() == nh_T);
197
198 // remove the constraint
199 ✗ model.removeConstraint("random_constraint");
200 ✗ BOOST_CHECK(model.get_ng() == 0);
201 ✗ BOOST_CHECK(model.get_nh() == 0);
202 ✗ BOOST_CHECK(model.get_ng_T() == 0);
203 ✗ BOOST_CHECK(model.get_nh_T() == 0);
204
205 // Checking that casted computation is the same
206 #ifdef NDEBUG // Run only in release mode
207 std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>
208 casted_rand_constraint = rand_constraint->cast<float>();
209 casted_model.addConstraint("random_constraint", casted_rand_constraint);
210 ng = casted_rand_constraint->get_ng();
211 nh = casted_rand_constraint->get_nh();
212 ng_T = casted_rand_constraint->get_T_constraint() ? ng : 0;
213 nh_T = casted_rand_constraint->get_T_constraint() ? nh : 0;
214 BOOST_CHECK(casted_model.get_ng() == ng);
215 BOOST_CHECK(casted_model.get_nh() == nh);
216 BOOST_CHECK(casted_model.get_ng_T() == ng_T);
217 BOOST_CHECK(casted_model.get_nh_T() == nh_T);
218 casted_model.removeConstraint("random_constraint");
219 BOOST_CHECK(casted_model.get_ng() == 0);
220 BOOST_CHECK(casted_model.get_nh() == 0);
221 BOOST_CHECK(casted_model.get_ng_T() == 0);
222 BOOST_CHECK(casted_model.get_nh_T() == 0);
223 #endif
224 ✗ }
225
226 ✗ void test_removeConstraint_error_message(StateModelTypes::Type state_type) {
227 // Setup the test
228 ✗ StateModelFactory state_factory;
229 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
230
231 // remove a none existing constraint form the container, we expect a cout
232 // message here
233 ✗ CaptureIOStream capture_ios;
234 ✗ capture_ios.beginCapture();
235 ✗ model.removeConstraint("random_constraint");
236 ✗ capture_ios.endCapture();
237
238 // Test that the error message is sent.
239 ✗ std::stringstream expected_buffer;
240 expected_buffer << "Warning: we couldn't remove the random_constraint "
241 ✗ "constraint item, it doesn't exist."
242 ✗ << std::endl;
243 ✗ BOOST_CHECK(capture_ios.str() == expected_buffer.str());
244 ✗ }
245
246 ✗ void test_calc(StateModelTypes::Type state_type) {
247 // setup the test
248 ✗ StateModelFactory state_factory;
249 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
250 // create the corresponding data object
251 const std::shared_ptr<crocoddyl::StateMultibody>& state =
252 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
253 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
254 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
255 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
256
257 // create and add some constraint objects
258 ✗ std::vector<std::shared_ptr<crocoddyl::ConstraintModelAbstract>> models;
259 ✗ std::vector<std::shared_ptr<crocoddyl::ConstraintDataAbstract>> datas;
260 ✗ for (std::size_t i = 0; i < 5; ++i) {
261 ✗ std::ostringstream os;
262 ✗ os << "random_constraint_" << i;
263 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& m =
264 ✗ create_random_constraint(state_type);
265 ✗ model.addConstraint(os.str(), m, 1.);
266 ✗ models.push_back(m);
267 ✗ datas.push_back(m->createData(&shared_data));
268 ✗ }
269
270 // create the data of the constraint sum
271 const std::shared_ptr<crocoddyl::ConstraintDataManager>& data =
272 ✗ model.createData(&shared_data);
273
274 // compute the constraint sum data for the case when all constraints are
275 // defined as active
276 ✗ const Eigen::VectorXd& x1 = state->rand();
277 ✗ const Eigen::VectorXd& u1 = Eigen::VectorXd::Random(model.get_nu());
278 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
279 x1);
280 ✗ model.calc(data, x1, u1);
281
282 // check the constraint against single constraint computations
283 ✗ std::size_t ng_i = 0;
284 ✗ std::size_t nh_i = 0;
285 ✗ Eigen::VectorXd g = Eigen::VectorXd::Zero(model.get_ng());
286 ✗ Eigen::VectorXd h = Eigen::VectorXd::Zero(model.get_nh());
287 ✗ for (std::size_t i = 0; i < 5; ++i) {
288 ✗ models[i]->calc(datas[i], x1, u1);
289 ✗ const std::size_t ng = models[i]->get_ng();
290 ✗ const std::size_t nh = models[i]->get_nh();
291 ✗ g.segment(ng_i, ng) = datas[i]->g;
292 ✗ h.segment(nh_i, nh) = datas[i]->h;
293 ✗ ng_i += ng;
294 ✗ nh_i += nh;
295 }
296 ✗ BOOST_CHECK(data->g.isApprox(g, 1e-9));
297 ✗ BOOST_CHECK(data->h.isApprox(h, 1e-9));
298
299 // Checking that casted computation is the same
300 #ifdef NDEBUG // Run only in release mode
301 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
302 model.cast<float>();
303 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
304 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
305 casted_model.get_state());
306 pinocchio::ModelTpl<float>& casted_pinocchio_model =
307 *casted_state->get_pinocchio().get();
308 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
309 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
310 &casted_pinocchio_data);
311 std::vector<std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>>
312 casted_models;
313 std::vector<std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>>
314 casted_datas;
315 for (std::size_t i = 0; i < 5; ++i) {
316 casted_models.push_back(models[i]->cast<float>());
317 casted_datas.push_back(casted_models[i]->createData(&casted_shared_data));
318 }
319 const std::shared_ptr<crocoddyl::ConstraintDataManagerTpl<float>>&
320 casted_data = casted_model.createData(&casted_shared_data);
321 const Eigen::VectorXf& x1_f = x1.cast<float>();
322 const Eigen::VectorXf& u1_f = u1.cast<float>();
323 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
324 &casted_pinocchio_data, x1_f);
325 casted_model.calc(casted_data, x1_f, u1_f);
326 ng_i = 0;
327 nh_i = 0;
328 Eigen::VectorXf g_f = Eigen::VectorXf::Zero(casted_model.get_ng());
329 Eigen::VectorXf h_f = Eigen::VectorXf::Zero(casted_model.get_nh());
330 for (std::size_t i = 0; i < 5; ++i) {
331 casted_models[i]->calc(casted_datas[i], x1_f, u1_f);
332 const std::size_t ng = casted_models[i]->get_ng();
333 const std::size_t nh = casted_models[i]->get_nh();
334 g_f.segment(ng_i, ng) = casted_datas[i]->g;
335 h_f.segment(nh_i, nh) = casted_datas[i]->h;
336 ng_i += ng;
337 nh_i += nh;
338 }
339 BOOST_CHECK(casted_data->g.isApprox(g_f, 1e-9f));
340 BOOST_CHECK(casted_data->h.isApprox(h_f, 1e-9f));
341 #endif
342 ✗ }
343
344 ✗ void test_calcDiff(StateModelTypes::Type state_type) {
345 // setup the test
346 ✗ StateModelFactory state_factory;
347 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
348 // create the corresponding data object
349 const std::shared_ptr<crocoddyl::StateMultibody>& state =
350 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
351 ✗ pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
352 ✗ pinocchio::Data pinocchio_data(pinocchio_model);
353 ✗ crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
354
355 // create and add some constraint objects
356 ✗ std::vector<std::shared_ptr<crocoddyl::ConstraintModelAbstract>> models;
357 ✗ std::vector<std::shared_ptr<crocoddyl::ConstraintDataAbstract>> datas;
358 ✗ for (std::size_t i = 0; i < 5; ++i) {
359 ✗ std::ostringstream os;
360 ✗ os << "random_constraint_" << i;
361 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& m =
362 ✗ create_random_constraint(state_type);
363 ✗ model.addConstraint(os.str(), m, 1.);
364 ✗ models.push_back(m);
365 ✗ datas.push_back(m->createData(&shared_data));
366 ✗ }
367
368 // create the data of the constraint sum
369 const std::shared_ptr<crocoddyl::ConstraintDataManager>& data =
370 ✗ model.createData(&shared_data);
371
372 // compute the constraint sum data for the case when all constraints are
373 // defined as active
374 ✗ Eigen::VectorXd x1 = state->rand();
375 ✗ const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());
376 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
377 x1);
378 ✗ model.calc(data, x1, u1);
379 ✗ model.calcDiff(data, x1, u1);
380
381 // check the constraint against single constraint computations
382 ✗ std::size_t ng_i = 0;
383 ✗ std::size_t nh_i = 0;
384 ✗ const std::size_t ndx = state->get_ndx();
385 ✗ const std::size_t nu = model.get_nu();
386 ✗ Eigen::VectorXd g = Eigen::VectorXd::Zero(model.get_ng());
387 ✗ Eigen::VectorXd h = Eigen::VectorXd::Zero(model.get_nh());
388 ✗ Eigen::MatrixXd Gx = Eigen::MatrixXd::Zero(model.get_ng(), ndx);
389 ✗ Eigen::MatrixXd Gu = Eigen::MatrixXd::Zero(model.get_ng(), nu);
390 ✗ Eigen::MatrixXd Hx = Eigen::MatrixXd::Zero(model.get_nh(), ndx);
391 ✗ Eigen::MatrixXd Hu = Eigen::MatrixXd::Zero(model.get_nh(), nu);
392 ✗ for (std::size_t i = 0; i < 5; ++i) {
393 ✗ models[i]->calc(datas[i], x1, u1);
394 ✗ models[i]->calcDiff(datas[i], x1, u1);
395 ✗ const std::size_t ng = models[i]->get_ng();
396 ✗ const std::size_t nh = models[i]->get_nh();
397 ✗ g.segment(ng_i, ng) = datas[i]->g;
398 ✗ h.segment(nh_i, nh) = datas[i]->h;
399 ✗ Gx.block(ng_i, 0, ng, ndx) = datas[i]->Gx;
400 ✗ Gu.block(ng_i, 0, ng, nu) = datas[i]->Gu;
401 ✗ Hx.block(nh_i, 0, nh, ndx) = datas[i]->Hx;
402 ✗ Hu.block(nh_i, 0, nh, nu) = datas[i]->Hu;
403 ✗ ng_i += ng;
404 ✗ nh_i += nh;
405 }
406 ✗ BOOST_CHECK(data->g.isApprox(g, 1e-9));
407 ✗ BOOST_CHECK(data->h.isApprox(h, 1e-9));
408 ✗ BOOST_CHECK(data->Gx.isApprox(Gx, 1e-9));
409 ✗ BOOST_CHECK(data->Gu.isApprox(Gu, 1e-9));
410 ✗ BOOST_CHECK(data->Hx.isApprox(Hx, 1e-9));
411 ✗ BOOST_CHECK(data->Hu.isApprox(Hu, 1e-9));
412
413 ✗ x1 = state->rand();
414 ✗ crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
415 x1);
416 ✗ data->resize(&model, false);
417 ✗ model.calc(data, x1);
418 ✗ model.calcDiff(data, x1);
419
420 ✗ const std::size_t ng_T = model.get_ng_T();
421 ✗ const std::size_t nh_T = model.get_nh_T();
422 ✗ ng_i = 0;
423 ✗ nh_i = 0;
424 ✗ g.conservativeResize(ng_T);
425 ✗ h.conservativeResize(nh_T);
426 ✗ Gx.conservativeResize(ng_T, ndx);
427 ✗ Gu.conservativeResize(ng_T, nu);
428 ✗ Hx.conservativeResize(nh_T, ndx);
429 ✗ Hu.conservativeResize(nh_T, nu);
430 ✗ for (std::size_t i = 0; i < 5; ++i) {
431 ✗ if (models[i]->get_T_constraint()) {
432 ✗ models[i]->calc(datas[i], x1);
433 ✗ models[i]->calcDiff(datas[i], x1);
434 ✗ const std::size_t ng = models[i]->get_ng();
435 ✗ const std::size_t nh = models[i]->get_nh();
436 ✗ g.segment(ng_i, ng) = datas[i]->g;
437 ✗ h.segment(nh_i, nh) = datas[i]->h;
438 ✗ Gx.block(ng_i, 0, ng, ndx) = datas[i]->Gx;
439 ✗ Gu.block(ng_i, 0, ng, nu) = datas[i]->Gu;
440 ✗ Hx.block(nh_i, 0, nh, ndx) = datas[i]->Hx;
441 ✗ Hu.block(nh_i, 0, nh, nu) = datas[i]->Hu;
442 ✗ ng_i += ng;
443 ✗ nh_i += nh;
444 }
445 }
446 ✗ BOOST_CHECK(data->g.isApprox(g, 1e-9));
447 ✗ BOOST_CHECK(data->h.isApprox(h, 1e-9));
448 ✗ BOOST_CHECK(data->Gx.isApprox(Gx, 1e-9));
449 ✗ BOOST_CHECK(data->Hx.isApprox(Hx, 1e-9));
450
451 // Checking that casted computation is the same
452 #ifdef NDEBUG // Run only in release mode
453 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
454 model.cast<float>();
455 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
456 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
457 casted_model.get_state());
458 pinocchio::ModelTpl<float>& casted_pinocchio_model =
459 *casted_state->get_pinocchio().get();
460 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
461 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
462 &casted_pinocchio_data);
463 std::vector<std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>>
464 casted_models;
465 std::vector<std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>>
466 casted_datas;
467 for (std::size_t i = 0; i < 5; ++i) {
468 casted_models.push_back(models[i]->cast<float>());
469 casted_datas.push_back(casted_models[i]->createData(&casted_shared_data));
470 }
471 const std::shared_ptr<crocoddyl::ConstraintDataManagerTpl<float>>&
472 casted_data = casted_model.createData(&casted_shared_data);
473 const Eigen::VectorXf& x1_f = x1.cast<float>();
474 const Eigen::VectorXf& u1_f = u1.cast<float>();
475 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
476 &casted_pinocchio_data, x1_f);
477
478 casted_model.calc(casted_data, x1_f, u1_f);
479 casted_model.calcDiff(casted_data, x1_f, u1_f);
480
481 ng_i = 0;
482 nh_i = 0;
483 Eigen::VectorXf g_f = Eigen::VectorXf::Zero(casted_model.get_ng());
484 Eigen::VectorXf h_f = Eigen::VectorXf::Zero(casted_model.get_nh());
485 Eigen::MatrixXf Gx_f = Eigen::MatrixXf::Zero(casted_model.get_ng(), ndx);
486 Eigen::MatrixXf Gu_f = Eigen::MatrixXf::Zero(casted_model.get_ng(), nu);
487 Eigen::MatrixXf Hx_f = Eigen::MatrixXf::Zero(casted_model.get_nh(), ndx);
488 Eigen::MatrixXf Hu_f = Eigen::MatrixXf::Zero(casted_model.get_nh(), nu);
489 for (std::size_t i = 0; i < 5; ++i) {
490 casted_models[i]->calc(casted_datas[i], x1_f, u1_f);
491 casted_models[i]->calcDiff(casted_datas[i], x1_f, u1_f);
492 const std::size_t ng = casted_models[i]->get_ng();
493 const std::size_t nh = casted_models[i]->get_nh();
494 g_f.segment(ng_i, ng) = casted_datas[i]->g;
495 h_f.segment(nh_i, nh) = casted_datas[i]->h;
496 Gx_f.block(ng_i, 0, ng, ndx) = casted_datas[i]->Gx;
497 Gu_f.block(ng_i, 0, ng, nu) = casted_datas[i]->Gu;
498 Hx_f.block(nh_i, 0, nh, ndx) = casted_datas[i]->Hx;
499 Hu_f.block(nh_i, 0, nh, nu) = casted_datas[i]->Hu;
500 ng_i += ng;
501 nh_i += nh;
502 }
503 BOOST_CHECK(casted_data->g.isApprox(g_f, 1e-9f));
504 BOOST_CHECK(casted_data->h.isApprox(h_f, 1e-9f));
505 BOOST_CHECK(casted_data->Gx.isApprox(Gx_f, 1e-9f));
506 BOOST_CHECK(casted_data->Gu.isApprox(Gu_f, 1e-9f));
507 BOOST_CHECK(casted_data->Hx.isApprox(Hx_f, 1e-9f));
508 BOOST_CHECK(casted_data->Hu.isApprox(Hu_f, 1e-9f));
509 #endif
510 ✗ }
511
512 ✗ void test_get_constraints(StateModelTypes::Type state_type) {
513 // setup the test
514 ✗ StateModelFactory state_factory;
515 ✗ crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
516 // create the corresponding data object
517 const std::shared_ptr<crocoddyl::StateMultibody>& state =
518 ✗ std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
519 ✗ pinocchio::Data pinocchio_data(*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_constraint_" << i;
525 ✗ model.addConstraint(os.str(), create_random_constraint(state_type), 1.);
526 ✗ }
527
528 // get the contacts
529 const crocoddyl::ConstraintModelManager::ConstraintModelContainer&
530 ✗ constraints = model.get_constraints();
531
532 // test
533 crocoddyl::ConstraintModelManager::ConstraintModelContainer::const_iterator
534 ✗ it_m,
535 ✗ end_m;
536 unsigned i;
537 ✗ for (i = 0, it_m = constraints.begin(), end_m = constraints.end();
538 ✗ it_m != end_m; ++it_m, ++i) {
539 ✗ std::ostringstream os;
540 ✗ os << "random_constraint_" << i;
541 ✗ BOOST_CHECK(it_m->first == os.str());
542 ✗ }
543 ✗ }
544
545 ✗ void test_shareMemory(StateModelTypes::Type state_type) {
546 // setup the test
547 ✗ StateModelFactory state_factory;
548 const std::shared_ptr<crocoddyl::StateAbstract> state =
549 ✗ state_factory.create(state_type);
550 ✗ crocoddyl::ConstraintModelManager constraint_model(state);
551 ✗ crocoddyl::DataCollectorAbstract shared_data;
552 const std::shared_ptr<crocoddyl::ConstraintDataManager>& constraint_data =
553 ✗ constraint_model.createData(&shared_data);
554
555 ✗ std::size_t ng = state->get_ndx();
556 ✗ std::size_t nh = state->get_ndx();
557 ✗ const std::size_t ndx = state->get_ndx();
558 ✗ const std::size_t nu = constraint_model.get_nu();
559 ✗ crocoddyl::ActionModelLQR action_model(ndx, nu);
560 const std::shared_ptr<crocoddyl::ActionDataAbstract>& action_data =
561 ✗ action_model.createData();
562
563 ✗ action_data->h.resize(nh);
564 ✗ action_data->g.resize(ng);
565 ✗ action_data->Gx.resize(ng, ndx);
566 ✗ action_data->Gu.resize(ng, nu);
567 ✗ action_data->Hx.resize(nh, ndx);
568 ✗ action_data->Hu.resize(nh, nu);
569 ✗ constraint_data->shareMemory(action_data.get());
570 ✗ constraint_data->h = Eigen::VectorXd::Random(nh);
571 ✗ constraint_data->g = Eigen::VectorXd::Random(ng);
572 ✗ constraint_data->Gx = Eigen::MatrixXd::Random(ng, ndx);
573 ✗ constraint_data->Gu = Eigen::MatrixXd::Random(ng, nu);
574 ✗ constraint_data->Hx = Eigen::MatrixXd::Random(nh, ndx);
575 ✗ constraint_data->Hu = Eigen::MatrixXd::Random(nh, nu);
576
577 // check that the data has been shared
578 ✗ BOOST_CHECK(action_data->g.isApprox(constraint_data->g, 1e-9));
579 ✗ BOOST_CHECK(action_data->h.isApprox(constraint_data->h, 1e-9));
580 ✗ BOOST_CHECK(action_data->Gx.isApprox(constraint_data->Gx, 1e-9));
581 ✗ BOOST_CHECK(action_data->Gu.isApprox(constraint_data->Gu, 1e-9));
582 ✗ BOOST_CHECK(action_data->Hx.isApprox(constraint_data->Hx, 1e-9));
583 ✗ BOOST_CHECK(action_data->Hu.isApprox(constraint_data->Hu, 1e-9));
584
585 // let's now resize the data
586 ✗ constraint_data->resize(&action_model, action_data.get());
587
588 // check that the shared data has been resized
589 ✗ BOOST_CHECK(action_data->g.isApprox(constraint_data->g, 1e-9));
590 ✗ BOOST_CHECK(action_data->h.isApprox(constraint_data->h, 1e-9));
591 ✗ BOOST_CHECK(action_data->Gx.isApprox(constraint_data->Gx, 1e-9));
592 ✗ BOOST_CHECK(action_data->Gu.isApprox(constraint_data->Gu, 1e-9));
593 ✗ BOOST_CHECK(action_data->Hx.isApprox(constraint_data->Hx, 1e-9));
594 ✗ BOOST_CHECK(action_data->Hu.isApprox(constraint_data->Hu, 1e-9));
595 ✗ }
596
597 //----------------------------------------------------------------------------//
598
599 ✗ void register_unit_tests(StateModelTypes::Type state_type) {
600 ✗ boost::test_tools::output_test_stream test_name;
601 test_name << "test_ConstraintModelManager"
602 ✗ << "_" << state_type;
603 ✗ std::cout << "Running " << test_name.str() << std::endl;
604 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
605 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_constructor, state_type)));
606 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_addConstraint, state_type)));
607 ✗ ts->add(BOOST_TEST_CASE(
608 boost::bind(&test_addConstraint_error_message, state_type)));
609 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_removeConstraint, state_type)));
610 ✗ ts->add(BOOST_TEST_CASE(
611 boost::bind(&test_removeConstraint_error_message, state_type)));
612 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, state_type)));
613 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, state_type)));
614 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_get_constraints, state_type)));
615 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_shareMemory, state_type)));
616 ✗ framework::master_test_suite().add(ts);
617 ✗ }
618
619 ✗ bool init_function() {
620 ✗ register_unit_tests(StateModelTypes::StateMultibody_TalosArm);
621 ✗ register_unit_tests(StateModelTypes::StateMultibody_HyQ);
622 ✗ register_unit_tests(StateModelTypes::StateMultibody_Talos);
623 ✗ return true;
624 }
625
626 ✗ int main(int argc, char** argv) {
627 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
628 }
629