| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-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/cost.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::CostModelSum 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_costs().size() == 0); |
33 |
3 |
} |
|
34 |
|||
35 |
3 |
void test_addCost(StateModelTypes::Type state_type) { |
|
36 |
// Setup the test |
||
37 |
✓✗ | 6 |
StateModelFactory state_factory; |
38 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
39 |
|||
40 |
// add an active cost |
||
41 |
boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_1 = |
||
42 |
✓✗ | 6 |
create_random_cost(state_type); |
43 |
✓✗✓✗ |
3 |
model.addCost("random_cost_1", rand_cost_1, 1.); |
44 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr()); |
45 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr_total() == rand_cost_1->get_activation()->get_nr()); |
46 |
|||
47 |
// add an inactive cost |
||
48 |
boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost_2 = |
||
49 |
✓✗ | 6 |
create_random_cost(state_type); |
50 |
✓✗✓✗ |
3 |
model.addCost("random_cost_2", rand_cost_2, 1., false); |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr()); |
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr_total() == |
53 |
rand_cost_1->get_activation()->get_nr() + |
||
54 |
rand_cost_2->get_activation()->get_nr()); |
||
55 |
|||
56 |
// change the random cost 2 status |
||
57 |
✓✗✓✗ |
3 |
model.changeCostStatus("random_cost_2", true); |
58 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr() == rand_cost_1->get_activation()->get_nr() + |
59 |
rand_cost_2->get_activation()->get_nr()); |
||
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr_total() == |
61 |
rand_cost_1->get_activation()->get_nr() + |
||
62 |
rand_cost_2->get_activation()->get_nr()); |
||
63 |
|||
64 |
// change the random cost 1 status |
||
65 |
✓✗✓✗ |
3 |
model.changeCostStatus("random_cost_1", false); |
66 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr() == rand_cost_2->get_activation()->get_nr()); |
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr_total() == |
68 |
rand_cost_1->get_activation()->get_nr() + |
||
69 |
rand_cost_2->get_activation()->get_nr()); |
||
70 |
3 |
} |
|
71 |
|||
72 |
3 |
void test_addCost_error_message(StateModelTypes::Type state_type) { |
|
73 |
// Setup the test |
||
74 |
✓✗ | 6 |
StateModelFactory state_factory; |
75 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
76 |
|||
77 |
// create an cost object |
||
78 |
boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost = |
||
79 |
✓✗ | 6 |
create_random_cost(state_type); |
80 |
|||
81 |
// add twice the same cost object to the container |
||
82 |
✓✗✓✗ |
3 |
model.addCost("random_cost", rand_cost, 1.); |
83 |
|||
84 |
// test error message when we add a duplicate cost |
||
85 |
✓✗ | 6 |
CaptureIOStream capture_ios; |
86 |
✓✗ | 3 |
capture_ios.beginCapture(); |
87 |
✓✗✓✗ |
3 |
model.addCost("random_cost", rand_cost, 1.); |
88 |
✓✗ | 3 |
capture_ios.endCapture(); |
89 |
✓✗ | 6 |
std::stringstream expected_buffer; |
90 |
expected_buffer << "Warning: we couldn't add the random_cost cost item, it " |
||
91 |
✓✗ | 3 |
"already existed." |
92 |
✓✗ | 3 |
<< std::endl; |
93 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(capture_ios.str() == expected_buffer.str()); |
94 |
|||
95 |
// test error message when we change the cost status of an inexistent cost |
||
96 |
✓✗ | 3 |
capture_ios.beginCapture(); |
97 |
✓✗✓✗ |
3 |
model.changeCostStatus("no_exist_cost", 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_cost cost 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_removeCost(StateModelTypes::Type state_type) { |
|
107 |
// Setup the test |
||
108 |
✓✗ | 6 |
StateModelFactory state_factory; |
109 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
110 |
|||
111 |
// add an active cost |
||
112 |
boost::shared_ptr<crocoddyl::CostModelAbstract> rand_cost = |
||
113 |
✓✗ | 6 |
create_random_cost(state_type); |
114 |
✓✗✓✗ |
3 |
model.addCost("random_cost", rand_cost, 1.); |
115 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr() == rand_cost->get_activation()->get_nr()); |
116 |
|||
117 |
// remove the cost |
||
118 |
✓✗✓✗ |
3 |
model.removeCost("random_cost"); |
119 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr() == 0); |
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(model.get_nr_total() == 0); |
121 |
3 |
} |
|
122 |
|||
123 |
3 |
void test_removeCost_error_message(StateModelTypes::Type state_type) { |
|
124 |
// Setup the test |
||
125 |
✓✗ | 6 |
StateModelFactory state_factory; |
126 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
127 |
|||
128 |
// remove a none existing cost form the container, we expect a cout message |
||
129 |
// here |
||
130 |
✓✗ | 6 |
CaptureIOStream capture_ios; |
131 |
✓✗ | 3 |
capture_ios.beginCapture(); |
132 |
✓✗✓✗ |
3 |
model.removeCost("random_cost"); |
133 |
✓✗ | 3 |
capture_ios.endCapture(); |
134 |
|||
135 |
// Test that the error message is sent. |
||
136 |
✓✗ | 6 |
std::stringstream expected_buffer; |
137 |
expected_buffer << "Warning: we couldn't remove the random_cost cost item, " |
||
138 |
✓✗ | 3 |
"it doesn't exist." |
139 |
✓✗ | 3 |
<< std::endl; |
140 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(capture_ios.str() == expected_buffer.str()); |
141 |
3 |
} |
|
142 |
|||
143 |
3 |
void test_calc(StateModelTypes::Type state_type) { |
|
144 |
// setup the test |
||
145 |
✓✗ | 6 |
StateModelFactory state_factory; |
146 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
147 |
// create the corresponding data object |
||
148 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
149 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
150 |
3 |
pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); |
|
151 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(pinocchio_model); |
152 |
✓✗ | 6 |
crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data); |
153 |
|||
154 |
// create and add some cost objects |
||
155 |
6 |
std::vector<boost::shared_ptr<crocoddyl::CostModelAbstract> > models; |
|
156 |
6 |
std::vector<boost::shared_ptr<crocoddyl::CostDataAbstract> > datas; |
|
157 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
158 |
✓✗ | 30 |
std::ostringstream os; |
159 |
✓✗✓✗ |
15 |
os << "random_cost_" << i; |
160 |
const boost::shared_ptr<crocoddyl::CostModelAbstract>& m = |
||
161 |
✓✗ | 15 |
create_random_cost(state_type); |
162 |
✓✗✓✗ |
15 |
model.addCost(os.str(), m, 1.); |
163 |
✓✗ | 15 |
models.push_back(m); |
164 |
✓✗✓✗ |
15 |
datas.push_back(m->createData(&shared_data)); |
165 |
} |
||
166 |
|||
167 |
// create the data of the cost sum |
||
168 |
const boost::shared_ptr<crocoddyl::CostDataSum>& data = |
||
169 |
✓✗ | 6 |
model.createData(&shared_data); |
170 |
|||
171 |
// compute the cost sum data for the case when all costs are defined as active |
||
172 |
✓✗ | 6 |
const Eigen::VectorXd x1 = state->rand(); |
173 |
✓✗✓✗ |
6 |
const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu()); |
174 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
175 |
x1); |
||
176 |
✓✗✓✗ ✓✗ |
3 |
model.calc(data, x1, u1); |
177 |
|||
178 |
// check that the cost has been filled |
||
179 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost > 0.); |
180 |
|||
181 |
// check the cost against single cost computations |
||
182 |
3 |
double cost = 0; |
|
183 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
184 |
✓✗✓✗ ✓✗ |
15 |
models[i]->calc(datas[i], x1, u1); |
185 |
15 |
cost += datas[i]->cost; |
|
186 |
} |
||
187 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost == cost); |
188 |
|||
189 |
// compute the cost sum data for the case when the first three costs are |
||
190 |
// defined as active |
||
191 |
✓✗✓✗ |
3 |
model.changeCostStatus("random_cost_3", false); |
192 |
✓✗✓✗ |
3 |
model.changeCostStatus("random_cost_4", false); |
193 |
✓✗ | 6 |
const Eigen::VectorXd x2 = state->rand(); |
194 |
✓✗✓✗ |
6 |
const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu()); |
195 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
196 |
x1); |
||
197 |
✓✗✓✗ ✓✗ |
3 |
model.calc(data, x2, u2); |
198 |
3 |
cost = 0; |
|
199 |
✓✓ | 12 |
for (std::size_t i = 0; i < 3; |
200 |
++i) { // we need to update data because this costs are active |
||
201 |
✓✗✓✗ ✓✗ |
9 |
models[i]->calc(datas[i], x2, u2); |
202 |
9 |
cost += datas[i]->cost; |
|
203 |
} |
||
204 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost == cost); |
205 |
3 |
} |
|
206 |
|||
207 |
3 |
void test_calcDiff(StateModelTypes::Type state_type) { |
|
208 |
// setup the test |
||
209 |
✓✗ | 6 |
StateModelFactory state_factory; |
210 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
211 |
// create the corresponding data object |
||
212 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
213 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
214 |
3 |
pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); |
|
215 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(pinocchio_model); |
216 |
✓✗ | 6 |
crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data); |
217 |
|||
218 |
// create and add some cost objects |
||
219 |
6 |
std::vector<boost::shared_ptr<crocoddyl::CostModelAbstract> > models; |
|
220 |
6 |
std::vector<boost::shared_ptr<crocoddyl::CostDataAbstract> > datas; |
|
221 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
222 |
✓✗ | 30 |
std::ostringstream os; |
223 |
✓✗✓✗ |
15 |
os << "random_cost_" << i; |
224 |
const boost::shared_ptr<crocoddyl::CostModelAbstract>& m = |
||
225 |
✓✗ | 15 |
create_random_cost(state_type); |
226 |
✓✗✓✗ |
15 |
model.addCost(os.str(), m, 1.); |
227 |
✓✗ | 15 |
models.push_back(m); |
228 |
✓✗✓✗ |
15 |
datas.push_back(m->createData(&shared_data)); |
229 |
} |
||
230 |
|||
231 |
// create the data of the cost sum |
||
232 |
const boost::shared_ptr<crocoddyl::CostDataSum>& data = |
||
233 |
✓✗ | 6 |
model.createData(&shared_data); |
234 |
|||
235 |
// compute the cost sum data for the case when all costs are defined as active |
||
236 |
✓✗ | 6 |
Eigen::VectorXd x1 = state->rand(); |
237 |
✓✗✓✗ |
6 |
const Eigen::VectorXd u1 = Eigen::VectorXd::Random(model.get_nu()); |
238 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
239 |
x1); |
||
240 |
✓✗✓✗ ✓✗ |
3 |
model.calc(data, x1, u1); |
241 |
✓✗✓✗ ✓✗ |
3 |
model.calcDiff(data, x1, u1); |
242 |
|||
243 |
// check that the cost has been filled |
||
244 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost > 0.); |
245 |
|||
246 |
// check the cost against single cost computations |
||
247 |
3 |
double cost = 0; |
|
248 |
✓✗✓✗ |
6 |
Eigen::VectorXd Lx = Eigen::VectorXd::Zero(state->get_ndx()); |
249 |
✓✗✓✗ |
6 |
Eigen::VectorXd Lu = Eigen::VectorXd::Zero(model.get_nu()); |
250 |
Eigen::MatrixXd Lxx = |
||
251 |
✓✗✓✗ |
6 |
Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()); |
252 |
✓✗✓✗ |
6 |
Eigen::MatrixXd Lxu = Eigen::MatrixXd::Zero(state->get_ndx(), model.get_nu()); |
253 |
✓✗✓✗ |
6 |
Eigen::MatrixXd Luu = Eigen::MatrixXd::Zero(model.get_nu(), model.get_nu()); |
254 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
255 |
✓✗✓✗ ✓✗ |
15 |
models[i]->calc(datas[i], x1, u1); |
256 |
✓✗✓✗ ✓✗ |
15 |
models[i]->calcDiff(datas[i], x1, u1); |
257 |
15 |
cost += datas[i]->cost; |
|
258 |
✓✗ | 15 |
Lx += datas[i]->Lx; |
259 |
✓✗ | 15 |
Lu += datas[i]->Lu; |
260 |
✓✗ | 15 |
Lxx += datas[i]->Lxx; |
261 |
✓✗ | 15 |
Lxu += datas[i]->Lxu; |
262 |
✓✗ | 15 |
Luu += datas[i]->Luu; |
263 |
} |
||
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost == cost); |
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lx == Lx); |
266 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lu == Lu); |
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lxx == Lxx); |
268 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lxu == Lxu); |
269 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Luu == Luu); |
270 |
|||
271 |
✓✗ | 3 |
x1 = state->rand(); |
272 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
273 |
x1); |
||
274 |
✓✗✓✗ |
3 |
model.calc(data, x1); |
275 |
✓✗✓✗ |
3 |
model.calcDiff(data, x1); |
276 |
3 |
cost = 0.; |
|
277 |
✓✗ | 3 |
Lx.setZero(); |
278 |
✓✗ | 3 |
Lxx.setZero(); |
279 |
✓✓ | 18 |
for (std::size_t i = 0; i < 5; ++i) { |
280 |
✓✗✓✗ |
15 |
models[i]->calc(datas[i], x1); |
281 |
✓✗✓✗ |
15 |
models[i]->calcDiff(datas[i], x1); |
282 |
15 |
cost += datas[i]->cost; |
|
283 |
✓✗ | 15 |
Lx += datas[i]->Lx; |
284 |
✓✗ | 15 |
Lxx += datas[i]->Lxx; |
285 |
} |
||
286 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost == cost); |
287 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lx == Lx); |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lxx == Lxx); |
289 |
|||
290 |
// compute the cost sum data for the case when the first three costs are |
||
291 |
// defined as active |
||
292 |
✓✗✓✗ |
3 |
model.changeCostStatus("random_cost_3", false); |
293 |
✓✗✓✗ |
3 |
model.changeCostStatus("random_cost_4", false); |
294 |
✓✗ | 6 |
Eigen::VectorXd x2 = state->rand(); |
295 |
✓✗✓✗ |
6 |
const Eigen::VectorXd u2 = Eigen::VectorXd::Random(model.get_nu()); |
296 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
297 |
x2); |
||
298 |
✓✗✓✗ ✓✗ |
3 |
model.calc(data, x2, u2); |
299 |
✓✗✓✗ ✓✗ |
3 |
model.calcDiff(data, x2, u2); |
300 |
3 |
cost = 0; |
|
301 |
✓✗ | 3 |
Lx.setZero(); |
302 |
✓✗ | 3 |
Lu.setZero(); |
303 |
✓✗ | 3 |
Lxx.setZero(); |
304 |
✓✗ | 3 |
Lxu.setZero(); |
305 |
✓✗ | 3 |
Luu.setZero(); |
306 |
✓✓ | 12 |
for (std::size_t i = 0; i < 3; |
307 |
++i) { // we need to update data because this costs are active |
||
308 |
✓✗✓✗ ✓✗ |
9 |
models[i]->calc(datas[i], x2, u2); |
309 |
✓✗✓✗ ✓✗ |
9 |
models[i]->calcDiff(datas[i], x2, u2); |
310 |
9 |
cost += datas[i]->cost; |
|
311 |
✓✗ | 9 |
Lx += datas[i]->Lx; |
312 |
✓✗ | 9 |
Lu += datas[i]->Lu; |
313 |
✓✗ | 9 |
Lxx += datas[i]->Lxx; |
314 |
✓✗ | 9 |
Lxu += datas[i]->Lxu; |
315 |
✓✗ | 9 |
Luu += datas[i]->Luu; |
316 |
} |
||
317 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost == cost); |
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lx == Lx); |
319 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lu == Lu); |
320 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lxx == Lxx); |
321 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lxu == Lxu); |
322 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Luu == Luu); |
323 |
|||
324 |
✓✗ | 3 |
x2 = state->rand(); |
325 |
✓✗✓✗ |
3 |
crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, |
326 |
x2); |
||
327 |
✓✗✓✗ |
3 |
model.calc(data, x2); |
328 |
✓✗✓✗ |
3 |
model.calcDiff(data, x2); |
329 |
3 |
cost = 0.; |
|
330 |
✓✗ | 3 |
Lx.setZero(); |
331 |
✓✗ | 3 |
Lxx.setZero(); |
332 |
✓✓ | 12 |
for (std::size_t i = 0; i < 3; ++i) { |
333 |
✓✗✓✗ |
9 |
models[i]->calc(datas[i], x2); |
334 |
✓✗✓✗ |
9 |
models[i]->calcDiff(datas[i], x2); |
335 |
9 |
cost += datas[i]->cost; |
|
336 |
✓✗ | 9 |
Lx += datas[i]->Lx; |
337 |
✓✗ | 9 |
Lxx += datas[i]->Lxx; |
338 |
} |
||
339 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(data->cost == cost); |
340 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lx == Lx); |
341 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(data->Lxx == Lxx); |
342 |
3 |
} |
|
343 |
|||
344 |
3 |
void test_get_costs(StateModelTypes::Type state_type) { |
|
345 |
// setup the test |
||
346 |
✓✗ | 6 |
StateModelFactory state_factory; |
347 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
348 |
// create the corresponding data object |
||
349 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
350 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
351 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(*state->get_pinocchio().get()); |
352 |
|||
353 |
// create and add some contact objects |
||
354 |
✓✓ | 18 |
for (unsigned i = 0; i < 5; ++i) { |
355 |
✓✗ | 15 |
std::ostringstream os; |
356 |
✓✗✓✗ |
15 |
os << "random_cost_" << i; |
357 |
✓✗✓✗ ✓✗ |
15 |
model.addCost(os.str(), create_random_cost(state_type), 1.); |
358 |
} |
||
359 |
|||
360 |
// get the contacts |
||
361 |
3 |
const crocoddyl::CostModelSum::CostModelContainer& costs = model.get_costs(); |
|
362 |
|||
363 |
// test |
||
364 |
3 |
crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m; |
|
365 |
unsigned i; |
||
366 |
✓✓ | 18 |
for (i = 0, it_m = costs.begin(), end_m = costs.end(); it_m != end_m; |
367 |
15 |
++it_m, ++i) { |
|
368 |
✓✗ | 30 |
std::ostringstream os; |
369 |
✓✗✓✗ |
15 |
os << "random_cost_" << i; |
370 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
15 |
BOOST_CHECK(it_m->first == os.str()); |
371 |
} |
||
372 |
3 |
} |
|
373 |
|||
374 |
3 |
void test_get_nr(StateModelTypes::Type state_type) { |
|
375 |
// Setup the test |
||
376 |
✓✗ | 6 |
StateModelFactory state_factory; |
377 |
✓✗✓✗ |
6 |
crocoddyl::CostModelSum model(state_factory.create(state_type)); |
378 |
|||
379 |
// create the corresponding data object |
||
380 |
const boost::shared_ptr<crocoddyl::StateMultibody>& state = |
||
381 |
6 |
boost::static_pointer_cast<crocoddyl::StateMultibody>(model.get_state()); |
|
382 |
✓✗ | 6 |
pinocchio::Data pinocchio_data(*state->get_pinocchio().get()); |
383 |
|||
384 |
// create and add some contact objects |
||
385 |
✓✓ | 18 |
for (unsigned i = 0; i < 5; ++i) { |
386 |
✓✗ | 15 |
std::ostringstream os; |
387 |
✓✗✓✗ |
15 |
os << "random_cost_" << i; |
388 |
✓✗✓✗ ✓✗ |
15 |
model.addCost(os.str(), create_random_cost(state_type), 1.); |
389 |
} |
||
390 |
|||
391 |
// compute ni |
||
392 |
3 |
std::size_t nr = 0; |
|
393 |
3 |
crocoddyl::CostModelSum::CostModelContainer::const_iterator it_m, end_m; |
|
394 |
18 |
for (it_m = model.get_costs().begin(), end_m = model.get_costs().end(); |
|
395 |
✓✓ | 18 |
it_m != end_m; ++it_m) { |
396 |
15 |
nr += it_m->second->cost->get_activation()->get_nr(); |
|
397 |
} |
||
398 |
|||
399 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
3 |
BOOST_CHECK(nr == model.get_nr()); |
400 |
3 |
} |
|
401 |
|||
402 |
3 |
void test_shareMemory(StateModelTypes::Type state_type) { |
|
403 |
// setup the test |
||
404 |
✓✗ | 6 |
StateModelFactory state_factory; |
405 |
const boost::shared_ptr<crocoddyl::StateAbstract> state = |
||
406 |
✓✗ | 6 |
state_factory.create(state_type); |
407 |
✓✗ | 6 |
crocoddyl::CostModelSum cost_model(state); |
408 |
6 |
crocoddyl::DataCollectorAbstract shared_data; |
|
409 |
const boost::shared_ptr<crocoddyl::CostDataSum>& cost_data = |
||
410 |
✓✗ | 6 |
cost_model.createData(&shared_data); |
411 |
|||
412 |
3 |
const std::size_t ndx = state->get_ndx(); |
|
413 |
3 |
const std::size_t nu = cost_model.get_nu(); |
|
414 |
✓✗ | 6 |
crocoddyl::ActionModelLQR action_model(ndx, nu); |
415 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& action_data = |
||
416 |
✓✗ | 6 |
action_model.createData(); |
417 |
|||
418 |
✓✗ | 3 |
cost_data->shareMemory(action_data.get()); |
419 |
✓✗✓✗ |
3 |
cost_data->Lx = Eigen::VectorXd::Random(ndx); |
420 |
✓✗✓✗ |
3 |
cost_data->Lu = Eigen::VectorXd::Random(nu); |
421 |
✓✗✓✗ |
3 |
cost_data->Lxx = Eigen::MatrixXd::Random(ndx, ndx); |
422 |
✓✗✓✗ |
3 |
cost_data->Luu = Eigen::MatrixXd::Random(nu, nu); |
423 |
✓✗✓✗ |
3 |
cost_data->Lxu = Eigen::MatrixXd::Random(ndx, nu); |
424 |
|||
425 |
// check that the data has been shared |
||
426 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Lx.isApprox(cost_data->Lx, 1e-9)); |
427 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Lu.isApprox(cost_data->Lu, 1e-9)); |
428 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Lxx.isApprox(cost_data->Lxx, 1e-9)); |
429 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Luu.isApprox(cost_data->Luu, 1e-9)); |
430 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
3 |
BOOST_CHECK(action_data->Lxu.isApprox(cost_data->Lxu, 1e-9)); |
431 |
3 |
} |
|
432 |
|||
433 |
//----------------------------------------------------------------------------// |
||
434 |
|||
435 |
3 |
void register_unit_tests(StateModelTypes::Type state_type) { |
|
436 |
✓✗✓✗ |
6 |
boost::test_tools::output_test_stream test_name; |
437 |
test_name << "test_CostModelSum" |
||
438 |
✓✗✓✗ ✓✗ |
3 |
<< "_" << state_type; |
439 |
✓✗✓✗ ✓✗✓✗ |
3 |
std::cout << "Running " << test_name.str() << std::endl; |
440 |
✓✗✓✗ ✓✗✓✗ |
3 |
test_suite* ts = BOOST_TEST_SUITE(test_name.str()); |
441 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_constructor, state_type))); |
442 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_addCost, state_type))); |
443 |
3 |
ts->add( |
|
444 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
BOOST_TEST_CASE(boost::bind(&test_addCost_error_message, state_type))); |
445 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_removeCost, state_type))); |
446 |
3 |
ts->add( |
|
447 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
BOOST_TEST_CASE(boost::bind(&test_removeCost_error_message, state_type))); |
448 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, state_type))); |
449 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, state_type))); |
450 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_get_costs, state_type))); |
451 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_get_nr, state_type))); |
452 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
ts->add(BOOST_TEST_CASE(boost::bind(&test_shareMemory, state_type))); |
453 |
✓✗✓✗ ✓✗ |
3 |
framework::master_test_suite().add(ts); |
454 |
3 |
} |
|
455 |
|||
456 |
1 |
bool init_function() { |
|
457 |
1 |
register_unit_tests(StateModelTypes::StateMultibody_TalosArm); |
|
458 |
1 |
register_unit_tests(StateModelTypes::StateMultibody_HyQ); |
|
459 |
1 |
register_unit_tests(StateModelTypes::StateMultibody_Talos); |
|
460 |
1 |
return true; |
|
461 |
} |
||
462 |
|||
463 |
1 |
int main(int argc, char** argv) { |
|
464 |
1 |
return ::boost::unit_test::unit_test_main(&init_function, argc, argv); |
|
465 |
} |
| Generated by: GCOVR (Version 4.2) |