GCC Code Coverage Report

Directory: ./
File: unittest/test_actuation.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 144 0.0%
Branches: 0 674 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include "factory/actuation.hpp"
13 #include "unittest_common.hpp"
14
15 using namespace boost::unit_test;
16 using namespace crocoddyl::unittest;
17
18 //----------------------------------------------------------------------------//
19
20 ✗ void test_construct_data(ActuationModelTypes::Type actuation_type,
21 StateModelTypes::Type state_type) {
22 // create the model
23 ✗ ActuationModelFactory factory;
24 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
25 ✗ factory.create(actuation_type, state_type);
26
27 // create the corresponding data object
28 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
29 ✗ model->createData();
30 ✗ if (!data)
31 ✗ throw std::runtime_error("[test_construct_data] Data pointer is dead.");
32 }
33
34 ✗ void test_calc_returns_tau(ActuationModelTypes::Type actuation_type,
35 StateModelTypes::Type state_type) {
36 // create the model
37 ✗ ActuationModelFactory factory;
38 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
39 ✗ factory.create(actuation_type, state_type);
40
41 // create the corresponding data object
42 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
43 ✗ model->createData();
44
45 // Generating random state and control vectors
46 ✗ const Eigen::VectorXd x = model->get_state()->rand();
47 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
48
49 // Getting the state dimension from calc() call
50 ✗ model->calc(data, x, u);
51 ✗ BOOST_CHECK(static_cast<std::size_t>(data->tau.size()) ==
52 model->get_state()->get_nv());
53
54 // Checking that casted computation is the same
55 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
56 ✗ casted_model = model->cast<float>();
57 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
58 ✗ casted_data = casted_model->createData();
59 ✗ const Eigen::VectorXf x_f = x.cast<float>();
60 ✗ const Eigen::VectorXf u_f = u.cast<float>();
61 ✗ casted_model->calc(casted_data, x_f, u_f);
62 ✗ float tol_f = std::sqrt(float(2.0) * std::numeric_limits<float>::epsilon());
63 ✗ BOOST_CHECK((data->tau.cast<float>() - casted_data->tau).isZero(tol_f));
64 }
65
66 ✗ void test_actuationSet(ActuationModelTypes::Type actuation_type,
67 StateModelTypes::Type state_type) {
68 // create the model
69 ✗ ActuationModelFactory factory;
70 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
71 ✗ factory.create(actuation_type, state_type);
72
73 // create the corresponding data object and set the cost to nan
74 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
75 ✗ model->createData();
76
77 ✗ crocoddyl::ActuationModelNumDiff model_num_diff(model);
78 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
79 ✗ model_num_diff.createData();
80
81 // Generating random values for the state and control
82 ✗ Eigen::VectorXd x = model->get_state()->rand();
83 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
84
85 // Computing the selection matrix
86 ✗ model->calc(data, x, u);
87 ✗ model_num_diff.calc(data_num_diff, x, u);
88 ✗ model_num_diff.calcDiff(data_num_diff, x, u);
89
90 ✗ const std::size_t nv = model->get_state()->get_nv();
91 Eigen::MatrixXd S =
92 ✗ data_num_diff->dtau_du * crocoddyl::pseudoInverse(data_num_diff->dtau_du);
93 ✗ for (std::size_t k = 0; k < nv; ++k) {
94 ✗ if (fabs(S(k, k)) < std::numeric_limits<double>::epsilon()) {
95 ✗ BOOST_CHECK(data->tau_set[k] == false);
96 } else {
97 ✗ BOOST_CHECK(data->tau_set[k] == true);
98 }
99 }
100
101 // Checking that casted computation is the same
102 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
103 ✗ casted_model = model->cast<float>();
104 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
105 ✗ casted_data = casted_model->createData();
106 ✗ Eigen::VectorXf x_f = x.cast<float>();
107 ✗ const Eigen::VectorXf u_f = u.cast<float>();
108 ✗ casted_model->calc(casted_data, x_f, u_f);
109 ✗ for (std::size_t k = 0; k < nv; ++k) {
110 ✗ BOOST_CHECK(data->tau_set[k] == casted_data->tau_set[k]);
111 }
112 }
113
114 ✗ void test_partial_derivatives_against_numdiff(
115 ActuationModelTypes::Type actuation_type,
116 StateModelTypes::Type state_type) {
117 // create the model
118 ✗ ActuationModelFactory factory;
119 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
120 ✗ factory.create(actuation_type, state_type);
121
122 // create the corresponding data object and set the cost to nan
123 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
124 ✗ model->createData();
125
126 ✗ crocoddyl::ActuationModelNumDiff model_num_diff(model);
127 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
128 ✗ model_num_diff.createData();
129
130 // Generating random values for the state and control
131 ✗ Eigen::VectorXd x = model->get_state()->rand();
132 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
133
134 // Computing the actuation derivatives
135 ✗ model->calc(data, x, u);
136 ✗ model->calcDiff(data, x, u);
137 ✗ model_num_diff.calc(data_num_diff, x, u);
138 ✗ model_num_diff.calcDiff(data_num_diff, x, u);
139 // Tolerance defined as in
140 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
141 ✗ double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
142 ✗ BOOST_CHECK((data->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
143 ✗ BOOST_CHECK((data->dtau_du - data_num_diff->dtau_du).isZero(tol));
144
145 // Computing the actuation derivatives
146 ✗ x = model->get_state()->rand();
147 ✗ model->calc(data, x);
148 ✗ model->calcDiff(data, x);
149 ✗ model_num_diff.calc(data_num_diff, x);
150 ✗ model_num_diff.calcDiff(data_num_diff, x);
151
152 // Checking the partial derivatives against numdiff
153 ✗ BOOST_CHECK((data->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
154
155 // Checking that casted computation is the same
156 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
157 ✗ casted_model = model->cast<float>();
158 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
159 ✗ casted_data = casted_model->createData();
160 ✗ Eigen::VectorXf x_f = x.cast<float>();
161 ✗ const Eigen::VectorXf u_f = u.cast<float>();
162 ✗ casted_model->calc(casted_data, x_f, u_f);
163 ✗ casted_model->calcDiff(casted_data, x_f, u_f);
164 ✗ float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
165 ✗ BOOST_CHECK(
166 (data->dtau_dx.cast<float>() - casted_data->dtau_dx).isZero(tol_f));
167 ✗ BOOST_CHECK(
168 (data->dtau_du.cast<float>() - casted_data->dtau_du).isZero(tol_f));
169
170 ✗ casted_model->calc(casted_data, x_f);
171 ✗ casted_model->calcDiff(casted_data, x_f);
172 ✗ BOOST_CHECK(
173 (data->dtau_dx.cast<float>() - casted_data->dtau_dx).isZero(tol_f));
174 }
175
176 ✗ void test_commands(ActuationModelTypes::Type actuation_type,
177 StateModelTypes::Type state_type) {
178 // create the model
179 ✗ ActuationModelFactory factory;
180 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
181 ✗ factory.create(actuation_type, state_type);
182
183 // create the corresponding data object and set the cost to nan
184 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
185 ✗ model->createData();
186
187 ✗ crocoddyl::ActuationModelNumDiff model_num_diff(model);
188 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
189 ✗ model_num_diff.createData();
190
191 // Generating random values for the state and control
192 ✗ Eigen::VectorXd x = model->get_state()->rand();
193 const Eigen::VectorXd tau =
194 ✗ Eigen::VectorXd::Random(model->get_state()->get_nv());
195
196 // Computing the actuation commands
197 ✗ model->commands(data, x, tau);
198 ✗ model_num_diff.commands(data_num_diff, x, tau);
199
200 // Checking the joint torques
201 ✗ double tol = sqrt(model_num_diff.get_disturbance());
202 ✗ BOOST_CHECK((data->u - data_num_diff->u).isZero(tol));
203
204 // Checking that casted computation is the same
205 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
206 ✗ casted_model = model->cast<float>();
207 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
208 ✗ casted_data = casted_model->createData();
209 ✗ Eigen::VectorXf x_f = x.cast<float>();
210 ✗ const Eigen::VectorXf tau_f = tau.cast<float>();
211 ✗ casted_model->commands(casted_data, x_f, tau_f);
212 ✗ float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
213 ✗ BOOST_CHECK((data->u.cast<float>() - casted_data->u).isZero(tol_f));
214 }
215
216 ✗ void test_torqueTransform(ActuationModelTypes::Type actuation_type,
217 StateModelTypes::Type state_type) {
218 // create the model
219 ✗ ActuationModelFactory factory;
220 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
221 ✗ factory.create(actuation_type, state_type);
222
223 // create the corresponding data object and set the cost to nan
224 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
225 ✗ model->createData();
226
227 ✗ crocoddyl::ActuationModelNumDiff model_num_diff(model);
228 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
229 ✗ model_num_diff.createData();
230
231 // Generating random values for the state and control
232 ✗ Eigen::VectorXd x = model->get_state()->rand();
233 ✗ const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
234
235 // Computing the torque transform
236 ✗ model->torqueTransform(data, x, u);
237 ✗ model_num_diff.torqueTransform(data_num_diff, x, u);
238
239 // Checking the torque transform
240 // Tolerance defined as in
241 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
242 ✗ double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
243 ✗ BOOST_CHECK((data->Mtau - data_num_diff->Mtau).isZero(tol));
244
245 // Checking that casted computation is the same
246 const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>&
247 ✗ casted_model = model->cast<float>();
248 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
249 ✗ casted_data = casted_model->createData();
250 ✗ Eigen::VectorXf x_f = x.cast<float>();
251 ✗ const Eigen::VectorXf u_f = u.cast<float>();
252 ✗ casted_model->torqueTransform(casted_data, x_f, u_f);
253 ✗ float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
254 ✗ BOOST_CHECK((data->Mtau.cast<float>() - casted_data->Mtau).isZero(tol_f));
255 }
256
257 //----------------------------------------------------------------------------//
258
259 ✗ void register_actuation_model_unit_tests(
260 ActuationModelTypes::Type actuation_type,
261 StateModelTypes::Type state_type) {
262 ✗ boost::test_tools::output_test_stream test_name;
263 ✗ test_name << "test_" << actuation_type << "_" << state_type;
264 ✗ std::cout << "Running " << test_name.str() << std::endl;
265 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
266 ✗ ts->add(BOOST_TEST_CASE(
267 boost::bind(&test_construct_data, actuation_type, state_type)));
268 ✗ ts->add(BOOST_TEST_CASE(
269 boost::bind(&test_calc_returns_tau, actuation_type, state_type)));
270 ✗ ts->add(BOOST_TEST_CASE(
271 boost::bind(&test_actuationSet, actuation_type, state_type)));
272 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
273 actuation_type, state_type)));
274 ✗ ts->add(
275 ✗ BOOST_TEST_CASE(boost::bind(&test_commands, actuation_type, state_type)));
276 ✗ ts->add(BOOST_TEST_CASE(
277 boost::bind(&test_torqueTransform, actuation_type, state_type)));
278 ✗ framework::master_test_suite().add(ts);
279 }
280
281 ✗ bool init_function() {
282 ✗ for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
283 ✗ register_actuation_model_unit_tests(ActuationModelTypes::ActuationModelFull,
284 ✗ StateModelTypes::all[i]);
285 }
286 ✗ for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
287 ✗ if (StateModelTypes::all[i] != StateModelTypes::StateVector &&
288 ✗ StateModelTypes::all[i] != StateModelTypes::StateMultibody_Hector) {
289 ✗ register_actuation_model_unit_tests(
290 ActuationModelTypes::ActuationModelFloatingBase,
291 ✗ StateModelTypes::all[i]);
292 ✗ register_actuation_model_unit_tests(
293 ActuationModelTypes::ActuationModelSquashingFull,
294 ✗ StateModelTypes::all[i]);
295 }
296 }
297
298 ✗ register_actuation_model_unit_tests(
299 ActuationModelTypes::ActuationModelFloatingBaseThrusters,
300 StateModelTypes::StateMultibody_Hector);
301 ✗ return true;
302 }
303
304 ✗ int main(int argc, char** argv) {
305 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
306 }
307