| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2021-2023, 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 |
✓✗ | 6 |
StateModelFactory state_factory; |
25 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
26 |
|||
27 |
// Run the print function |
||
28 |
✓✗ | 6 |
std::ostringstream tmp; |
29 |
✓✗ | 3 |
tmp << model; |
30 |
|||
31 |
// Test the initial size of the map |
||
32 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_constraints().size() == 0); |
33 |
3 |
} |
|
34 |
|||
35 |
3 |
void test_addConstraint(StateModelTypes::Type state_type) { |
|
36 |
// Setup the test |
||
37 |
✓✗ | 6 |
StateModelFactory state_factory; |
38 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
39 |
|||
40 |
// add an active constraint |
||
41 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint_1 = |
||
42 |
✓✗ | 6 |
create_random_constraint(state_type); |
43 |
✓✗✓✗ |
3 |
model.addConstraint("random_constraint_1", rand_constraint_1); |
44 |
3 |
std::size_t ng = rand_constraint_1->get_ng(); |
|
45 |
3 |
std::size_t nh = rand_constraint_1->get_nh(); |
|
46 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_ng() == ng); |
47 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nh() == nh); |
48 |
|||
49 |
// add an inactive constraint |
||
50 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint_2 = |
||
51 |
✓✗ | 6 |
create_random_constraint(state_type); |
52 |
✓✗✓✗ |
3 |
model.addConstraint("random_constraint_2", rand_constraint_2, false); |
53 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_ng() == ng); |
54 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nh() == nh); |
55 |
|||
56 |
// change the random constraint 2 status |
||
57 |
✓✗✓✗ |
3 |
model.changeConstraintStatus("random_constraint_2", true); |
58 |
3 |
ng += rand_constraint_2->get_ng(); |
|
59 |
3 |
nh += rand_constraint_2->get_nh(); |
|
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_ng() == ng); |
61 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nh() == nh); |
62 |
|||
63 |
// change the random constraint 1 status |
||
64 |
✓✗✓✗ |
3 |
model.changeConstraintStatus("random_constraint_1", false); |
65 |
3 |
ng -= rand_constraint_1->get_ng(); |
|
66 |
3 |
nh -= rand_constraint_1->get_nh(); |
|
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_ng() == ng); |
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nh() == nh); |
69 |
3 |
} |
|
70 |
|||
71 |
3 |
void test_addConstraint_error_message(StateModelTypes::Type state_type) { |
|
72 |
// Setup the test |
||
73 |
✓✗ | 6 |
StateModelFactory state_factory; |
74 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
75 |
|||
76 |
// create an constraint object |
||
77 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint = |
||
78 |
✓✗ | 6 |
create_random_constraint(state_type); |
79 |
|||
80 |
// add twice the same constraint object to the container |
||
81 |
✓✗✓✗ |
3 |
model.addConstraint("random_constraint", rand_constraint); |
82 |
|||
83 |
// test error message when we add a duplicate constraint |
||
84 |
✓✗ | 6 |
CaptureIOStream capture_ios; |
85 |
✓✗ | 3 |
capture_ios.beginCapture(); |
86 |
✓✗✓✗ |
3 |
model.addConstraint("random_constraint", rand_constraint); |
87 |
✓✗ | 3 |
capture_ios.endCapture(); |
88 |
✓✗ | 6 |
std::stringstream expected_buffer; |
89 |
expected_buffer << "Warning: we couldn't add the random_constraint " |
||
90 |
✓✗ | 3 |
"constraint item, it already existed." |
91 |
✓✗ | 3 |
<< std::endl; |
92 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(capture_ios.str() == expected_buffer.str()); |
93 |
|||
94 |
// test error message when we change the constraint status of an inexistent |
||
95 |
// constraint |
||
96 |
✓✗ | 3 |
capture_ios.beginCapture(); |
97 |
✓✗✓✗ |
3 |
model.changeConstraintStatus("no_exist_constraint", true); |
98 |
✓✗ | 3 |
capture_ios.endCapture(); |
99 |
✓✗ | 3 |
expected_buffer.clear(); |
100 |
expected_buffer << "Warning: we couldn't change the status of the " |
||
101 |
✓✗ | 3 |
"no_exist_constraint constraint item, it doesn't exist." |
102 |
✓✗ | 3 |
<< std::endl; |
103 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(capture_ios.str() == expected_buffer.str()); |
104 |
3 |
} |
|
105 |
|||
106 |
3 |
void test_removeConstraint(StateModelTypes::Type state_type) { |
|
107 |
// Setup the test |
||
108 |
✓✗ | 6 |
StateModelFactory state_factory; |
109 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
110 |
|||
111 |
// add an active constraint |
||
112 |
boost::shared_ptr<crocoddyl::ConstraintModelAbstract> rand_constraint = |
||
113 |
✓✗ | 6 |
create_random_constraint(state_type); |
114 |
✓✗✓✗ |
3 |
model.addConstraint("random_constraint", rand_constraint); |
115 |
3 |
std::size_t ng = rand_constraint->get_ng(); |
|
116 |
3 |
std::size_t nh = rand_constraint->get_nh(); |
|
117 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_ng() == ng); |
118 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nh() == nh); |
119 |
|||
120 |
// remove the constraint |
||
121 |
✓✗✓✗ |
3 |
model.removeConstraint("random_constraint"); |
122 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_ng() == 0); |
123 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nh() == 0); |
124 |
3 |
} |
|
125 |
|||
126 |
3 |
void test_removeConstraint_error_message(StateModelTypes::Type state_type) { |
|
127 |
// Setup the test |
||
128 |
✓✗ | 6 |
StateModelFactory state_factory; |
129 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
130 |
|||
131 |
// remove a none existing constraint form the container, we expect a cout |
||
132 |
// message here |
||
133 |
✓✗ | 6 |
CaptureIOStream capture_ios; |
134 |
✓✗ | 3 |
capture_ios.beginCapture(); |
135 |
✓✗✓✗ |
3 |
model.removeConstraint("random_constraint"); |
136 |
✓✗ | 3 |
capture_ios.endCapture(); |
137 |
|||
138 |
// Test that the error message is sent. |
||
139 |
✓✗ | 6 |
std::stringstream expected_buffer; |
140 |
expected_buffer << "Warning: we couldn't remove the random_constraint " |
||
141 |
✓✗ | 3 |
"constraint item, it doesn't exist." |
142 |
✓✗ | 3 |
<< std::endl; |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(capture_ios.str() == expected_buffer.str()); |
144 |
3 |
} |
|
145 |
|||
146 |
3 |
void test_calc(StateModelTypes::Type state_type) { |
|
147 |
// setup the test |
||
148 |
✓✗ | 6 |
StateModelFactory state_factory; |
149 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
150 |
// create the corresponding data object |
||
151 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
152 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
153 |
3 |
pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); |
|
154 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(pinocchio_model); |
155 |
✓✗ | 6 |
crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data); |
156 |
|||
157 |
// create and add some constraint objects |
||
158 |
6 |
std::vector<boost::shared_ptr<crocoddyl::ConstraintModelAbstract> > models; |
|
159 |
6 |
std::vector<boost::shared_ptr<crocoddyl::ConstraintDataAbstract> > datas; |
|
160 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
161 |
✓✗ | 30 |
std::ostringstream os; |
162 |
✓✗✓✗ |
15 |
os << "random_constraint_" << i; |
163 |
const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& m = |
||
164 |
✓✗ | 15 |
create_random_constraint(state_type); |
165 |
✓✗✓✗ |
15 |
model.addConstraint(os.str(), m, 1.); |
166 |
✓✗ | 15 |
models.push_back(m); |
167 |
✓✗✓✗ |
15 |
datas.push_back(m->createData(&shared_data)); |
168 |
} |
||
169 |
|||
170 |
// create the data of the constraint sum |
||
171 |
const boost::shared_ptr<crocoddyl::ConstraintDataManager>& data = |
||
172 |
✓✗ | 6 |
model.createData(&shared_data); |
173 |
|||
174 |
// compute the constraint sum data for the case when all constraints are |
||
175 |
// defined as active |
||
176 |
✓✗ | 6 |
const Eigen::VectorXd& x1 = state->rand(); |
177 |
✓✗✓✗ |
6 |
const Eigen::VectorXd& u1 = Eigen::VectorXd::Random(model.get_nu()); |
178 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
179 |
x1); |
||
180 |
✓✗✓✗ ✓✗ |
3 |
model.calc(data, x1, u1); |
181 |
|||
182 |
// check the constraint against single constraint computations |
||
183 |
3 |
std::size_t ng_i = 0; |
|
184 |
3 |
std::size_t nh_i = 0; |
|
185 |
✓✗✓✗ |
6 |
Eigen::VectorXd g = Eigen::VectorXd::Zero(model.get_ng()); |
186 |
✓✗✓✗ |
6 |
Eigen::VectorXd h = Eigen::VectorXd::Zero(model.get_nh()); |
187 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
188 |
✓✗✓✗ ✓✗ |
15 |
models[i]->calc(datas[i], x1, u1); |
189 |
15 |
const std::size_t ng = models[i]->get_ng(); |
|
190 |
15 |
const std::size_t nh = models[i]->get_nh(); |
|
191 |
✓✗✓✗ |
15 |
g.segment(ng_i, ng) = datas[i]->g; |
192 |
✓✗✓✗ |
15 |
h.segment(nh_i, nh) = datas[i]->h; |
193 |
15 |
ng_i += ng; |
|
194 |
15 |
nh_i += nh; |
|
195 |
} |
||
196 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->g.isApprox(g, 1e-9)); |
197 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->h.isApprox(h, 1e-9)); |
198 |
3 |
} |
|
199 |
|||
200 |
3 |
void test_calcDiff(StateModelTypes::Type state_type) { |
|
201 |
// setup the test |
||
202 |
✓✗ | 6 |
StateModelFactory state_factory; |
203 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
204 |
// create the corresponding data object |
||
205 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
206 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
207 |
3 |
pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); |
|
208 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(pinocchio_model); |
209 |
✓✗ | 6 |
crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data); |
210 |
|||
211 |
// create and add some constraint objects |
||
212 |
6 |
std::vector<boost::shared_ptr<crocoddyl::ConstraintModelAbstract> > models; |
|
213 |
6 |
std::vector<boost::shared_ptr<crocoddyl::ConstraintDataAbstract> > datas; |
|
214 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
215 |
✓✗ | 30 |
std::ostringstream os; |
216 |
✓✗✓✗ |
15 |
os << "random_constraint_" << i; |
217 |
const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& m = |
||
218 |
✓✗ | 15 |
create_random_constraint(state_type); |
219 |
✓✗✓✗ |
15 |
model.addConstraint(os.str(), m, 1.); |
220 |
✓✗ | 15 |
models.push_back(m); |
221 |
✓✗✓✗ |
15 |
datas.push_back(m->createData(&shared_data)); |
222 |
} |
||
223 |
|||
224 |
// create the data of the constraint sum |
||
225 |
const boost::shared_ptr<crocoddyl::ConstraintDataManager>& data = |
||
226 |
✓✗ | 6 |
model.createData(&shared_data); |
227 |
|||
228 |
// compute the constraint sum data for the case when all constraints are |
||
229 |
// defined as active |
||
230 |
✓✗ | 6 |
Eigen::VectorXd x1 = state->rand(); |
231 |
✓✗✓✗ |
6 |
const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu()); |
232 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
233 |
x1); |
||
234 |
✓✗✓✗ ✓✗ |
3 |
model.calc(data, x1, u1); |
235 |
✓✗✓✗ ✓✗ |
3 |
model.calcDiff(data, x1, u1); |
236 |
|||
237 |
// check the constraint against single constraint computations |
||
238 |
3 |
std::size_t ng_i = 0; |
|
239 |
3 |
std::size_t nh_i = 0; |
|
240 |
3 |
const std::size_t ndx = state->get_ndx(); |
|
241 |
3 |
const std::size_t nu = model.get_nu(); |
|
242 |
✓✗✓✗ |
6 |
Eigen::VectorXd g = Eigen::VectorXd::Zero(model.get_ng()); |
243 |
✓✗✓✗ |
6 |
Eigen::VectorXd h = Eigen::VectorXd::Zero(model.get_nh()); |
244 |
✓✗✓✗ |
6 |
Eigen::MatrixXd Gx = Eigen::MatrixXd::Zero(model.get_ng(), ndx); |
245 |
✓✗✓✗ |
6 |
Eigen::MatrixXd Gu = Eigen::MatrixXd::Zero(model.get_ng(), nu); |
246 |
✓✗✓✗ |
6 |
Eigen::MatrixXd Hx = Eigen::MatrixXd::Zero(model.get_nh(), ndx); |
247 |
✓✗✓✗ |
6 |
Eigen::MatrixXd Hu = Eigen::MatrixXd::Zero(model.get_nh(), nu); |
248 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
249 |
✓✗✓✗ ✓✗ |
15 |
models[i]->calc(datas[i], x1, u1); |
250 |
✓✗✓✗ ✓✗ |
15 |
models[i]->calcDiff(datas[i], x1, u1); |
251 |
15 |
const std::size_t ng = models[i]->get_ng(); |
|
252 |
15 |
const std::size_t nh = models[i]->get_nh(); |
|
253 |
✓✗✓✗ |
15 |
g.segment(ng_i, ng) = datas[i]->g; |
254 |
✓✗✓✗ |
15 |
h.segment(nh_i, nh) = datas[i]->h; |
255 |
✓✗✓✗ |
15 |
Gx.block(ng_i, 0, ng, ndx) = datas[i]->Gx; |
256 |
✓✗✓✗ |
15 |
Gu.block(ng_i, 0, ng, nu) = datas[i]->Gu; |
257 |
✓✗✓✗ |
15 |
Hx.block(nh_i, 0, nh, ndx) = datas[i]->Hx; |
258 |
✓✗✓✗ |
15 |
Hu.block(nh_i, 0, nh, nu) = datas[i]->Hu; |
259 |
15 |
ng_i += ng; |
|
260 |
15 |
nh_i += nh; |
|
261 |
} |
||
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->g.isApprox(g, 1e-9)); |
263 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->h.isApprox(h, 1e-9)); |
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Gx.isApprox(Gx, 1e-9)); |
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Gu.isApprox(Gu, 1e-9)); |
266 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Hx.isApprox(Hx, 1e-9)); |
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Hu.isApprox(Hu, 1e-9)); |
268 |
|||
269 |
✓✗ | 3 |
x1 = state->rand(); |
270 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
271 |
x1); |
||
272 |
✓✗✓✗ |
3 |
model.calc(data, x1); |
273 |
✓✗✓✗ |
3 |
model.calcDiff(data, x1); |
274 |
|||
275 |
3 |
ng_i = 0; |
|
276 |
3 |
nh_i = 0; |
|
277 |
✓✗ | 3 |
g.setZero(); |
278 |
✓✗ | 3 |
h.setZero(); |
279 |
✓✗ | 3 |
Gx.setZero(); |
280 |
✓✗ | 3 |
Gu.setZero(); |
281 |
✓✗ | 3 |
Hx.setZero(); |
282 |
✓✗ | 3 |
Hu.setZero(); |
283 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
284 |
✓✗✓✗ |
15 |
models[i]->calc(datas[i], x1); |
285 |
✓✗✓✗ |
15 |
models[i]->calcDiff(datas[i], x1); |
286 |
15 |
const std::size_t ng = models[i]->get_ng(); |
|
287 |
15 |
const std::size_t nh = models[i]->get_nh(); |
|
288 |
✓✗✓✗ |
15 |
g.segment(ng_i, ng) = datas[i]->g; |
289 |
✓✗✓✗ |
15 |
h.segment(nh_i, nh) = datas[i]->h; |
290 |
✓✗✓✗ |
15 |
Gx.block(ng_i, 0, ng, ndx) = datas[i]->Gx; |
291 |
✓✗✓✗ |
15 |
Gu.block(ng_i, 0, ng, nu) = datas[i]->Gu; |
292 |
✓✗✓✗ |
15 |
Hx.block(nh_i, 0, nh, ndx) = datas[i]->Hx; |
293 |
✓✗✓✗ |
15 |
Hu.block(nh_i, 0, nh, nu) = datas[i]->Hu; |
294 |
15 |
ng_i += ng; |
|
295 |
15 |
nh_i += nh; |
|
296 |
} |
||
297 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->g.isApprox(g, 1e-9)); |
298 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->h.isApprox(h, 1e-9)); |
299 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Gx.isApprox(Gx, 1e-9)); |
300 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Hx.isApprox(Hx, 1e-9)); |
301 |
3 |
} |
|
302 |
|||
303 |
3 |
void test_get_constraints(StateModelTypes::Type state_type) { |
|
304 |
// setup the test |
||
305 |
✓✗ | 6 |
StateModelFactory state_factory; |
306 |
✓✗✓✗ |
6 |
crocoddyl::ConstraintModelManager model(state_factory.create(state_type)); |
307 |
// create the corresponding data object |
||
308 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
309 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
310 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(*state->get_pinocchio().get()); |
311 |
|||
312 |
// create and add some contact objects |
||
313 |
✓✓ | 18 |
for (unsigned i = 0; i < 5; ++i) { |
314 |
✓✗ | 15 |
std::ostringstream os; |
315 |
✓✗✓✗ |
15 |
os << "random_constraint_" << i; |
316 |
✓✗✓✗ ✓✗ |
15 |
model.addConstraint(os.str(), create_random_constraint(state_type), 1.); |
317 |
} |
||
318 |
|||
319 |
// get the contacts |
||
320 |
const crocoddyl::ConstraintModelManager::ConstraintModelContainer& |
||
321 |
3 |
constraints = model.get_constraints(); |
|
322 |
|||
323 |
// test |
||
324 |
crocoddyl::ConstraintModelManager::ConstraintModelContainer::const_iterator |
||
325 |
3 |
it_m, |
|
326 |
3 |
end_m; |
|
327 |
unsigned i; |
||
328 |
18 |
for (i = 0, it_m = constraints.begin(), end_m = constraints.end(); |
|
329 |
✓✓ | 33 |
it_m != end_m; ++it_m, ++i) { |
330 |
✓✗ | 30 |
std::ostringstream os; |
331 |
✓✗✓✗ |
15 |
os << "random_constraint_" << i; |
332 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
15 |
BOOST_CHECK(it_m->first == os.str()); |
333 |
} |
||
334 |
3 |
} |
|
335 |
|||
336 |
3 |
void test_shareMemory(StateModelTypes::Type state_type) { |
|
337 |
// setup the test |
||
338 |
✓✗ | 6 |
StateModelFactory state_factory; |
339 |
const boost::shared_ptr<crocoddyl::StateAbstract> state = |
||
340 |
✓✗ | 6 |
state_factory.create(state_type); |
341 |
✓✗ | 6 |
crocoddyl::ConstraintModelManager constraint_model(state); |
342 |
6 |
crocoddyl::DataCollectorAbstract shared_data; |
|
343 |
const boost::shared_ptr<crocoddyl::ConstraintDataManager>& constraint_data = |
||
344 |
✓✗ | 6 |
constraint_model.createData(&shared_data); |
345 |
|||
346 |
3 |
std::size_t ng = state->get_ndx(); |
|
347 |
3 |
std::size_t nh = state->get_ndx(); |
|
348 |
3 |
const std::size_t ndx = state->get_ndx(); |
|
349 |
3 |
const std::size_t nu = constraint_model.get_nu(); |
|
350 |
✓✗ | 6 |
crocoddyl::ActionModelLQR action_model(ndx, nu); |
351 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& action_data = |
||
352 |
✓✗ | 6 |
action_model.createData(); |
353 |
|||
354 |
✓✗ | 3 |
action_data->h.resize(nh); |
355 |
✓✗ | 3 |
action_data->g.resize(ng); |
356 |
✓✗ | 3 |
action_data->Gx.resize(ng, ndx); |
357 |
✓✗ | 3 |
action_data->Gu.resize(ng, nu); |
358 |
✓✗ | 3 |
action_data->Hx.resize(nh, ndx); |
359 |
✓✗ | 3 |
action_data->Hu.resize(nh, nu); |
360 |
✓✗ | 3 |
constraint_data->shareMemory(action_data.get()); |
361 |
✓✗✓✗ |
3 |
constraint_data->h = Eigen::VectorXd::Random(nh); |
362 |
✓✗✓✗ |
3 |
constraint_data->g = Eigen::VectorXd::Random(ng); |
363 |
✓✗✓✗ |
3 |
constraint_data->Gx = Eigen::MatrixXd::Random(ng, ndx); |
364 |
✓✗✓✗ |
3 |
constraint_data->Gu = Eigen::MatrixXd::Random(ng, nu); |
365 |
✓✗✓✗ |
3 |
constraint_data->Hx = Eigen::MatrixXd::Random(nh, ndx); |
366 |
✓✗✓✗ |
3 |
constraint_data->Hu = Eigen::MatrixXd::Random(nh, nu); |
367 |
|||
368 |
// check that the data has been shared |
||
369 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->g.isApprox(constraint_data->g, 1e-9)); |
370 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->h.isApprox(constraint_data->h, 1e-9)); |
371 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Gx.isApprox(constraint_data->Gx, 1e-9)); |
372 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Gu.isApprox(constraint_data->Gu, 1e-9)); |
373 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Hx.isApprox(constraint_data->Hx, 1e-9)); |
374 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Hu.isApprox(constraint_data->Hu, 1e-9)); |
375 |
|||
376 |
// let's now resize the data |
||
377 |
✓✗ | 3 |
constraint_data->resize(&action_model, action_data.get()); |
378 |
|||
379 |
// check that the shared data has been resized |
||
380 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->g.isApprox(constraint_data->g, 1e-9)); |
381 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->h.isApprox(constraint_data->h, 1e-9)); |
382 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Gx.isApprox(constraint_data->Gx, 1e-9)); |
383 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Gu.isApprox(constraint_data->Gu, 1e-9)); |
384 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Hx.isApprox(constraint_data->Hx, 1e-9)); |
385 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Hu.isApprox(constraint_data->Hu, 1e-9)); |
386 |
3 |
} |
|
387 |
|||
388 |
//----------------------------------------------------------------------------// |
||
389 |
|||
390 |
3 |
void register_unit_tests(StateModelTypes::Type state_type) { |
|
391 |
✓✗✓✗ |
6 |
boost::test_tools::output_test_stream test_name; |
392 |
test_name << "test_ConstraintModelManager" |
||
393 |
✓✗✓✗ ✓✗ |
3 |
<< "_" << state_type; |
394 |
✓✗✓✗ ✓✗✓✗ |
3 |
std::cout << "Running " << test_name.str() << std::endl; |
395 |
✓✗✓✗ ✓✗✓✗ |
3 |
test_suite* ts = BOOST_TEST_SUITE(test_name.str()); |
396 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_constructor, state_type))); |
397 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_addConstraint, state_type))); |
398 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE( |
399 |
boost::bind(&test_addConstraint_error_message, state_type))); |
||
400 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_removeConstraint, state_type))); |
401 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE( |
402 |
boost::bind(&test_removeConstraint_error_message, state_type))); |
||
403 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, state_type))); |
404 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, state_type))); |
405 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_get_constraints, state_type))); |
406 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_shareMemory, state_type))); |
407 |
✓✗✓✗ ✓✗ |
3 |
framework::master_test_suite().add(ts); |
408 |
3 |
} |
|
409 |
|||
410 |
1 |
bool init_function() { |
|
411 |
1 |
register_unit_tests(StateModelTypes::StateMultibody_TalosArm); |
|
412 |
1 |
register_unit_tests(StateModelTypes::StateMultibody_HyQ); |
|
413 |
1 |
register_unit_tests(StateModelTypes::StateMultibody_Talos); |
|
414 |
1 |
return true; |
|
415 |
} |
||
416 |
|||
417 |
1 |
int main(int argc, char** argv) { |
|
418 |
1 |
return ::boost::unit_test::unit_test_main(&init_function, argc, argv); |
|
419 |
} |
| Generated by: GCOVR (Version 4.2) |