GCC Code Coverage Report

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