GCC Code Coverage Report

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