GCC Code Coverage Report

Directory: ./
File: unittest/test_states.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 212 0.0%
Branches: 0 1426 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, New York University,
5 // Max Planck Gesellschaft, University of Edinburgh
6 // INRIA, Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #define BOOST_TEST_NO_MAIN
12 #define BOOST_TEST_ALTERNATIVE_INIT_API
13
14 #include "factory/state.hpp"
15 #include "unittest_common.hpp"
16
17 using namespace boost::unit_test;
18 using namespace crocoddyl::unittest;
19
20 //----------------------------------------------------------------------------//
21
22 ✗ void test_state_dimension(StateModelTypes::Type state_type) {
23 ✗ StateModelFactory factory;
24 const std::shared_ptr<crocoddyl::StateAbstract>& state =
25 ✗ factory.create(state_type);
26
27 // Checking the dimension of zero and random states
28 ✗ BOOST_CHECK(static_cast<std::size_t>(state->zero().size()) ==
29 state->get_nx());
30 ✗ BOOST_CHECK(static_cast<std::size_t>(state->rand().size()) ==
31 state->get_nx());
32 ✗ BOOST_CHECK(state->get_nx() == (state->get_nq() + state->get_nv()));
33 ✗ BOOST_CHECK(state->get_ndx() == (2 * state->get_nv()));
34 ✗ BOOST_CHECK(static_cast<std::size_t>(state->get_lb().size()) ==
35 state->get_nx());
36 ✗ BOOST_CHECK(static_cast<std::size_t>(state->get_ub().size()) ==
37 state->get_nx());
38
39 // Checking that casted computation is the same
40 #ifdef NDEBUG // Run only in release mode
41 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
42 state->cast<float>();
43 BOOST_CHECK(static_cast<std::size_t>(casted_state->zero().size()) ==
44 casted_state->get_nx());
45 BOOST_CHECK(static_cast<std::size_t>(casted_state->rand().size()) ==
46 casted_state->get_nx());
47 BOOST_CHECK(casted_state->get_nx() ==
48 (casted_state->get_nq() + casted_state->get_nv()));
49 BOOST_CHECK(casted_state->get_ndx() == (2 * casted_state->get_nv()));
50 BOOST_CHECK(static_cast<std::size_t>(casted_state->get_lb().size()) ==
51 casted_state->get_nx());
52 BOOST_CHECK(static_cast<std::size_t>(casted_state->get_ub().size()) ==
53 casted_state->get_nx());
54 #endif
55 }
56
57 ✗ void test_integrate_against_difference(StateModelTypes::Type state_type) {
58 ✗ StateModelFactory factory;
59 const std::shared_ptr<crocoddyl::StateAbstract>& state =
60 ✗ factory.create(state_type);
61
62 // Generating random states
63 ✗ const Eigen::VectorXd x1 = state->rand();
64 ✗ const Eigen::VectorXd x2 = state->rand();
65
66 // Computing x2 by integrating its difference
67 ✗ Eigen::VectorXd dx(state->get_ndx());
68 ✗ Eigen::VectorXd x2i(state->get_nx());
69 ✗ Eigen::VectorXd dxi(state->get_ndx());
70 ✗ state->diff(x1, x2, dx);
71 ✗ state->integrate(x1, dx, x2i);
72 ✗ state->diff(x2i, x2, dxi);
73
74 // Checking that both states agree
75 ✗ BOOST_CHECK(dxi.isZero(1e-9));
76
77 // Checking that casted computation is the same
78 #ifdef NDEBUG // Run only in release mode
79 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
80 state->cast<float>();
81 const Eigen::VectorXf x1_f = casted_state->rand();
82 const Eigen::VectorXf x2_f = casted_state->rand();
83 Eigen::VectorXf dx_f(casted_state->get_ndx());
84 Eigen::VectorXf x2i_f(casted_state->get_nx());
85 Eigen::VectorXf dxi_f(casted_state->get_ndx());
86 casted_state->diff(x1_f, x2_f, dx_f);
87 casted_state->integrate(x1_f, dx_f, x2i_f);
88 casted_state->diff(x2i_f, x2_f, dxi_f);
89 BOOST_CHECK(dxi_f.isZero(1e-6f));
90 #endif
91 }
92
93 ✗ void test_difference_against_integrate(StateModelTypes::Type state_type) {
94 ✗ StateModelFactory factory;
95 const std::shared_ptr<crocoddyl::StateAbstract>& state =
96 ✗ factory.create(state_type);
97
98 // Generating random states
99 ✗ const Eigen::VectorXd x = state->rand();
100 ✗ const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
101
102 // Computing dx by differentiation of its integrate
103 ✗ Eigen::VectorXd xidx(state->get_nx());
104 ✗ Eigen::VectorXd dxd(state->get_ndx());
105 ✗ state->integrate(x, dx, xidx);
106 ✗ state->diff(x, xidx, dxd);
107
108 // Checking that both states agree
109 ✗ BOOST_CHECK((dxd - dx).isZero(1e-9));
110
111 // Checking that casted computation is the same
112 #ifdef NDEBUG // Run only in release mode
113 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
114 state->cast<float>();
115 const Eigen::VectorXf x_f = casted_state->rand();
116 const Eigen::VectorXf dx_f = Eigen::VectorXf::Random(casted_state->get_ndx());
117 Eigen::VectorXf xidx_f(casted_state->get_nx());
118 Eigen::VectorXf dxd_f(casted_state->get_ndx());
119 casted_state->integrate(x_f, dx_f, xidx_f);
120 casted_state->diff(x_f, xidx_f, dxd_f);
121 BOOST_CHECK((dxd_f - dx_f).isZero(1e-6f));
122 #endif
123 }
124
125 ✗ void test_Jdiff_firstsecond(StateModelTypes::Type state_type) {
126 ✗ StateModelFactory factory;
127 const std::shared_ptr<crocoddyl::StateAbstract>& state =
128 ✗ factory.create(state_type);
129
130 // Generating random values for the initial and terminal states
131 ✗ const Eigen::VectorXd x1 = state->rand();
132 ✗ const Eigen::VectorXd x2 = state->rand();
133
134 // Computing the partial derivatives of the difference function separately
135 Eigen::MatrixXd Jdiff_tmp(
136 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
137 Eigen::MatrixXd Jdiff_first(
138 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
139 Eigen::MatrixXd Jdiff_second(
140 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
141 ✗ state->Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first);
142 ✗ state->Jdiff(x1, x2, Jdiff_tmp, Jdiff_second, crocoddyl::second);
143
144 // Computing the partial derivatives of the difference function separately
145 Eigen::MatrixXd Jdiff_both_first(
146 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
147 Eigen::MatrixXd Jdiff_both_second(
148 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
149 ✗ state->Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second);
150
151 ✗ BOOST_CHECK((Jdiff_first - Jdiff_both_first).isZero(1e-9));
152 ✗ BOOST_CHECK((Jdiff_second - Jdiff_both_second).isZero(1e-9));
153
154 // Checking that casted computation is the same
155 #ifdef NDEBUG // Run only in release mode
156 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
157 state->cast<float>();
158 const Eigen::VectorXf x1_f = casted_state->rand();
159 const Eigen::VectorXf x2_f = casted_state->rand();
160 Eigen::MatrixXf Jdiff_tmp_f(
161 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
162 Eigen::MatrixXf Jdiff_first_f(
163 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
164 Eigen::MatrixXf Jdiff_second_f(
165 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
166 Eigen::MatrixXf Jdiff_both_first_f(
167 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
168 Eigen::MatrixXf Jdiff_both_second_f(
169 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
170 casted_state->Jdiff(x1_f, x2_f, Jdiff_first_f, Jdiff_tmp_f, crocoddyl::first);
171 casted_state->Jdiff(x1_f, x2_f, Jdiff_tmp_f, Jdiff_second_f,
172 crocoddyl::second);
173 casted_state->Jdiff(x1_f, x2_f, Jdiff_both_first_f, Jdiff_both_second_f);
174 BOOST_CHECK((Jdiff_first_f - Jdiff_both_first_f).isZero(1e-9f));
175 BOOST_CHECK((Jdiff_second_f - Jdiff_both_second_f).isZero(1e-9f));
176 #endif
177 }
178
179 ✗ void test_Jint_firstsecond(StateModelTypes::Type state_type) {
180 ✗ StateModelFactory factory;
181 const std::shared_ptr<crocoddyl::StateAbstract>& state =
182 ✗ factory.create(state_type);
183
184 // Generating random values for the initial and terminal states
185 ✗ const Eigen::VectorXd x = state->rand();
186 ✗ const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
187
188 // Computing the partial derivatives of the difference function separately
189 Eigen::MatrixXd Jint_tmp(
190 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
191 Eigen::MatrixXd Jint_first(
192 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
193 Eigen::MatrixXd Jint_second(
194 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
195 ✗ state->Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first);
196 ✗ state->Jintegrate(x, dx, Jint_tmp, Jint_second, crocoddyl::second);
197
198 // Computing the partial derivatives of the integrate function separately
199 Eigen::MatrixXd Jint_both_first(
200 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
201 Eigen::MatrixXd Jint_both_second(
202 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
203 ✗ state->Jintegrate(x, dx, Jint_both_first, Jint_both_second);
204
205 ✗ BOOST_CHECK((Jint_first - Jint_both_first).isZero(1e-9));
206 ✗ BOOST_CHECK((Jint_second - Jint_both_second).isZero(1e-9));
207
208 // Checking that casted computation is the same
209 #ifdef NDEBUG // Run only in release mode
210 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
211 state->cast<float>();
212 const Eigen::VectorXf x_f = casted_state->rand();
213 const Eigen::VectorXf dx_f = Eigen::VectorXf::Random(casted_state->get_ndx());
214 Eigen::MatrixXf Jint_tmp_f(
215 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
216 Eigen::MatrixXf Jint_first_f(
217 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
218 Eigen::MatrixXf Jint_second_f(
219 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
220 Eigen::MatrixXf Jint_both_first_f(
221 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
222 Eigen::MatrixXf Jint_both_second_f(
223 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
224 casted_state->Jintegrate(x_f, dx_f, Jint_first_f, Jint_tmp_f,
225 crocoddyl::first);
226 casted_state->Jintegrate(x_f, dx_f, Jint_tmp_f, Jint_second_f,
227 crocoddyl::second);
228 casted_state->Jintegrate(x_f, dx_f, Jint_both_first_f, Jint_both_second_f);
229 BOOST_CHECK((Jint_first_f - Jint_both_first_f).isZero(1e-9f));
230 BOOST_CHECK((Jint_second_f - Jint_both_second_f).isZero(1e-9f));
231 #endif
232 }
233
234 ✗ void test_Jdiff_num_diff_firstsecond(StateModelTypes::Type state_type) {
235 ✗ StateModelFactory factory;
236 const std::shared_ptr<crocoddyl::StateAbstract>& state =
237 ✗ factory.create(state_type);
238 // Generating random values for the initial and terminal states
239 ✗ const Eigen::VectorXd x1 = state->rand();
240 ✗ const Eigen::VectorXd x2 = state->rand();
241
242 // Get the num diff state
243 ✗ crocoddyl::StateNumDiff state_num_diff(state);
244
245 // Computing the partial derivatives of the difference function separately
246 Eigen::MatrixXd Jdiff_num_diff_tmp(
247 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
248 Eigen::MatrixXd Jdiff_num_diff_first(
249 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
250 Eigen::MatrixXd Jdiff_num_diff_second(
251 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
252 ✗ state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_first, Jdiff_num_diff_tmp,
253 crocoddyl::first);
254 ✗ state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_tmp, Jdiff_num_diff_second,
255 crocoddyl::second);
256
257 // Computing the partial derivatives of the difference function separately
258 Eigen::MatrixXd Jdiff_num_diff_both_first(
259 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
260 Eigen::MatrixXd Jdiff_num_diff_both_second(
261 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
262 ✗ state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_both_first,
263 Jdiff_num_diff_both_second);
264
265 ✗ BOOST_CHECK((Jdiff_num_diff_first - Jdiff_num_diff_both_first).isZero(1e-9));
266 ✗ BOOST_CHECK(
267 (Jdiff_num_diff_second - Jdiff_num_diff_both_second).isZero(1e-9));
268 }
269
270 ✗ void test_Jint_num_diff_firstsecond(StateModelTypes::Type state_type) {
271 ✗ StateModelFactory factory;
272 const std::shared_ptr<crocoddyl::StateAbstract>& state =
273 ✗ factory.create(state_type);
274 // Generating random values for the initial and terminal states
275 ✗ const Eigen::VectorXd x = state->rand();
276 ✗ const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
277
278 // Get the num diff state
279 ✗ crocoddyl::StateNumDiff state_num_diff(state);
280
281 // Computing the partial derivatives of the difference function separately
282 Eigen::MatrixXd Jint_num_diff_tmp(
283 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
284 Eigen::MatrixXd Jint_num_diff_first(
285 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
286 Eigen::MatrixXd Jint_num_diff_second(
287 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
288 ✗ state_num_diff.Jintegrate(x, dx, Jint_num_diff_first, Jint_num_diff_tmp,
289 crocoddyl::first);
290 ✗ state_num_diff.Jintegrate(x, dx, Jint_num_diff_tmp, Jint_num_diff_second,
291 crocoddyl::second);
292
293 // Computing the partial derivatives of the given function separately
294 Eigen::MatrixXd Jint_num_diff_both_first(
295 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
296 Eigen::MatrixXd Jint_num_diff_both_second(
297 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
298 ✗ state_num_diff.Jintegrate(x, dx, Jint_num_diff_both_first,
299 Jint_num_diff_both_second);
300
301 ✗ BOOST_CHECK((Jint_num_diff_first - Jint_num_diff_both_first).isZero(1e-9));
302 ✗ BOOST_CHECK((Jint_num_diff_second - Jint_num_diff_both_second).isZero(1e-9));
303 }
304
305 ✗ void test_Jdiff_against_numdiff(StateModelTypes::Type state_type) {
306 ✗ StateModelFactory factory;
307 const std::shared_ptr<crocoddyl::StateAbstract>& state =
308 ✗ factory.create(state_type);
309 // Generating random values for the initial and terminal states
310 ✗ const Eigen::VectorXd x1 = state->rand();
311 ✗ const Eigen::VectorXd x2 = state->rand();
312
313 // Computing the partial derivatives of the difference function analytically
314 Eigen::MatrixXd Jdiff_1(
315 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
316 Eigen::MatrixXd Jdiff_2(
317 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
318 ✗ state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first);
319 ✗ state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second);
320
321 // Computing the partial derivatives of the difference function numerically
322 ✗ crocoddyl::StateNumDiff state_num_diff(state);
323 Eigen::MatrixXd Jdiff_num_1(
324 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
325 Eigen::MatrixXd Jdiff_num_2(
326 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
327 ✗ state_num_diff.Jdiff(x1, x2, Jdiff_num_1, Jdiff_num_2);
328
329 // Checking the partial derivatives against numerical differentiation
330 // Tolerance defined as in
331 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
332 ✗ double tol = std::pow(std::sqrt(2.0 * std::numeric_limits<double>::epsilon()),
333 1. / 3.);
334 ✗ BOOST_CHECK((Jdiff_1 - Jdiff_num_1).isZero(tol));
335 ✗ BOOST_CHECK((Jdiff_2 - Jdiff_num_2).isZero(tol));
336 }
337
338 ✗ void test_Jintegrate_against_numdiff(StateModelTypes::Type state_type) {
339 ✗ StateModelFactory factory;
340 const std::shared_ptr<crocoddyl::StateAbstract>& state =
341 ✗ factory.create(state_type);
342
343 // Generating random values for the initial state and its rate of change
344 ✗ const Eigen::VectorXd x = state->rand();
345 ✗ const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
346
347 // Computing the partial derivatives of the difference function analytically
348 Eigen::MatrixXd Jint_1(
349 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
350 Eigen::MatrixXd Jint_2(
351 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
352 ✗ state->Jintegrate(x, dx, Jint_1, Jint_2);
353
354 // Computing the partial derivatives of the difference function numerically
355 ✗ crocoddyl::StateNumDiff state_num_diff(state);
356 Eigen::MatrixXd Jint_num_1(
357 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
358 Eigen::MatrixXd Jint_num_2(
359 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
360 ✗ state_num_diff.Jintegrate(x, dx, Jint_num_1, Jint_num_2);
361
362 // Checking the partial derivatives against numerical differentiation
363 // Tolerance defined as in
364 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
365 ✗ double tol = std::pow(std::sqrt(2.0 * std::numeric_limits<double>::epsilon()),
366 1. / 3.);
367 ✗ BOOST_CHECK((Jint_1 - Jint_num_1).isZero(tol));
368 ✗ BOOST_CHECK((Jint_2 - Jint_num_2).isZero(tol));
369
370 // Checking that casted computation is the same
371 #ifdef NDEBUG // Run only in release mode
372 float tol_f = std::sqrt(float(2.0) * std::numeric_limits<float>::epsilon());
373 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
374 state->cast<float>();
375 crocoddyl::StateNumDiffTpl<float> casted_state_num_diff =
376 state_num_diff.cast<float>();
377 const Eigen::VectorXf x_f = casted_state->rand();
378 const Eigen::VectorXf dx_f = Eigen::VectorXf::Random(casted_state->get_ndx());
379 Eigen::MatrixXf Jint_1_f(
380 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
381 Eigen::MatrixXf Jint_2_f(
382 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
383 Eigen::MatrixXf Jint_num_1_f(
384 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
385 Eigen::MatrixXf Jint_num_2_f(
386 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
387 casted_state_num_diff.Jintegrate(x_f, dx_f, Jint_num_1_f, Jint_num_2_f);
388 casted_state->Jintegrate(x_f, dx_f, Jint_1_f, Jint_2_f);
389 BOOST_CHECK((Jint_1_f - Jint_num_1_f).isZero(tol_f));
390 BOOST_CHECK((Jint_2_f - Jint_num_2_f).isZero(tol_f));
391 #endif
392 }
393
394 ✗ void test_JintegrateTransport(StateModelTypes::Type state_type) {
395 ✗ StateModelFactory factory;
396 const std::shared_ptr<crocoddyl::StateAbstract>& state =
397 ✗ factory.create(state_type);
398
399 // Generating random values for the initial state and its rate of change
400 ✗ const Eigen::VectorXd x = state->rand();
401 ✗ const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
402
403 // Computing the partial derivatives of the difference function analytically
404 Eigen::MatrixXd Jint_1(
405 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
406 Eigen::MatrixXd Jint_2(
407 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
408 ✗ state->Jintegrate(x, dx, Jint_1, Jint_2);
409
410 Eigen::MatrixXd Jref(
411 ✗ Eigen::MatrixXd::Random(state->get_ndx(), 2 * state->get_ndx()));
412 ✗ const Eigen::MatrixXd Jtest(Jref);
413
414 ✗ state->JintegrateTransport(x, dx, Jref, crocoddyl::first);
415 ✗ BOOST_CHECK((Jref - Jint_1 * Jtest).isZero(1e-10));
416
417 ✗ Jref = Jtest;
418 ✗ state->JintegrateTransport(x, dx, Jref, crocoddyl::second);
419 ✗ BOOST_CHECK((Jref - Jint_2 * Jtest).isZero(1e-10));
420
421 // Checking that casted computation is the same
422 #ifdef NDEBUG // Run only in release mode
423 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
424 state->cast<float>();
425 const Eigen::VectorXf x_f = casted_state->rand();
426 const Eigen::VectorXf dx_f = Eigen::VectorXf::Random(casted_state->get_ndx());
427 Eigen::MatrixXf Jint_1_f(
428 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
429 Eigen::MatrixXf Jint_2_f(
430 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
431 Eigen::MatrixXf Jref_f(Eigen::MatrixXf::Random(casted_state->get_ndx(),
432 2 * casted_state->get_ndx()));
433 const Eigen::MatrixXf Jtest_f(Jref_f);
434 casted_state->Jintegrate(x_f, dx_f, Jint_1_f, Jint_2_f);
435 Jref_f = Jtest_f;
436 casted_state->JintegrateTransport(x_f, dx_f, Jref_f, crocoddyl::first);
437 BOOST_CHECK((Jref_f - Jint_1_f * Jtest_f).isZero(1e-6f));
438 casted_state->JintegrateTransport(x_f, dx_f, Jref_f, crocoddyl::second);
439 Jref_f = Jtest_f;
440 casted_state->JintegrateTransport(x_f, dx_f, Jref_f, crocoddyl::second);
441 BOOST_CHECK((Jref_f - Jint_2_f * Jtest_f).isZero(1e-6f));
442 #endif
443 }
444
445 ✗ void test_Jdiff_and_Jintegrate_are_inverses(StateModelTypes::Type state_type) {
446 ✗ StateModelFactory factory;
447 const std::shared_ptr<crocoddyl::StateAbstract>& state =
448 ✗ factory.create(state_type);
449
450 // Generating random states
451 ✗ const Eigen::VectorXd x1 = state->rand();
452 ✗ const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
453 ✗ Eigen::VectorXd x2(state->get_nx());
454 ✗ state->integrate(x1, dx, x2);
455
456 // Computing the partial derivatives of the integrate and difference function
457 ✗ Eigen::MatrixXd Jx(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
458 Eigen::MatrixXd Jdx(
459 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
460 ✗ state->Jintegrate(x1, dx, Jx, Jdx);
461 ✗ Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
462 ✗ Eigen::MatrixXd J2(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
463 ✗ state->Jdiff(x1, x2, J1, J2);
464
465 // Checking that Jdiff and Jintegrate are inverses
466 ✗ Eigen::MatrixXd dX_dDX = Jdx;
467 ✗ Eigen::MatrixXd dDX_dX = J2;
468 ✗ BOOST_CHECK((dX_dDX - dDX_dX.inverse()).isZero(1e-9));
469
470 // Checking that casted computation is the same
471 #ifdef NDEBUG // Run only in release mode
472 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
473 state->cast<float>();
474 const Eigen::VectorXf x1_f = casted_state->rand();
475 const Eigen::VectorXf dx_f = Eigen::VectorXf::Random(casted_state->get_ndx());
476 Eigen::VectorXf x2_f(casted_state->get_nx());
477
478 Eigen::MatrixXf Jx_f(
479 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
480 Eigen::MatrixXf Jdx_f(
481 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
482 Eigen::MatrixXf J1_f(
483 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
484 Eigen::MatrixXf J2_f(
485 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
486 Eigen::MatrixXf dX_dDX_f = Jdx_f;
487 Eigen::MatrixXf dDX_dX_f = J2_f;
488 casted_state->integrate(x1_f, dx_f, x2_f);
489 casted_state->Jintegrate(x1_f, dx_f, Jx_f, Jdx_f);
490 casted_state->Jdiff(x1_f, x2_f, J1_f, J2_f);
491 dX_dDX_f = Jdx_f;
492 dDX_dX_f = J2_f;
493 BOOST_CHECK((dX_dDX_f - dDX_dX_f.inverse()).isZero(1e-4f));
494 #endif
495 }
496
497 ✗ void test_velocity_from_Jintegrate_Jdiff(StateModelTypes::Type state_type) {
498 ✗ StateModelFactory factory;
499 const std::shared_ptr<crocoddyl::StateAbstract>& state =
500 ✗ factory.create(state_type);
501
502 // Generating random states
503 ✗ const Eigen::VectorXd x1 = state->rand();
504 ✗ Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
505 ✗ Eigen::VectorXd x2(state->get_nx());
506 ✗ state->integrate(x1, dx, x2);
507 ✗ Eigen::VectorXd eps = Eigen::VectorXd::Random(state->get_ndx());
508 ✗ double h = 1e-8;
509
510 // Computing the partial derivatives of the integrate and difference function
511 ✗ Eigen::MatrixXd Jx(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
512 Eigen::MatrixXd Jdx(
513 ✗ Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
514 ✗ state->Jintegrate(x1, dx, Jx, Jdx);
515 ✗ Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
516 ✗ Eigen::MatrixXd J2(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
517 ✗ state->Jdiff(x1, x2, J1, J2);
518
519 // Checking that computed velocity from Jintegrate
520 ✗ Eigen::MatrixXd dX_dDX = Jdx;
521 ✗ Eigen::VectorXd x2eps(state->get_nx());
522 ✗ state->integrate(x1, dx + eps * h, x2eps);
523 ✗ Eigen::VectorXd x2_eps(state->get_ndx());
524 ✗ state->diff(x2, x2eps, x2_eps);
525 ✗ BOOST_CHECK((dX_dDX * eps - x2_eps / h).isZero(1e-3));
526
527 // Checking the velocity computed from Jdiff
528 ✗ const Eigen::VectorXd x = state->rand();
529 ✗ dx.setZero();
530 ✗ state->diff(x1, x, dx);
531 ✗ Eigen::VectorXd x2i(state->get_nx());
532 ✗ state->integrate(x, eps * h, x2i);
533 ✗ Eigen::VectorXd dxi(state->get_ndx());
534 ✗ state->diff(x1, x2i, dxi);
535 ✗ J1.setZero();
536 ✗ J2.setZero();
537 ✗ state->Jdiff(x1, x, J1, J2);
538 ✗ BOOST_CHECK((J2 * eps - (-dx + dxi) / h).isZero(1e-3));
539
540 // Checking that casted computation is the same
541 #ifdef NDEBUG // Run only in release mode
542 const std::shared_ptr<crocoddyl::StateAbstractTpl<float>>& casted_state =
543 state->cast<float>();
544 float h_f = std::sqrt(float(2.0) * std::numeric_limits<float>::epsilon());
545 Eigen::VectorXf eps_f = Eigen::VectorXf::Random(casted_state->get_ndx());
546 const Eigen::VectorXf x1_f = casted_state->rand();
547 Eigen::VectorXf dx_f = Eigen::VectorXf::Random(casted_state->get_ndx());
548 Eigen::VectorXf x2_f(casted_state->get_nx());
549 Eigen::MatrixXf Jx_f(
550 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
551 Eigen::MatrixXf Jdx_f(
552 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
553 Eigen::MatrixXf J1_f(
554 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
555 Eigen::MatrixXf J2_f(
556 Eigen::MatrixXf::Zero(casted_state->get_ndx(), casted_state->get_ndx()));
557 Eigen::MatrixXf dX_dDX_f = Jdx_f;
558 Eigen::VectorXf x2eps_f(casted_state->get_nx());
559 Eigen::VectorXf x2_eps_f(casted_state->get_ndx());
560 const Eigen::VectorXf x_f = casted_state->rand();
561 Eigen::VectorXf x2i_f(casted_state->get_nx());
562 Eigen::VectorXf dxi_f(casted_state->get_ndx());
563 casted_state->integrate(x1_f, dx_f, x2_f);
564 casted_state->Jintegrate(x1_f, dx_f, Jx_f, Jdx_f);
565 casted_state->Jdiff(x1_f, x2_f, J1_f, J2_f);
566 dX_dDX_f = Jdx_f;
567 casted_state->integrate(x1_f, dx_f + eps_f * h_f, x2eps_f);
568 casted_state->diff(x2_f, x2eps_f, x2_eps_f);
569 BOOST_CHECK((dX_dDX_f * eps_f - x2_eps_f / h_f).isZero(1e-3f));
570 dx_f.setZero();
571 casted_state->diff(x1_f, x_f, dx_f);
572 casted_state->integrate(x_f, eps_f * h_f, x2i_f);
573 casted_state->diff(x1_f, x2i_f, dxi_f);
574 casted_state->Jdiff(x1_f, x_f, J1_f, J2_f);
575 BOOST_CHECK((J2_f * eps_f - (dxi_f - dx_f) / h_f).isZero(1e-2f));
576 #endif
577 }
578
579 //----------------------------------------------------------------------------//
580
581 ✗ void register_state_unit_tests(StateModelTypes::Type state_type) {
582 ✗ boost::test_tools::output_test_stream test_name;
583 ✗ test_name << "test_" << state_type;
584 ✗ std::cout << "Running " << test_name.str() << std::endl;
585 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
586 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_state_dimension, state_type)));
587 ✗ ts->add(BOOST_TEST_CASE(
588 boost::bind(&test_integrate_against_difference, state_type)));
589 ✗ ts->add(BOOST_TEST_CASE(
590 boost::bind(&test_difference_against_integrate, state_type)));
591 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, state_type)));
592 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, state_type)));
593 ✗ ts->add(BOOST_TEST_CASE(
594 boost::bind(&test_Jdiff_num_diff_firstsecond, state_type)));
595 ✗ ts->add(BOOST_TEST_CASE(
596 boost::bind(&test_Jint_num_diff_firstsecond, state_type)));
597 ✗ ts->add(
598 ✗ BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, state_type)));
599 ✗ ts->add(BOOST_TEST_CASE(
600 boost::bind(&test_Jintegrate_against_numdiff, state_type)));
601 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_JintegrateTransport, state_type)));
602 ✗ ts->add(BOOST_TEST_CASE(
603 boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, state_type)));
604 ✗ ts->add(BOOST_TEST_CASE(
605 boost::bind(&test_velocity_from_Jintegrate_Jdiff, state_type)));
606 ✗ framework::master_test_suite().add(ts);
607 }
608
609 ✗ bool init_function() {
610 ✗ for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
611 ✗ register_state_unit_tests(StateModelTypes::all[i]);
612 }
613 ✗ return true;
614 }
615
616 ✗ int main(int argc, char** argv) {
617 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
618 }
619