GCC Code Coverage Report

Directory: ./
File: unittest/test_diff_actions.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 107 111 96.4%
Branches: 299 642 46.6%
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, INRIA, University of
6 // Oxford, 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/diff_action.hpp"
15 #include "unittest_common.hpp"
16
17 using namespace boost::unit_test;
18 using namespace crocoddyl::unittest;
19
20 //----------------------------------------------------------------------------//
21
22 23 void test_check_data(DifferentialActionModelTypes::Type action_type) {
23 // create the model
24
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 DifferentialActionModelFactory factory;
25 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
26
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 factory.create(action_type);
27
28 // Run the print function
29
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 std::ostringstream tmp;
30
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 tmp << *model;
31
32 // create the corresponding data object
33 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
34
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 model->createData();
35
36
7/14
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 23 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 23 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 23 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 23 times.
 23 BOOST_CHECK(model->checkData(data));
37
38 // Checking that casted computation is the same
39 #ifdef NDEBUG // Run only in release mode
40 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
41 casted_model = model->cast<float>();
42 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
43 casted_data = casted_model->createData();
44 BOOST_CHECK(casted_model->checkData(casted_data));
45 #endif
46 23 }
47
48 23 void test_calc_returns_state(DifferentialActionModelTypes::Type action_type) {
49 // create the model
50
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 DifferentialActionModelFactory factory;
51 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
52
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 factory.create(action_type);
53
54 // create the corresponding data object
55 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
56
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 model->createData();
57
58 // Generating random state and control vectors
59
2/4
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
 23 const Eigen::VectorXd x = model->get_state()->rand();
60
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
61
62 // Getting the state dimension from calc() call
63
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 model->calc(data, x, u);
64
65
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 23 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 23 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 23 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 23 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 23 times.
 23 BOOST_CHECK(static_cast<std::size_t>(data->xout.size()) ==
66 model->get_state()->get_nv());
67
68 // Checking that casted computation is the same
69 #ifdef NDEBUG // Run only in release mode
70 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
71 casted_model = model->cast<float>();
72 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
73 casted_data = casted_model->createData();
74 const Eigen::VectorXf x_f = x.cast<float>();
75 const Eigen::VectorXf u_f = u.cast<float>();
76 casted_model->calc(casted_data, x_f, u_f);
77 BOOST_CHECK(static_cast<std::size_t>(casted_data->xout.size()) ==
78 casted_model->get_state()->get_nv());
79 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
80 BOOST_CHECK((data->xout.cast<float>() - casted_data->xout).isZero(tol_f));
81 #endif
82 23 }
83
84 23 void test_calc_returns_a_cost(DifferentialActionModelTypes::Type action_type) {
85 // create the model
86
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 DifferentialActionModelFactory factory;
87 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
88
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 factory.create(action_type);
89
90 // create the corresponding data object and set the cost to nan
91 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
92
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 model->createData();
93 23 data->cost = nan("");
94
95 // Getting the cost value computed by calc()
96
2/4
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
 23 const Eigen::VectorXd x = model->get_state()->rand();
97
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
98
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 model->calc(data, x, u);
99
100 // Checking that calc returns a cost value
101
6/12
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 23 times.
 23 BOOST_CHECK(!std::isnan(data->cost));
102
103 // Checking that casted computation is the same
104 #ifdef NDEBUG // Run only in release mode
105 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
106 casted_model = model->cast<float>();
107 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
108 casted_data = casted_model->createData();
109 casted_data->cost = float(nan(""));
110 const Eigen::VectorXf x_f = x.cast<float>();
111 const Eigen::VectorXf u_f = u.cast<float>();
112 casted_model->calc(casted_data, x_f, u_f);
113 BOOST_CHECK(!std::isnan(casted_data->cost));
114 float tol_f = 50.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
115 BOOST_CHECK(std::abs(float(data->cost) - casted_data->cost) <= tol_f);
116 #endif
117 23 }
118
119 23 void test_quasi_static(DifferentialActionModelTypes::Type action_type) {
120
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 22 times.
 23 if (action_type ==
121 DifferentialActionModelTypes::
122 DifferentialActionModelFreeFwdDynamics_TalosArm_Squashed)
123 1 return;
124 // create the model
125
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 DifferentialActionModelFactory factory;
126 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
127
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 factory.create(action_type, false);
128
129 // create the corresponding data object and set the cost to nan
130 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
131
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 22 model->createData();
132
133 // Getting the cost value computed by calc()
134
2/4
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
 22 Eigen::VectorXd x = model->get_state()->rand();
135
4/8
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 22 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 22 times.
✗ Branch 13 not taken.
 22 x.tail(model->get_state()->get_nv()).setZero();
136
3/6
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
 22 Eigen::VectorXd u = Eigen::VectorXd::Zero(model->get_nu());
137
3/6
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
 22 model->quasiStatic(data, u, x);
138
3/6
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 22 times.
✗ Branch 9 not taken.
 22 model->calc(data, x, u);
139
140 // Check for inactive contacts
141
2/2
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 1 times.
 22 if (action_type == DifferentialActionModelTypes::
142
2/2
✓ Branch 0 taken 20 times.
✓ Branch 1 taken 1 times.
 21 DifferentialActionModelContactFwdDynamics_HyQ ||
143 action_type ==
144 DifferentialActionModelTypes::
145
2/2
✓ Branch 0 taken 19 times.
✓ Branch 1 taken 1 times.
 20 DifferentialActionModelContactFwdDynamicsWithFriction_HyQ ||
146 action_type == DifferentialActionModelTypes::
147
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 1 times.
 19 DifferentialActionModelContactFwdDynamics_Talos ||
148 action_type ==
149 DifferentialActionModelTypes::
150
2/2
✓ Branch 0 taken 17 times.
✓ Branch 1 taken 1 times.
 18 DifferentialActionModelContactFwdDynamicsWithFriction_Talos ||
151 action_type == DifferentialActionModelTypes::
152
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 1 times.
 17 DifferentialActionModelContactInvDynamics_HyQ ||
153 action_type ==
154 DifferentialActionModelTypes::
155
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 1 times.
 16 DifferentialActionModelContactInvDynamicsWithFriction_HyQ ||
156 action_type == DifferentialActionModelTypes::
157
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 14 times.
 15 DifferentialActionModelContactInvDynamics_Talos ||
158 action_type ==
159 DifferentialActionModelTypes::
160 DifferentialActionModelContactInvDynamicsWithFriction_Talos) {
161 std::shared_ptr<crocoddyl::DifferentialActionModelContactFwdDynamics> m =
162 std::static_pointer_cast<
163 8 crocoddyl::DifferentialActionModelContactFwdDynamics>(model);
164
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 8 m->get_contacts()->changeContactStatus("lf", false);
165
166
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 8 model->quasiStatic(data, u, x);
167
3/6
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
 8 model->calc(data, x, u);
168
169 // Checking that the acceleration is zero as supposed to be in a quasi
170 // static condition
171
7/14
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 8 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 8 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 8 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 8 times.
 8 BOOST_CHECK(data->xout.norm() <= 1e-8);
172
173 // Checking that casted computation is the same
174 #ifdef NDEBUG // Run only in release mode
175 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
176 casted_model = model->cast<float>();
177 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
178 casted_data = casted_model->createData();
179 Eigen::VectorXf x_f = x.cast<float>();
180 x_f.tail(casted_model->get_state()->get_nv()).setZero();
181 Eigen::VectorXf u_f = Eigen::VectorXf::Zero(casted_model->get_nu());
182 casted_model->quasiStatic(casted_data, u_f, x_f);
183 casted_model->calc(casted_data, x_f, u_f);
184 float tol_f =
185 50.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
186 BOOST_CHECK(casted_data->xout.norm() <= tol_f);
187 BOOST_CHECK((data->xout.cast<float>() - casted_data->xout).isZero(tol_f));
188 #endif
189 8 }
190 22 }
191
192 23 void test_partial_derivatives_against_numdiff(
193 DifferentialActionModelTypes::Type action_type) {
194 // create the model
195
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 DifferentialActionModelFactory factory;
196 std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> model =
197
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 factory.create(action_type);
198
199 // create the corresponding data object and set the cost to nan
200 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data =
201
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 model->createData();
202
203
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
204 std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> data_num_diff =
205
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 model_num_diff.createData();
206
207 // Generating random values for the state and control
208
2/4
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
 23 Eigen::VectorXd x = model->get_state()->rand();
209
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
210
211 // Computing the action derivatives
212
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 model->calc(data, x, u);
213
3/6
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 23 model->calcDiff(data, x, u);
214
3/6
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
 23 model_num_diff.calc(data_num_diff, x, u);
215
3/6
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
 23 model_num_diff.calcDiff(data_num_diff, x, u);
216 // Tolerance defined as in
217 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
218
1/2
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
 23 double tol = 2. * std::pow(model_num_diff.get_disturbance(), 1. / 3.);
219
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->h - data_num_diff->h).isZero(tol));
220
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->g - data_num_diff->g).isZero(tol));
221
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(tol));
222
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(tol));
223
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
224
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
225
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
 23 if (model_num_diff.get_with_gauss_approx()) {
226 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
227 ✗ BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
228 ✗ BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
229 }
230
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
231
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
232
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
233
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
234
235 // Computing the action derivatives
236
2/4
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
 23 x = model->get_state()->rand();
237
2/4
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
 23 model->calc(data, x);
238
2/4
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
 23 model->calcDiff(data, x);
239
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 model_num_diff.calc(data_num_diff, x);
240
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 model_num_diff.calcDiff(data_num_diff, x);
241
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->h - data_num_diff->h).isZero(tol));
242
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->g - data_num_diff->g).isZero(tol));
243
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
244
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
 23 if (model_num_diff.get_with_gauss_approx()) {
245 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
246 }
247
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
248
8/16
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 23 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 23 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 23 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 23 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 23 times.
 23 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
249
250 // Checking that casted computation is the same
251 #ifdef NDEBUG // Run only in release mode
252 std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
253 casted_model = model->cast<float>();
254 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
255 casted_data = casted_model->createData();
256 const Eigen::VectorXf x_f = x.cast<float>();
257 const Eigen::VectorXf u_f = u.cast<float>();
258 model->calc(data, x, u);
259 model->calcDiff(data, x, u);
260 casted_model->calc(casted_data, x_f, u_f);
261 casted_model->calcDiff(casted_data, x_f, u_f);
262 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
263 BOOST_CHECK((data->h.cast<float>() - casted_data->h).isZero(tol_f));
264 BOOST_CHECK((data->g.cast<float>() - casted_data->g).isZero(tol_f));
265 BOOST_CHECK((data->Fx.cast<float>() - casted_data->Fx).isZero(tol_f));
266 BOOST_CHECK((data->Fu.cast<float>() - casted_data->Fu).isZero(tol_f));
267 BOOST_CHECK((data->Lx.cast<float>() - casted_data->Lx).isZero(tol_f));
268 BOOST_CHECK((data->Lu.cast<float>() - casted_data->Lu).isZero(tol_f));
269 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
270 BOOST_CHECK((data->Gu.cast<float>() - casted_data->Gu).isZero(tol_f));
271 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
272 BOOST_CHECK((data->Hu.cast<float>() - casted_data->Hu).isZero(tol_f));
273 crocoddyl::DifferentialActionModelNumDiffTpl<float> casted_model_num_diff =
274 model_num_diff.cast<float>();
275 std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
276 casted_data_num_diff = casted_model_num_diff.createData();
277 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
278 casted_model_num_diff.calcDiff(casted_data_num_diff, x_f, u_f);
279 tol_f = 80.0f * sqrt(casted_model_num_diff.get_disturbance());
280 BOOST_CHECK((casted_data->Gx - casted_data_num_diff->Gx).isZero(tol_f));
281 BOOST_CHECK((casted_data->Gu - casted_data_num_diff->Gu).isZero(tol_f));
282 BOOST_CHECK((casted_data->Hx - casted_data_num_diff->Hx).isZero(tol_f));
283 BOOST_CHECK((casted_data->Hu - casted_data_num_diff->Hu).isZero(tol_f));
284 #endif
285 23 }
286
287 //----------------------------------------------------------------------------//
288
289 23 void register_action_model_unit_tests(
290 DifferentialActionModelTypes::Type action_type) {
291
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 boost::test_tools::output_test_stream test_name;
292
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 test_name << "test_" << action_type;
293
4/8
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 23 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 23 times.
✗ Branch 11 not taken.
 23 std::cout << "Running " << test_name.str() << std::endl;
294
4/8
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
 23 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
295
5/10
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 23 times.
✗ Branch 15 not taken.
 23 ts->add(BOOST_TEST_CASE(boost::bind(&test_check_data, action_type)));
296
5/10
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 23 times.
✗ Branch 15 not taken.
 23 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, action_type)));
297
5/10
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 23 times.
✗ Branch 15 not taken.
 23 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, action_type)));
298
5/10
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 23 times.
✗ Branch 15 not taken.
 23 ts->add(BOOST_TEST_CASE(
299 boost::bind(&test_partial_derivatives_against_numdiff, action_type)));
300
5/10
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 23 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 23 times.
✗ Branch 15 not taken.
 23 ts->add(BOOST_TEST_CASE(boost::bind(&test_quasi_static, action_type)));
301
3/6
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
 23 framework::master_test_suite().add(ts);
302 23 }
303
304 1 bool init_function() {
305
2/2
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 1 times.
 24 for (size_t i = 0; i < DifferentialActionModelTypes::all.size(); ++i) {
306 23 register_action_model_unit_tests(DifferentialActionModelTypes::all[i]);
307 }
308 // register_action_model_unit_tests(DifferentialActionModelTypes::DifferentialActionModelContactInvDynamicsWithFriction_Talos);
309 // register_action_model_unit_tests(DifferentialActionModelTypes::DifferentialActionModelContactInvDynamics_TalosArm);
310 // register_action_model_unit_tests(DifferentialActionModelTypes::DifferentialActionModelContactInvDynamics_HyQ);
311 1 return true;
312 }
313
314 1 int main(int argc, char** argv) {
315 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
316 }
317