GCC Code Coverage Report

Directory: ./
File: unittest/test_problem.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 314 0.0%
Functions: 0 12 0.0%
Branches: 0 1774 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include "crocoddyl/core/integrator/euler.hpp"
13 #include "crocoddyl/core/optctrl/shooting.hpp"
14 #include "factory/action.hpp"
15 #include "factory/diff_action.hpp"
16 #include "factory/integrator.hpp"
17 #include "unittest_common.hpp"
18
19 using namespace boost::unit_test;
20 using namespace crocoddyl::unittest;
21
22 //----------------------------------------------------------------------------//
23
24 ✗ void test_calc(ActionModelTypes::Type action_model_type) {
25 // create the model
26 ✗ ActionModelFactory factory;
27 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
28 ✗ factory.create(action_model_type);
29
30 // create two shooting problems (with and without data allocation)
31 ✗ std::size_t T = 20;
32 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
33 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
34 ✗ std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
35 ✗ for (std::size_t i = 0; i < T; ++i) {
36 ✗ datas[i] = model->createData();
37 }
38 ✗ crocoddyl::ShootingProblem problem1(x0, models, model);
39 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
40 ✗ model->createData());
41
42 // Run the print function
43 ✗ std::ostringstream tmp;
44 ✗ tmp << problem1;
45
46 // create random trajectory
47 ✗ std::vector<Eigen::VectorXd> xs(T + 1);
48 ✗ std::vector<Eigen::VectorXd> us(T);
49 ✗ for (std::size_t i = 0; i < T; ++i) {
50 ✗ xs[i] = model->get_state()->rand();
51 ✗ us[i] = Eigen::VectorXd::Random(model->get_nu());
52 }
53 ✗ xs.back() = model->get_state()->rand();
54
55 // check the state and cost in each node
56 ✗ double cost = problem1.calc(xs, us);
57 ✗ problem2.calc(xs, us);
58 ✗ for (std::size_t i = 0; i < T; ++i) {
59 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
60 ✗ model->createData();
61 ✗ model->calc(data, xs[i], us[i]);
62 ✗ BOOST_CHECK(problem1.get_runningDatas()[i]->cost == data->cost);
63 ✗ BOOST_CHECK(problem2.get_runningDatas()[i]->cost == data->cost);
64 ✗ BOOST_CHECK(
65 (problem1.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
66 ✗ BOOST_CHECK(
67 (problem2.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
68 ✗ }
69 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
70 ✗ model->createData();
71 ✗ model->calc(data, xs.back());
72 ✗ BOOST_CHECK(problem1.get_terminalData()->cost == data->cost);
73 ✗ BOOST_CHECK(problem2.get_terminalData()->cost == data->cost);
74 ✗ BOOST_CHECK((problem1.get_terminalData()->xnext - data->xnext).isZero(1e-9));
75 ✗ BOOST_CHECK((problem2.get_terminalData()->xnext - data->xnext).isZero(1e-9));
76
77 // Checking that casted computation is the same
78 #ifdef NDEBUG // Run only in release mode
79 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
80 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
81 std::vector<Eigen::VectorXf> xs_f(T + 1);
82 std::vector<Eigen::VectorXf> us_f(T);
83 for (std::size_t i = 0; i < T; ++i) {
84 xs_f[i] = xs[i].cast<float>();
85 us_f[i] = us[i].cast<float>();
86 }
87 xs_f.back() = xs.back().cast<float>();
88 float cost_f = casted_problem1.calc(xs_f, us_f);
89 casted_problem2.calc(xs_f, us_f);
90 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
91 for (std::size_t i = 0; i < T; ++i) {
92 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
93 casted_model = model->cast<float>();
94 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
95 casted_data = casted_model->createData();
96 casted_model->calc(casted_data, xs_f[i], us_f[i]);
97 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->cost ==
98 casted_data->cost);
99 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->cost ==
100 casted_data->cost);
101 BOOST_CHECK(
102 (casted_problem1.get_runningDatas()[i]->xnext - casted_data->xnext)
103 .isZero(1e-9f));
104 BOOST_CHECK(
105 (casted_problem2.get_runningDatas()[i]->xnext - casted_data->xnext)
106 .isZero(1e-9f));
107 BOOST_CHECK(float(problem1.get_runningDatas()[i]->cost) -
108 casted_data->cost <=
109 tol_f);
110 BOOST_CHECK((problem1.get_runningDatas()[i]->xnext.cast<float>() -
111 casted_data->xnext)
112 .isZero(tol_f));
113 }
114 BOOST_CHECK(std::abs(float(cost) - cost_f) <= tol_f);
115 #endif
116 ✗ }
117
118 ✗ void test_calc_diffAction(DifferentialActionModelTypes::Type action_model_type,
119 IntegratorTypes::Type integrator_type) {
120 // create the model
121 ✗ DifferentialActionModelFactory factory;
122 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
123 ✗ factory.create(action_model_type);
124 ✗ IntegratorFactory factory_int;
125 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
126 ✗ factory_int.create(integrator_type, diffModel);
127
128 // create two shooting problems (with and without data allocation)
129 ✗ std::size_t T = 20;
130 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
131 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
132 ✗ std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
133 ✗ for (std::size_t i = 0; i < T; ++i) {
134 ✗ datas[i] = model->createData();
135 }
136 ✗ crocoddyl::ShootingProblem problem1(x0, models, model);
137 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
138 ✗ model->createData());
139
140 // create random trajectory
141 ✗ std::vector<Eigen::VectorXd> xs(T + 1);
142 ✗ std::vector<Eigen::VectorXd> us(T);
143 ✗ for (std::size_t i = 0; i < T; ++i) {
144 ✗ xs[i] = model->get_state()->rand();
145 ✗ us[i] = Eigen::VectorXd::Random(model->get_nu());
146 }
147 ✗ xs.back() = model->get_state()->rand();
148
149 // check the state and cost in each node
150 ✗ double cost = problem1.calc(xs, us);
151 ✗ problem2.calc(xs, us);
152 ✗ for (std::size_t i = 0; i < T; ++i) {
153 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
154 ✗ model->createData();
155 ✗ model->calc(data, xs[i], us[i]);
156 ✗ BOOST_CHECK(problem1.get_runningDatas()[i]->cost == data->cost);
157 ✗ BOOST_CHECK(problem2.get_runningDatas()[i]->cost == data->cost);
158 ✗ BOOST_CHECK(
159 (problem1.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
160 ✗ BOOST_CHECK(
161 (problem2.get_runningDatas()[i]->xnext - data->xnext).isZero(1e-9));
162 ✗ }
163 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
164 ✗ model->createData();
165 ✗ model->calc(data, xs.back());
166 ✗ BOOST_CHECK(problem1.get_terminalData()->cost == data->cost);
167 ✗ BOOST_CHECK(problem2.get_terminalData()->cost == data->cost);
168 ✗ BOOST_CHECK((problem1.get_terminalData()->xnext - data->xnext).isZero(1e-9));
169 ✗ BOOST_CHECK((problem2.get_terminalData()->xnext - data->xnext).isZero(1e-9));
170
171 // Checking that casted computation is the same
172 #ifdef NDEBUG // Run only in release mode
173 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
174 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
175 std::vector<Eigen::VectorXf> xs_f(T + 1);
176 std::vector<Eigen::VectorXf> us_f(T);
177 for (std::size_t i = 0; i < T; ++i) {
178 xs_f[i] = xs[i].cast<float>();
179 us_f[i] = us[i].cast<float>();
180 }
181 xs_f.back() = xs.back().cast<float>();
182 float cost_f = casted_problem1.calc(xs_f, us_f);
183 casted_problem2.calc(xs_f, us_f);
184 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
185 for (std::size_t i = 0; i < T; ++i) {
186 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
187 casted_model = model->cast<float>();
188 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
189 casted_data = casted_model->createData();
190 casted_model->calc(casted_data, xs_f[i], us_f[i]);
191 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->cost ==
192 casted_data->cost);
193 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->cost ==
194 casted_data->cost);
195 BOOST_CHECK(
196 (casted_problem1.get_runningDatas()[i]->xnext - casted_data->xnext)
197 .isZero(1e-9f));
198 BOOST_CHECK(
199 (casted_problem2.get_runningDatas()[i]->xnext - casted_data->xnext)
200 .isZero(1e-9f));
201 BOOST_CHECK(std::abs(float(problem1.get_runningDatas()[i]->cost) -
202 casted_data->cost) <= tol_f);
203 BOOST_CHECK((problem1.get_runningDatas()[i]->xnext.cast<float>() -
204 casted_data->xnext)
205 .isZero(tol_f));
206 }
207 BOOST_CHECK(std::abs(float(cost) - cost_f) <= tol_f);
208 #endif
209 ✗ }
210
211 ✗ void test_calcDiff(ActionModelTypes::Type action_model_type) {
212 // create the model
213 ✗ ActionModelFactory factory;
214 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
215 ✗ factory.create(action_model_type);
216
217 // create two shooting problems (with and without data allocation)
218 ✗ std::size_t T = 20;
219 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
220 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
221 ✗ std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
222 ✗ for (std::size_t i = 0; i < T; ++i) {
223 ✗ datas[i] = model->createData();
224 }
225 ✗ crocoddyl::ShootingProblem problem1(x0, models, model);
226 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
227 ✗ model->createData());
228
229 // create random trajectory
230 ✗ std::vector<Eigen::VectorXd> xs(T + 1);
231 ✗ std::vector<Eigen::VectorXd> us(T);
232 ✗ for (std::size_t i = 0; i < T; ++i) {
233 ✗ xs[i] = model->get_state()->rand();
234 ✗ us[i] = Eigen::VectorXd::Random(model->get_nu());
235 }
236 ✗ xs.back() = model->get_state()->rand();
237
238 // check the state and cost in each node
239 ✗ problem1.calc(xs, us);
240 ✗ problem2.calc(xs, us);
241 ✗ problem1.calcDiff(xs, us);
242 ✗ problem2.calcDiff(xs, us);
243 ✗ for (std::size_t i = 0; i < T; ++i) {
244 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
245 ✗ model->createData();
246 ✗ model->calc(data, xs[i], us[i]);
247 ✗ model->calcDiff(data, xs[i], us[i]);
248 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-9));
249 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-9));
250 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-9));
251 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-9));
252 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-9));
253 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-9));
254 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-9));
255 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-9));
256 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-9));
257 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-9));
258 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-9));
259 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-9));
260 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-9));
261 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-9));
262 ✗ }
263 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
264 ✗ model->createData();
265 ✗ model->calc(data, xs.back());
266 ✗ model->calcDiff(data, xs.back());
267 ✗ BOOST_CHECK((problem1.get_terminalData()->Fx - data->Fx).isZero(1e-9));
268 ✗ BOOST_CHECK((problem2.get_terminalData()->Fx - data->Fx).isZero(1e-9));
269 ✗ BOOST_CHECK((problem1.get_terminalData()->Lx - data->Lx).isZero(1e-9));
270 ✗ BOOST_CHECK((problem2.get_terminalData()->Lx - data->Lx).isZero(1e-9));
271 ✗ BOOST_CHECK((problem1.get_terminalData()->Lxx - data->Lxx).isZero(1e-9));
272 ✗ BOOST_CHECK((problem2.get_terminalData()->Lxx - data->Lxx).isZero(1e-9));
273
274 // Checking that casted computation is the same
275 #ifdef NDEBUG // Run only in release mode
276 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
277 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
278 std::vector<Eigen::VectorXf> xs_f(T + 1);
279 std::vector<Eigen::VectorXf> us_f(T);
280 for (std::size_t i = 0; i < T; ++i) {
281 xs_f[i] = xs[i].cast<float>();
282 us_f[i] = us[i].cast<float>();
283 }
284 xs_f.back() = xs.back().cast<float>();
285 casted_problem1.calc(xs_f, us_f);
286 casted_problem1.calcDiff(xs_f, us_f);
287 casted_problem2.calc(xs_f, us_f);
288 casted_problem2.calcDiff(xs_f, us_f);
289 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
290 for (std::size_t i = 0; i < T; ++i) {
291 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
292 casted_model = model->cast<float>();
293 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
294 casted_data = casted_model->createData();
295 casted_model->calc(casted_data, xs_f[i], us_f[i]);
296 casted_model->calcDiff(casted_data, xs_f[i], us_f[i]);
297 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fx == casted_data->Fx);
298 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fx == casted_data->Fx);
299 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fu == casted_data->Fu);
300 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fu == casted_data->Fu);
301 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lx == casted_data->Lx);
302 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lx == casted_data->Lx);
303 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lu == casted_data->Lu);
304 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lu == casted_data->Lu);
305 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxx == casted_data->Lxx);
306 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxx == casted_data->Lxx);
307 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxu == casted_data->Lxu);
308 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxu == casted_data->Lxu);
309 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Luu == casted_data->Luu);
310 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Luu == casted_data->Luu);
311 BOOST_CHECK(
312 (problem1.get_runningDatas()[i]->Fx.cast<float>() - casted_data->Fx)
313 .isZero(tol_f));
314 BOOST_CHECK(
315 (problem1.get_runningDatas()[i]->Fu.cast<float>() - casted_data->Fu)
316 .isZero(tol_f));
317 BOOST_CHECK(
318 (problem1.get_runningDatas()[i]->Lx.cast<float>() - casted_data->Lx)
319 .isZero(tol_f));
320 BOOST_CHECK(
321 (problem1.get_runningDatas()[i]->Lu.cast<float>() - casted_data->Lu)
322 .isZero(tol_f));
323 BOOST_CHECK(
324 (problem1.get_runningDatas()[i]->Lxx.cast<float>() - casted_data->Lxx)
325 .isZero(tol_f));
326 BOOST_CHECK(
327 (problem1.get_runningDatas()[i]->Lxu.cast<float>() - casted_data->Lxu)
328 .isZero(tol_f));
329 BOOST_CHECK(
330 (problem1.get_runningDatas()[i]->Luu.cast<float>() - casted_data->Luu)
331 .isZero(tol_f));
332 }
333 #endif
334 ✗ }
335
336 ✗ void test_calcDiff_diffAction(
337 DifferentialActionModelTypes::Type action_model_type,
338 IntegratorTypes::Type integrator_type) {
339 // create the model
340 ✗ DifferentialActionModelFactory factory;
341 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
342 ✗ factory.create(action_model_type);
343 ✗ IntegratorFactory factory_int;
344 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
345 ✗ factory_int.create(integrator_type, diffModel);
346
347 // create two shooting problems (with and without data allocation)
348 ✗ std::size_t T = 20;
349 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
350 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
351 ✗ std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
352 ✗ for (std::size_t i = 0; i < T; ++i) {
353 ✗ datas[i] = model->createData();
354 }
355 ✗ crocoddyl::ShootingProblem problem1(x0, models, model);
356 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
357 ✗ model->createData());
358
359 // create random trajectory
360 ✗ std::vector<Eigen::VectorXd> xs(T + 1);
361 ✗ std::vector<Eigen::VectorXd> us(T);
362 ✗ for (std::size_t i = 0; i < T; ++i) {
363 ✗ xs[i] = model->get_state()->rand();
364 ✗ us[i] = Eigen::VectorXd::Random(model->get_nu());
365 }
366 ✗ xs.back() = model->get_state()->rand();
367
368 // check the state and cost in each node
369 ✗ problem1.calc(xs, us);
370 ✗ problem2.calc(xs, us);
371 ✗ problem1.calcDiff(xs, us);
372 ✗ problem2.calcDiff(xs, us);
373 ✗ for (std::size_t i = 0; i < T; ++i) {
374 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
375 ✗ model->createData();
376 ✗ model->calc(data, xs[i], us[i]);
377 ✗ model->calcDiff(data, xs[i], us[i]);
378 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-7));
379 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Fx - data->Fx).isZero(1e-7));
380 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-7));
381 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Fu - data->Fu).isZero(1e-7));
382 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-7));
383 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lx - data->Lx).isZero(1e-7));
384 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-7));
385 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lu - data->Lu).isZero(1e-7));
386 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-7));
387 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lxx - data->Lxx).isZero(1e-7));
388 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-7));
389 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Lxu - data->Lxu).isZero(1e-7));
390 ✗ BOOST_CHECK((problem1.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-7));
391 ✗ BOOST_CHECK((problem2.get_runningDatas()[i]->Luu - data->Luu).isZero(1e-7));
392 ✗ }
393 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
394 ✗ model->createData();
395 ✗ model->calc(data, xs.back());
396 ✗ model->calcDiff(data, xs.back());
397 ✗ BOOST_CHECK((problem1.get_terminalData()->Fx - data->Fx).isZero(1e-7));
398 ✗ BOOST_CHECK((problem2.get_terminalData()->Fx - data->Fx).isZero(1e-7));
399 ✗ BOOST_CHECK((problem1.get_terminalData()->Lx - data->Lx).isZero(1e-7));
400 ✗ BOOST_CHECK((problem2.get_terminalData()->Lx - data->Lx).isZero(1e-7));
401 ✗ BOOST_CHECK((problem1.get_terminalData()->Lxx - data->Lxx).isZero(1e-7));
402 ✗ BOOST_CHECK((problem2.get_terminalData()->Lxx - data->Lxx).isZero(1e-7));
403
404 // Checking that casted computation is the same
405 #ifdef NDEBUG // Run only in release mode
406 crocoddyl::ShootingProblemTpl<float> casted_problem1 = problem1.cast<float>();
407 crocoddyl::ShootingProblemTpl<float> casted_problem2 = problem2.cast<float>();
408 std::vector<Eigen::VectorXf> xs_f(T + 1);
409 std::vector<Eigen::VectorXf> us_f(T);
410 for (std::size_t i = 0; i < T; ++i) {
411 xs_f[i] = xs[i].cast<float>();
412 us_f[i] = us[i].cast<float>();
413 }
414 xs_f.back() = xs.back().cast<float>();
415 casted_problem1.calc(xs_f, us_f);
416 casted_problem1.calcDiff(xs_f, us_f);
417 casted_problem2.calc(xs_f, us_f);
418 casted_problem2.calcDiff(xs_f, us_f);
419 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
420 for (std::size_t i = 0; i < T; ++i) {
421 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>&
422 casted_model = model->cast<float>();
423 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>>&
424 casted_data = casted_model->createData();
425 casted_model->calc(casted_data, xs_f[i], us_f[i]);
426 casted_model->calcDiff(casted_data, xs_f[i], us_f[i]);
427 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fx == casted_data->Fx);
428 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fx == casted_data->Fx);
429 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Fu == casted_data->Fu);
430 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Fu == casted_data->Fu);
431 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lx == casted_data->Lx);
432 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lx == casted_data->Lx);
433 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lu == casted_data->Lu);
434 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lu == casted_data->Lu);
435 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxx == casted_data->Lxx);
436 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxx == casted_data->Lxx);
437 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Lxu == casted_data->Lxu);
438 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Lxu == casted_data->Lxu);
439 BOOST_CHECK(casted_problem1.get_runningDatas()[i]->Luu == casted_data->Luu);
440 BOOST_CHECK(casted_problem2.get_runningDatas()[i]->Luu == casted_data->Luu);
441 BOOST_CHECK(
442 (problem1.get_runningDatas()[i]->Fx.cast<float>() - casted_data->Fx)
443 .isZero(tol_f));
444 BOOST_CHECK(
445 (problem1.get_runningDatas()[i]->Fu.cast<float>() - casted_data->Fu)
446 .isZero(tol_f));
447 BOOST_CHECK(
448 (problem1.get_runningDatas()[i]->Lx.cast<float>() - casted_data->Lx)
449 .isZero(tol_f));
450 BOOST_CHECK(
451 (problem1.get_runningDatas()[i]->Lu.cast<float>() - casted_data->Lu)
452 .isZero(tol_f));
453 BOOST_CHECK(
454 (problem1.get_runningDatas()[i]->Lxx.cast<float>() - casted_data->Lxx)
455 .isZero(20.f * tol_f));
456 BOOST_CHECK(
457 (problem1.get_runningDatas()[i]->Lxu.cast<float>() - casted_data->Lxu)
458 .isZero(tol_f));
459 BOOST_CHECK(
460 (problem1.get_runningDatas()[i]->Luu.cast<float>() - casted_data->Luu)
461 .isZero(tol_f));
462 }
463 #endif
464 ✗ }
465
466 ✗ void test_rollout(ActionModelTypes::Type action_model_type) {
467 // create the model
468 ✗ ActionModelFactory factory;
469 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
470 ✗ factory.create(action_model_type);
471
472 // create the shooting problem
473 ✗ std::size_t T = 20;
474 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
475 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
476 ✗ crocoddyl::ShootingProblem problem(x0, models, model);
477
478 // create random trajectory
479 ✗ std::vector<Eigen::VectorXd> xs(T + 1);
480 ✗ std::vector<Eigen::VectorXd> us(T);
481 ✗ for (std::size_t i = 0; i < T; ++i) {
482 ✗ xs[i] = model->get_state()->zero();
483 ✗ us[i] = Eigen::VectorXd::Random(model->get_nu());
484 }
485 ✗ xs.back() = model->get_state()->zero();
486
487 // check the state and cost in each node
488 ✗ problem.rollout(us, xs);
489 ✗ for (std::size_t i = 0; i < T; ++i) {
490 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
491 ✗ model->createData();
492 ✗ model->calc(data, xs[i], us[i]);
493 ✗ BOOST_CHECK((xs[i + 1] - data->xnext).isZero(1e-7));
494 ✗ }
495 ✗ }
496
497 ✗ void test_rollout_diffAction(
498 DifferentialActionModelTypes::Type action_model_type,
499 IntegratorTypes::Type integrator_type) {
500 // create the model
501 ✗ DifferentialActionModelFactory factory;
502 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
503 ✗ factory.create(action_model_type);
504 ✗ IntegratorFactory factory_int;
505 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
506 ✗ factory_int.create(integrator_type, diffModel);
507
508 // create the shooting problem
509 ✗ std::size_t T = 20;
510 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
511 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
512 ✗ crocoddyl::ShootingProblem problem(x0, models, model);
513
514 // create random trajectory
515 ✗ std::vector<Eigen::VectorXd> xs(T + 1);
516 ✗ std::vector<Eigen::VectorXd> us(T);
517 ✗ for (std::size_t i = 0; i < T; ++i) {
518 ✗ xs[i] = model->get_state()->zero();
519 ✗ us[i] = Eigen::VectorXd::Random(model->get_nu());
520 }
521 ✗ xs.back() = model->get_state()->zero();
522
523 // check the state and cost in each node
524 ✗ problem.rollout(us, xs);
525 ✗ for (std::size_t i = 0; i < T; ++i) {
526 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
527 ✗ model->createData();
528 ✗ model->calc(data, xs[i], us[i]);
529 ✗ BOOST_CHECK((xs[i + 1] - data->xnext).isZero(1e-7));
530 ✗ }
531 ✗ }
532
533 ✗ void test_quasiStatic(ActionModelTypes::Type action_model_type) {
534 // create the model
535 ✗ ActionModelFactory factory;
536 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
537 ✗ factory.create(action_model_type);
538
539 // create two shooting problems (with and without data allocation)
540 ✗ std::size_t T = 20;
541 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
542 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
543 ✗ std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
544 ✗ for (std::size_t i = 0; i < T; ++i) {
545 ✗ datas[i] = model->createData();
546 }
547 ✗ crocoddyl::ShootingProblem problem1(x0, models, model);
548 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
549 ✗ model->createData());
550
551 // create random trajectory
552 ✗ std::vector<Eigen::VectorXd> xs(T);
553 ✗ std::vector<Eigen::VectorXd> us(T);
554 ✗ for (std::size_t i = 0; i < T; ++i) {
555 ✗ xs[i] = model->get_state()->rand();
556 ✗ xs[i].tail(model->get_state()->get_nv()) *= 0;
557 ✗ us[i] = Eigen::VectorXd::Zero(model->get_nu());
558 }
559
560 // check the state and cost in each node
561 ✗ problem1.quasiStatic(us, xs);
562 ✗ for (std::size_t i = 0; i < T; ++i) {
563 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
564 ✗ model->createData();
565 ✗ Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
566 ✗ model->quasiStatic(data, u, xs[i]);
567 ✗ BOOST_CHECK((u - us[i]).isZero(1e-7));
568 ✗ }
569 ✗ problem2.quasiStatic(us, xs);
570 ✗ for (std::size_t i = 0; i < T; ++i) {
571 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
572 ✗ model->createData();
573 ✗ Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
574 ✗ model->quasiStatic(data, u, xs[i]);
575 ✗ BOOST_CHECK((u - us[i]).isZero(1e-7));
576 ✗ }
577 ✗ }
578
579 ✗ void test_quasiStatic_diffAction(
580 DifferentialActionModelTypes::Type action_model_type,
581 IntegratorTypes::Type integrator_type) {
582 // create the model
583 ✗ DifferentialActionModelFactory factory;
584 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract>& diffModel =
585 ✗ factory.create(action_model_type);
586 ✗ IntegratorFactory factory_int;
587 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
588 ✗ factory_int.create(integrator_type, diffModel);
589
590 // create two shooting problems (with and without data allocation)
591 ✗ std::size_t T = 20;
592 ✗ const Eigen::VectorXd& x0 = model->get_state()->rand();
593 ✗ std::vector<std::shared_ptr<crocoddyl::ActionModelAbstract>> models(T, model);
594 ✗ std::vector<std::shared_ptr<crocoddyl::ActionDataAbstract>> datas(T);
595 ✗ for (std::size_t i = 0; i < T; ++i) {
596 ✗ datas[i] = model->createData();
597 }
598 ✗ crocoddyl::ShootingProblem problem1(x0, models, model);
599 crocoddyl::ShootingProblem problem2(x0, models, model, datas,
600 ✗ model->createData());
601
602 // create random trajectory
603 ✗ std::vector<Eigen::VectorXd> xs(T);
604 ✗ std::vector<Eigen::VectorXd> us(T);
605 ✗ for (std::size_t i = 0; i < T; ++i) {
606 ✗ xs[i] = model->get_state()->rand();
607 ✗ xs[i].tail(model->get_state()->get_nv()) *= 0;
608 ✗ us[i] = Eigen::VectorXd::Zero(model->get_nu());
609 }
610
611 // check the state and cost in each node
612 ✗ problem1.quasiStatic(us, xs);
613 ✗ for (std::size_t i = 0; i < T; ++i) {
614 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
615 ✗ model->createData();
616 ✗ Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
617 ✗ model->quasiStatic(data, u, xs[i]);
618 ✗ BOOST_CHECK((u - us[i]).isZero(1e-7));
619 ✗ }
620 ✗ problem2.quasiStatic(us, xs);
621 ✗ for (std::size_t i = 0; i < T; ++i) {
622 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
623 ✗ model->createData();
624 ✗ Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
625 ✗ model->quasiStatic(data, u, xs[i]);
626 ✗ BOOST_CHECK((u - us[i]).isZero(1e-7));
627 ✗ }
628 ✗ }
629
630 //----------------------------------------------------------------------------//
631
632 ✗ void register_action_model_unit_tests(
633 ActionModelTypes::Type action_model_type) {
634 ✗ boost::test_tools::output_test_stream test_name;
635 ✗ test_name << "test_" << action_model_type;
636 ✗ std::cout << "Running " << test_name.str() << std::endl;
637 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
638 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calc, action_model_type)));
639 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff, action_model_type)));
640 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_quasiStatic, action_model_type)));
641 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_rollout, action_model_type)));
642 ✗ framework::master_test_suite().add(ts);
643 ✗ }
644
645 ✗ void register_diff_action_model_unit_tests(
646 DifferentialActionModelTypes::Type action_model_type,
647 IntegratorTypes::Type integrator_type) {
648 ✗ boost::test_tools::output_test_stream test_name;
649 ✗ test_name << "test_" << action_model_type << "_" << integrator_type;
650 ✗ std::cout << "Running " << test_name.str() << std::endl;
651 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
652 ✗ ts->add(BOOST_TEST_CASE(
653 boost::bind(&test_calc_diffAction, action_model_type, integrator_type)));
654 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_diffAction,
655 action_model_type, integrator_type)));
656 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_quasiStatic_diffAction,
657 action_model_type, integrator_type)));
658 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_rollout_diffAction,
659 action_model_type, integrator_type)));
660 ✗ framework::master_test_suite().add(ts);
661 ✗ }
662
663 ✗ bool init_function() {
664 ✗ for (size_t i = 0; i < ActionModelTypes::all.size(); ++i) {
665 ✗ register_action_model_unit_tests(ActionModelTypes::all[i]);
666 }
667 ✗ for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
668 ✗ for (size_t j = 0; j < IntegratorTypes::all.size(); ++j) {
669 ✗ register_diff_action_model_unit_tests(
670 ✗ DifferentialActionModelTypes::all[i], IntegratorTypes::all[j]);
671 }
672 }
673 ✗ return true;
674 }
675
676 ✗ int main(int argc, char** argv) {
677 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
678 }
679