GCC Code Coverage Report

Directory: ./
File: unittest/test_constraint_manager.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 306 306 100.0%
Branches: 732 1438 50.9%
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 3 void test_constructor(StateModelTypes::Type state_type) {
23 // Setup the test
24
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
25
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
26
27 // Run the print function
28
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::ostringstream tmp;
29
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 tmp << model;
30
31 // Test the initial size of the map
32
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 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 3 }
41
42 3 void test_addConstraint(StateModelTypes::Type state_type) {
43 // Setup the test
44
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
45
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
46 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
47
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.cast<float>();
48
49 // add an active constraint
50 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint_1 =
51
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 create_random_constraint(state_type);
52
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 model.addConstraint("random_constraint_1", rand_constraint_1);
53
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::size_t ng = rand_constraint_1->get_ng();
54
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::size_t nh = rand_constraint_1->get_nh();
55
3/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 std::size_t ng_T = rand_constraint_1->get_T_constraint() ? ng : 0;
56
3/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 std::size_t nh_T = rand_constraint_1->get_T_constraint() ? nh : 0;
57
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng() == ng);
58
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh() == nh);
59
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng_T() == ng_T);
60
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh_T() == nh_T);
61
62 // add an inactive constraint
63 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint_2 =
64
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 create_random_constraint(state_type);
65
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 model.addConstraint("random_constraint_2", rand_constraint_2, false);
66
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng() == ng);
67
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh() == nh);
68
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng_T() == ng_T);
69
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh_T() == nh_T);
70
71 // change the random constraint 2 status
72
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 model.changeConstraintStatus("random_constraint_2", true);
73
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ng += rand_constraint_2->get_ng();
74
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 nh += rand_constraint_2->get_nh();
75
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 if (rand_constraint_2->get_T_constraint()) {
76
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ng_T += rand_constraint_2->get_ng();
77
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 nh_T += rand_constraint_2->get_nh();
78 }
79
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng() == ng);
80
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh() == nh);
81
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng_T() == ng_T);
82
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh_T() == nh_T);
83
84 // change the random constraint 1 status
85
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 model.changeConstraintStatus("random_constraint_1", false);
86
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 ng -= rand_constraint_1->get_ng();
87
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 nh -= rand_constraint_1->get_nh();
88
3/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 if (rand_constraint_1->get_T_constraint()) {
89
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 ng_T -= rand_constraint_1->get_ng();
90
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 nh_T -= rand_constraint_1->get_nh();
91 }
92
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng() == ng);
93
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh() == nh);
94
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng_T() == ng_T);
95
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 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 3 }
142
143 3 void test_addConstraint_error_message(StateModelTypes::Type state_type) {
144 // Setup the test
145
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
146
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
147
148 // create an constraint object
149 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint =
150
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 create_random_constraint(state_type);
151
152 // add twice the same constraint object to the container
153
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 model.addConstraint("random_constraint", rand_constraint);
154
155 // test error message when we add a duplicate constraint
156
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 CaptureIOStream capture_ios;
157
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.beginCapture();
158
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 model.addConstraint("random_constraint", rand_constraint);
159
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.endCapture();
160
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::stringstream expected_buffer;
161 expected_buffer << "Warning: we couldn't add the random_constraint "
162
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 "constraint item, it already existed."
163
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 << std::endl;
164
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 3 times.
 3 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
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.beginCapture();
169
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 model.changeConstraintStatus("no_exist_constraint", true);
170
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.endCapture();
171
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 expected_buffer.clear();
172 expected_buffer << "Warning: we couldn't change the status of the "
173
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 "no_exist_constraint constraint item, it doesn't exist."
174
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 << std::endl;
175
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 3 times.
 3 BOOST_CHECK(capture_ios.str() == expected_buffer.str());
176 3 }
177
178 3 void test_removeConstraint(StateModelTypes::Type state_type) {
179 // Setup the test
180
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
181
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
182 crocoddyl::ConstraintModelManagerTpl<float> casted_model =
183
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.cast<float>();
184
185 // add an active constraint
186 std::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint =
187
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 create_random_constraint(state_type);
188
2/4
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 model.addConstraint("random_constraint", rand_constraint);
189
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::size_t ng = rand_constraint->get_ng();
190
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::size_t nh = rand_constraint->get_nh();
191
3/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 std::size_t ng_T = rand_constraint->get_T_constraint() ? ng : 0;
192
3/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 1 times.
 3 std::size_t nh_T = rand_constraint->get_T_constraint() ? nh : 0;
193
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng() == ng);
194
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh() == nh);
195
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng_T() == ng_T);
196
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh_T() == nh_T);
197
198 // remove the constraint
199
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 model.removeConstraint("random_constraint");
200
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng() == 0);
201
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_nh() == 0);
202
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 BOOST_CHECK(model.get_ng_T() == 0);
203
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 3 times.
 3 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 3 }
225
226 3 void test_removeConstraint_error_message(StateModelTypes::Type state_type) {
227 // Setup the test
228
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
229
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 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
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 CaptureIOStream capture_ios;
234
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.beginCapture();
235
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 model.removeConstraint("random_constraint");
236
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 capture_ios.endCapture();
237
238 // Test that the error message is sent.
239
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::stringstream expected_buffer;
240 expected_buffer << "Warning: we couldn't remove the random_constraint "
241
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 "constraint item, it doesn't exist."
242
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 << std::endl;
243
8/16
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 3 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 3 times.
✗ Branch 25 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 3 times.
 3 BOOST_CHECK(capture_ios.str() == expected_buffer.str());
244 3 }
245
246 3 void test_calc(StateModelTypes::Type state_type) {
247 // setup the test
248
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
249
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
250 // create the corresponding data object
251 const std::shared_ptr<crocoddyl::StateMultibody>& state =
252
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
253
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
254
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 pinocchio::Data pinocchio_data(pinocchio_model);
255
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
256
257 // create and add some constraint objects
258 3 std::vector<std::shared_ptr<crocoddyl::ConstraintModelAbstract>> models;
259 3 std::vector<std::shared_ptr<crocoddyl::ConstraintDataAbstract>> datas;
260
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
261
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
262
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 os << "random_constraint_" << i;
263 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& m =
264
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 create_random_constraint(state_type);
265
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 model.addConstraint(os.str(), m, 1.);
266
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 models.push_back(m);
267
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 datas.push_back(m->createData(&shared_data));
268 15 }
269
270 // create the data of the constraint sum
271 const std::shared_ptr<crocoddyl::ConstraintDataManager>& data =
272
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.createData(&shared_data);
273
274 // compute the constraint sum data for the case when all constraints are
275 // defined as active
276
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const Eigen::VectorXd& x1 = state->rand();
277
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 const Eigen::VectorXd& u1 = Eigen::VectorXd::Random(model.get_nu());
278
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
279 x1);
280
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 model.calc(data, x1, u1);
281
282 // check the constraint against single constraint computations
283 3 std::size_t ng_i = 0;
284 3 std::size_t nh_i = 0;
285
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::VectorXd g = Eigen::VectorXd::Zero(model.get_ng());
286
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::VectorXd h = Eigen::VectorXd::Zero(model.get_nh());
287
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
288
3/6
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 15 models[i]->calc(datas[i], x1, u1);
289
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 const std::size_t ng = models[i]->get_ng();
290
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 const std::size_t nh = models[i]->get_nh();
291
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 g.segment(ng_i, ng) = datas[i]->g;
292
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 h.segment(nh_i, nh) = datas[i]->h;
293 15 ng_i += ng;
294 15 nh_i += nh;
295 }
296
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->g.isApprox(g, 1e-9));
297
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 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 3 }
343
344 3 void test_calcDiff(StateModelTypes::Type state_type) {
345 // setup the test
346
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
347
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
348 // create the corresponding data object
349 const std::shared_ptr<crocoddyl::StateMultibody>& state =
350
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
351
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
352
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 pinocchio::Data pinocchio_data(pinocchio_model);
353
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
354
355 // create and add some constraint objects
356 3 std::vector<std::shared_ptr<crocoddyl::ConstraintModelAbstract>> models;
357 3 std::vector<std::shared_ptr<crocoddyl::ConstraintDataAbstract>> datas;
358
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
359
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
360
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 os << "random_constraint_" << i;
361 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& m =
362
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 create_random_constraint(state_type);
363
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 model.addConstraint(os.str(), m, 1.);
364
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 models.push_back(m);
365
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 datas.push_back(m->createData(&shared_data));
366 15 }
367
368 // create the data of the constraint sum
369 const std::shared_ptr<crocoddyl::ConstraintDataManager>& data =
370
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 model.createData(&shared_data);
371
372 // compute the constraint sum data for the case when all constraints are
373 // defined as active
374
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 Eigen::VectorXd x1 = state->rand();
375
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu());
376
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
377 x1);
378
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 model.calc(data, x1, u1);
379
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 model.calcDiff(data, x1, u1);
380
381 // check the constraint against single constraint computations
382 3 std::size_t ng_i = 0;
383 3 std::size_t nh_i = 0;
384
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::size_t ndx = state->get_ndx();
385
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const std::size_t nu = model.get_nu();
386
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::VectorXd g = Eigen::VectorXd::Zero(model.get_ng());
387
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::VectorXd h = Eigen::VectorXd::Zero(model.get_nh());
388
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::MatrixXd Gx = Eigen::MatrixXd::Zero(model.get_ng(), ndx);
389
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::MatrixXd Gu = Eigen::MatrixXd::Zero(model.get_ng(), nu);
390
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::MatrixXd Hx = Eigen::MatrixXd::Zero(model.get_nh(), ndx);
391
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 Eigen::MatrixXd Hu = Eigen::MatrixXd::Zero(model.get_nh(), nu);
392
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
393
3/6
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 15 models[i]->calc(datas[i], x1, u1);
394
3/6
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
 15 models[i]->calcDiff(datas[i], x1, u1);
395
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 const std::size_t ng = models[i]->get_ng();
396
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 const std::size_t nh = models[i]->get_nh();
397
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 g.segment(ng_i, ng) = datas[i]->g;
398
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 h.segment(nh_i, nh) = datas[i]->h;
399
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 Gx.block(ng_i, 0, ng, ndx) = datas[i]->Gx;
400
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 Gu.block(ng_i, 0, ng, nu) = datas[i]->Gu;
401
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 Hx.block(nh_i, 0, nh, ndx) = datas[i]->Hx;
402
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 15 Hu.block(nh_i, 0, nh, nu) = datas[i]->Hu;
403 15 ng_i += ng;
404 15 nh_i += nh;
405 }
406
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->g.isApprox(g, 1e-9));
407
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->h.isApprox(h, 1e-9));
408
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->Gx.isApprox(Gx, 1e-9));
409
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->Gu.isApprox(Gu, 1e-9));
410
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->Hx.isApprox(Hx, 1e-9));
411
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->Hu.isApprox(Hu, 1e-9));
412
413
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 x1 = state->rand();
414
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data,
415 x1);
416
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 data->resize(&model, false);
417
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 model.calc(data, x1);
418
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 model.calcDiff(data, x1);
419
420
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const std::size_t ng_T = model.get_ng_T();
421
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const std::size_t nh_T = model.get_nh_T();
422 3 ng_i = 0;
423 3 nh_i = 0;
424
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 g.conservativeResize(ng_T);
425
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 h.conservativeResize(nh_T);
426
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Gx.conservativeResize(ng_T, ndx);
427
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Gu.conservativeResize(ng_T, nu);
428
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Hx.conservativeResize(nh_T, ndx);
429
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Hu.conservativeResize(nh_T, nu);
430
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (std::size_t i = 0; i < 5; ++i) {
431
3/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 13 times.
✓ Branch 6 taken 2 times.
 15 if (models[i]->get_T_constraint()) {
432
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 models[i]->calc(datas[i], x1);
433
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 models[i]->calcDiff(datas[i], x1);
434
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 const std::size_t ng = models[i]->get_ng();
435
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 const std::size_t nh = models[i]->get_nh();
436
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 g.segment(ng_i, ng) = datas[i]->g;
437
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 h.segment(nh_i, nh) = datas[i]->h;
438
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 Gx.block(ng_i, 0, ng, ndx) = datas[i]->Gx;
439
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 Gu.block(ng_i, 0, ng, nu) = datas[i]->Gu;
440
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 Hx.block(nh_i, 0, nh, ndx) = datas[i]->Hx;
441
2/4
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 Hu.block(nh_i, 0, nh, nu) = datas[i]->Hu;
442 13 ng_i += ng;
443 13 nh_i += nh;
444 }
445 }
446
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->g.isApprox(g, 1e-9));
447
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->h.isApprox(h, 1e-9));
448
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 BOOST_CHECK(data->Gx.isApprox(Gx, 1e-9));
449
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 3 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 3 times.
 3 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 3 }
511
512 3 void test_get_constraints(StateModelTypes::Type state_type) {
513 // setup the test
514
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
515
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 crocoddyl::ConstraintModelManager model(state_factory.create(state_type));
516 // create the corresponding data object
517 const std::shared_ptr<crocoddyl::StateMultibody>& state =
518
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 std::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state());
519
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 pinocchio::Data pinocchio_data(*state->get_pinocchio().get());
520
521 // create and add some contact objects
522
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 3 times.
 18 for (unsigned i = 0; i < 5; ++i) {
523
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
524
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 os << "random_constraint_" << i;
525
3/6
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 15 model.addConstraint(os.str(), create_random_constraint(state_type), 1.);
526 15 }
527
528 // get the contacts
529 const crocoddyl::ConstraintModelManager::ConstraintModelContainer&
530
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 constraints = model.get_constraints();
531
532 // test
533 crocoddyl::ConstraintModelManager::ConstraintModelContainer::const_iterator
534 3 it_m,
535 3 end_m;
536 unsigned i;
537 3 for (i = 0, it_m = constraints.begin(), end_m = constraints.end();
538
2/2
✓ Branch 2 taken 15 times.
✓ Branch 3 taken 3 times.
 18 it_m != end_m; ++it_m, ++i) {
539
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 std::ostringstream os;
540
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 os << "random_constraint_" << i;
541
7/14
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 15 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 15 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 15 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 15 times.
✗ Branch 23 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 15 times.
 15 BOOST_CHECK(it_m->first == os.str());
542 15 }
543 3 }
544
545 3 void test_shareMemory(StateModelTypes::Type state_type) {
546 // setup the test
547
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 StateModelFactory state_factory;
548 const std::shared_ptr<crocoddyl::StateAbstract> state =
549
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 state_factory.create(state_type);
550
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 crocoddyl::ConstraintModelManager constraint_model(state);
551
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::DataCollectorAbstract shared_data;
552 const std::shared_ptr<crocoddyl::ConstraintDataManager>& constraint_data =
553
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 constraint_model.createData(&shared_data);
554
555
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::size_t ng = state->get_ndx();
556
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 std::size_t nh = state->get_ndx();
557
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::size_t ndx = state->get_ndx();
558
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 const std::size_t nu = constraint_model.get_nu();
559
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 crocoddyl::ActionModelLQR action_model(ndx, nu);
560 const std::shared_ptr<crocoddyl::ActionDataAbstract>& action_data =
561
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 action_model.createData();
562
563
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 action_data->h.resize(nh);
564
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 action_data->g.resize(ng);
565
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 action_data->Gx.resize(ng, ndx);
566
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 action_data->Gu.resize(ng, nu);
567
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 action_data->Hx.resize(nh, ndx);
568
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 action_data->Hu.resize(nh, nu);
569
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 constraint_data->shareMemory(action_data.get());
570
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 constraint_data->h = Eigen::VectorXd::Random(nh);
571
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 constraint_data->g = Eigen::VectorXd::Random(ng);
572
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 constraint_data->Gx = Eigen::MatrixXd::Random(ng, ndx);
573
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 constraint_data->Gu = Eigen::MatrixXd::Random(ng, nu);
574
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 constraint_data->Hx = Eigen::MatrixXd::Random(nh, ndx);
575
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 constraint_data->Hu = Eigen::MatrixXd::Random(nh, nu);
576
577 // check that the data has been shared
578
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->g.isApprox(constraint_data->g, 1e-9));
579
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->h.isApprox(constraint_data->h, 1e-9));
580
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Gx.isApprox(constraint_data->Gx, 1e-9));
581
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Gu.isApprox(constraint_data->Gu, 1e-9));
582
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Hx.isApprox(constraint_data->Hx, 1e-9));
583
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Hu.isApprox(constraint_data->Hu, 1e-9));
584
585 // let's now resize the data
586
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 3 constraint_data->resize(&action_model, action_data.get());
587
588 // check that the shared data has been resized
589
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->g.isApprox(constraint_data->g, 1e-9));
590
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->h.isApprox(constraint_data->h, 1e-9));
591
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Gx.isApprox(constraint_data->Gx, 1e-9));
592
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Gu.isApprox(constraint_data->Gu, 1e-9));
593
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Hx.isApprox(constraint_data->Hx, 1e-9));
594
7/14
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 3 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 3 times.
 3 BOOST_CHECK(action_data->Hu.isApprox(constraint_data->Hu, 1e-9));
595 3 }
596
597 //----------------------------------------------------------------------------//
598
599 3 void register_unit_tests(StateModelTypes::Type state_type) {
600
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 boost::test_tools::output_test_stream test_name;
601 test_name << "test_ConstraintModelManager"
602
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 << "_" << state_type;
603
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 std::cout << "Running " << test_name.str() << std::endl;
604
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 3 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
605
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_constructor, state_type)));
606
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_addConstraint, state_type)));
607
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(
608 boost::bind(&test_addConstraint_error_message, state_type)));
609
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_removeConstraint, state_type)));
610
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(
611 boost::bind(&test_removeConstraint_error_message, state_type)));
612
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, state_type)));
613
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, state_type)));
614
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_get_constraints, state_type)));
615
5/10
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
 3 ts->add(BOOST_TEST_CASE(boost::bind(&test_shareMemory, state_type)));
616
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 framework::master_test_suite().add(ts);
617 3 }
618
619 1 bool init_function() {
620 1 register_unit_tests(StateModelTypes::StateMultibody_TalosArm);
621 1 register_unit_tests(StateModelTypes::StateMultibody_HyQ);
622 1 register_unit_tests(StateModelTypes::StateMultibody_Talos);
623 1 return true;
624 }
625
626 1 int main(int argc, char** argv) {
627 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
628 }
629