GCC Code Coverage Report

Directory: ./
File: unittest/test_states.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 225 225 100.0%
Branches: 714 1426 50.1%
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 7 void test_state_dimension(StateModelTypes::Type state_type) {
23
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
24 const std::shared_ptr<crocoddyl::StateAbstract>& state =
25
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
26
27 // Checking the dimension of zero and random states
28
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->zero().size()) ==
29 state->get_nx());
30
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->rand().size()) ==
31 state->get_nx());
32
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 7 times.
 7 BOOST_CHECK(state->get_nx() == (state->get_nq() + state->get_nv()));
33
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 7 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 7 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 7 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 7 times.
 7 BOOST_CHECK(state->get_ndx() == (2 * state->get_nv()));
34
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 BOOST_CHECK(static_cast<std::size_t>(state->get_lb().size()) ==
35 state->get_nx());
36
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 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 7 }
56
57 7 void test_integrate_against_difference(StateModelTypes::Type state_type) {
58
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
59 const std::shared_ptr<crocoddyl::StateAbstract>& state =
60
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
61
62 // Generating random states
63
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
64
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
65
66 // Computing x2 by integrating its difference
67
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd dx(state->get_ndx());
68
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd x2i(state->get_nx());
69
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd dxi(state->get_ndx());
70
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x1, x2, dx);
71
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x1, dx, x2i);
72
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x2i, x2, dxi);
73
74 // Checking that both states agree
75
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 7 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 7 }
92
93 7 void test_difference_against_integrate(StateModelTypes::Type state_type) {
94
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
95 const std::shared_ptr<crocoddyl::StateAbstract>& state =
96
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
97
98 // Generating random states
99
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
100
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
101
102 // Computing dx by differentiation of its integrate
103
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd xidx(state->get_nx());
104
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd dxd(state->get_ndx());
105
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x, dx, xidx);
106
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x, xidx, dxd);
107
108 // Checking that both states agree
109
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 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 7 }
124
125 7 void test_Jdiff_firstsecond(StateModelTypes::Type state_type) {
126
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
127 const std::shared_ptr<crocoddyl::StateAbstract>& state =
128
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
129
130 // Generating random values for the initial and terminal states
131
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
132
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
133
134 // Computing the partial derivatives of the difference function separately
135 Eigen::MatrixXd Jdiff_tmp(
136
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
137 Eigen::MatrixXd Jdiff_first(
138
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
139 Eigen::MatrixXd Jdiff_second(
140
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
141
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_first, Jdiff_tmp, crocoddyl::first);
142
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
147 Eigen::MatrixXd Jdiff_both_second(
148
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
149
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_both_first, Jdiff_both_second);
150
151
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_first - Jdiff_both_first).isZero(1e-9));
152
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 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 7 }
178
179 7 void test_Jint_firstsecond(StateModelTypes::Type state_type) {
180
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
181 const std::shared_ptr<crocoddyl::StateAbstract>& state =
182
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
183
184 // Generating random values for the initial and terminal states
185
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
186
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
191 Eigen::MatrixXd Jint_first(
192
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
193 Eigen::MatrixXd Jint_second(
194
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
195
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_first, Jint_tmp, crocoddyl::first);
196
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
201 Eigen::MatrixXd Jint_both_second(
202
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
203
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_both_first, Jint_both_second);
204
205
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_first - Jint_both_first).isZero(1e-9));
206
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 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 7 }
233
234 7 void test_Jdiff_num_diff_firstsecond(StateModelTypes::Type state_type) {
235
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
236 const std::shared_ptr<crocoddyl::StateAbstract>& state =
237
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
238 // Generating random values for the initial and terminal states
239
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
240
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
241
242 // Get the num diff state
243
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
248 Eigen::MatrixXd Jdiff_num_diff_first(
249
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
250 Eigen::MatrixXd Jdiff_num_diff_second(
251
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
252
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_first, Jdiff_num_diff_tmp,
253 crocoddyl::first);
254
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
260 Eigen::MatrixXd Jdiff_num_diff_both_second(
261
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
262
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jdiff(x1, x2, Jdiff_num_diff_both_first,
263 Jdiff_num_diff_both_second);
264
265
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_num_diff_first - Jdiff_num_diff_both_first).isZero(1e-9));
266
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK(
267 (Jdiff_num_diff_second - Jdiff_num_diff_both_second).isZero(1e-9));
268 7 }
269
270 7 void test_Jint_num_diff_firstsecond(StateModelTypes::Type state_type) {
271
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
272 const std::shared_ptr<crocoddyl::StateAbstract>& state =
273
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
274 // Generating random values for the initial and terminal states
275
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
276
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
277
278 // Get the num diff state
279
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
284 Eigen::MatrixXd Jint_num_diff_first(
285
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
286 Eigen::MatrixXd Jint_num_diff_second(
287
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
288
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jintegrate(x, dx, Jint_num_diff_first, Jint_num_diff_tmp,
289 crocoddyl::first);
290
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
296 Eigen::MatrixXd Jint_num_diff_both_second(
297
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
298
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 state_num_diff.Jintegrate(x, dx, Jint_num_diff_both_first,
299 Jint_num_diff_both_second);
300
301
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_num_diff_first - Jint_num_diff_both_first).isZero(1e-9));
302
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_num_diff_second - Jint_num_diff_both_second).isZero(1e-9));
303 7 }
304
305 7 void test_Jdiff_against_numdiff(StateModelTypes::Type state_type) {
306
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
307 const std::shared_ptr<crocoddyl::StateAbstract>& state =
308
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
309 // Generating random values for the initial and terminal states
310
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
311
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x2 = state->rand();
312
313 // Computing the partial derivatives of the difference function analytically
314 Eigen::MatrixXd Jdiff_1(
315
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
316 Eigen::MatrixXd Jdiff_2(
317
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
318
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::first);
319
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, Jdiff_1, Jdiff_2, crocoddyl::second);
320
321 // Computing the partial derivatives of the difference function numerically
322
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::StateNumDiff state_num_diff(state);
323 Eigen::MatrixXd Jdiff_num_1(
324
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
325 Eigen::MatrixXd Jdiff_num_2(
326
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
327
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 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 7 double tol = std::pow(std::sqrt(2.0 * std::numeric_limits<double>::epsilon()),
333 1. / 3.);
334
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_1 - Jdiff_num_1).isZero(tol));
335
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jdiff_2 - Jdiff_num_2).isZero(tol));
336 7 }
337
338 7 void test_Jintegrate_against_numdiff(StateModelTypes::Type state_type) {
339
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
340 const std::shared_ptr<crocoddyl::StateAbstract>& state =
341
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
342
343 // Generating random values for the initial state and its rate of change
344
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
345
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
350 Eigen::MatrixXd Jint_2(
351
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
352
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_1, Jint_2);
353
354 // Computing the partial derivatives of the difference function numerically
355
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 crocoddyl::StateNumDiff state_num_diff(state);
356 Eigen::MatrixXd Jint_num_1(
357
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
358 Eigen::MatrixXd Jint_num_2(
359
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
360
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 7 times.
✗ Branch 14 not taken.
 7 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 7 double tol = std::pow(std::sqrt(2.0 * std::numeric_limits<double>::epsilon()),
366 1. / 3.);
367
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 BOOST_CHECK((Jint_1 - Jint_num_1).isZero(tol));
368
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 7 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 7 }
393
394 7 void test_JintegrateTransport(StateModelTypes::Type state_type) {
395
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
396 const std::shared_ptr<crocoddyl::StateAbstract>& state =
397
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
398
399 // Generating random values for the initial state and its rate of change
400
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
401
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 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
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
406 Eigen::MatrixXd Jint_2(
407
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
408
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x, dx, Jint_1, Jint_2);
409
410 Eigen::MatrixXd Jref(
411
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Random(state->get_ndx(), 2 * state->get_ndx()));
412
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 const Eigen::MatrixXd Jtest(Jref);
413
414
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->JintegrateTransport(x, dx, Jref, crocoddyl::first);
415
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 BOOST_CHECK((Jref - Jint_1 * Jtest).isZero(1e-10));
416
417
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Jref = Jtest;
418
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->JintegrateTransport(x, dx, Jref, crocoddyl::second);
419
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 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 7 }
444
445 7 void test_Jdiff_and_Jintegrate_are_inverses(StateModelTypes::Type state_type) {
446
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
447 const std::shared_ptr<crocoddyl::StateAbstract>& state =
448
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
449
450 // Generating random states
451
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
452
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 const Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
453
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd x2(state->get_nx());
454
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x1, dx, x2);
455
456 // Computing the partial derivatives of the integrate and difference function
457
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd Jx(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
458 Eigen::MatrixXd Jdx(
459
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
460
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x1, dx, Jx, Jdx);
461
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
462
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd J2(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
463
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, J1, J2);
464
465 // Checking that Jdiff and Jintegrate are inverses
466
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Eigen::MatrixXd dX_dDX = Jdx;
467
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Eigen::MatrixXd dDX_dX = J2;
468
9/18
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 7 times.
 7 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 7 }
496
497 7 void test_velocity_from_Jintegrate_Jdiff(StateModelTypes::Type state_type) {
498
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 StateModelFactory factory;
499 const std::shared_ptr<crocoddyl::StateAbstract>& state =
500
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 factory.create(state_type);
501
502 // Generating random states
503
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x1 = state->rand();
504
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::VectorXd dx = Eigen::VectorXd::Random(state->get_ndx());
505
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd x2(state->get_nx());
506
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->integrate(x1, dx, x2);
507
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 Eigen::VectorXd eps = Eigen::VectorXd::Random(state->get_ndx());
508 7 double h = 1e-8;
509
510 // Computing the partial derivatives of the integrate and difference function
511
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd Jx(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
512 Eigen::MatrixXd Jdx(
513
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
514
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jintegrate(x1, dx, Jx, Jdx);
515
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
516
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 7 times.
✗ Branch 13 not taken.
 7 Eigen::MatrixXd J2(Eigen::MatrixXd::Zero(state->get_ndx(), state->get_ndx()));
517
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x2, J1, J2);
518
519 // Checking that computed velocity from Jintegrate
520
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 Eigen::MatrixXd dX_dDX = Jdx;
521
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd x2eps(state->get_nx());
522
6/12
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
 7 state->integrate(x1, dx + eps * h, x2eps);
523
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd x2_eps(state->get_ndx());
524
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x2, x2eps, x2_eps);
525
10/20
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7 times.
✗ Branch 30 not taken.
✗ Branch 34 not taken.
✓ Branch 35 taken 7 times.
 7 BOOST_CHECK((dX_dDX * eps - x2_eps / h).isZero(1e-3));
526
527 // Checking the velocity computed from Jdiff
528
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 const Eigen::VectorXd x = state->rand();
529
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 dx.setZero();
530
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x1, x, dx);
531
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd x2i(state->get_nx());
532
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->integrate(x, eps * h, x2i);
533
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 Eigen::VectorXd dxi(state->get_ndx());
534
4/8
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 state->diff(x1, x2i, dxi);
535
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 J1.setZero();
536
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 J2.setZero();
537
5/10
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 state->Jdiff(x1, x, J1, J2);
538
12/24
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 7 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 7 times.
✗ Branch 36 not taken.
✗ Branch 40 not taken.
✓ Branch 41 taken 7 times.
 7 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 7 }
578
579 //----------------------------------------------------------------------------//
580
581 7 void register_state_unit_tests(StateModelTypes::Type state_type) {
582
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 boost::test_tools::output_test_stream test_name;
583
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 test_name << "test_" << state_type;
584
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 std::cout << "Running " << test_name.str() << std::endl;
585
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 7 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
586
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_state_dimension, state_type)));
587
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
588 boost::bind(&test_integrate_against_difference, state_type)));
589
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
590 boost::bind(&test_difference_against_integrate, state_type)));
591
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_Jdiff_firstsecond, state_type)));
592
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_Jint_firstsecond, state_type)));
593
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
594 boost::bind(&test_Jdiff_num_diff_firstsecond, state_type)));
595
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
596 boost::bind(&test_Jint_num_diff_firstsecond, state_type)));
597 14 ts->add(
598
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 BOOST_TEST_CASE(boost::bind(&test_Jdiff_against_numdiff, state_type)));
599
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
600 boost::bind(&test_Jintegrate_against_numdiff, state_type)));
601
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(boost::bind(&test_JintegrateTransport, state_type)));
602
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
603 boost::bind(&test_Jdiff_and_Jintegrate_are_inverses, state_type)));
604
5/10
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
 7 ts->add(BOOST_TEST_CASE(
605 boost::bind(&test_velocity_from_Jintegrate_Jdiff, state_type)));
606
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 framework::master_test_suite().add(ts);
607 7 }
608
609 1 bool init_function() {
610
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 8 for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
611 7 register_state_unit_tests(StateModelTypes::all[i]);
612 }
613 1 return true;
614 }
615
616 1 int main(int argc, char** argv) {
617 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
618 }
619