GCC Code Coverage Report

Directory: ./
File: unittest/test_actuation.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 150 151 99.3%
Branches: 344 674 51.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 18 void test_construct_data(ActuationModelTypes::Type actuation_type,
21 StateModelTypes::Type state_type) {
22 // create the model
23
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 ActuationModelFactory factory;
24 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
25
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 factory.create(actuation_type, state_type);
26
27 // create the corresponding data object
28 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
29
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 model->createData();
30
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
 18 if (!data)
31 ✗ throw std::runtime_error("[test_construct_data] Data pointer is dead.");
32 18 }
33
34 18 void test_calc_returns_tau(ActuationModelTypes::Type actuation_type,
35 StateModelTypes::Type state_type) {
36 // create the model
37
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 ActuationModelFactory factory;
38 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
39
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 factory.create(actuation_type, state_type);
40
41 // create the corresponding data object
42 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data =
43
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 model->createData();
44
45 // Generating random state and control vectors
46
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 const Eigen::VectorXd x = model->get_state()->rand();
47
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
48
49 // Getting the state dimension from calc() call
50
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 model->calc(data, x, u);
51
8/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 18 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 18 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 18 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 18 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 18 times.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_model = model->cast<float>();
57 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
58
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_data = casted_model->createData();
59
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 const Eigen::VectorXf x_f = x.cast<float>();
60
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 const Eigen::VectorXf u_f = u.cast<float>();
61
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 casted_model->calc(casted_data, x_f, u_f);
62 18 float tol_f = std::sqrt(float(2.0) * std::numeric_limits<float>::epsilon());
63
9/18
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 18 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 18 times.
 18 BOOST_CHECK((data->tau.cast<float>() - casted_data->tau).isZero(tol_f));
64 18 }
65
66 18 void test_actuationSet(ActuationModelTypes::Type actuation_type,
67 StateModelTypes::Type state_type) {
68 // create the model
69
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 ActuationModelFactory factory;
70 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
71
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 model->createData();
76
77
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 crocoddyl::ActuationModelNumDiff model_num_diff(model);
78 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
79
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 model_num_diff.createData();
80
81 // Generating random values for the state and control
82
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 Eigen::VectorXd x = model->get_state()->rand();
83
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
84
85 // Computing the selection matrix
86
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 model->calc(data, x, u);
87
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 model_num_diff.calc(data_num_diff, x, u);
88
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 model_num_diff.calcDiff(data_num_diff, x, u);
89
90
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 const std::size_t nv = model->get_state()->get_nv();
91 Eigen::MatrixXd S =
92
3/6
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 data_num_diff->dtau_du * crocoddyl::pseudoInverse(data_num_diff->dtau_du);
93
2/2
✓ Branch 0 taken 358 times.
✓ Branch 1 taken 18 times.
 376 for (std::size_t k = 0; k < nv; ++k) {
94
3/4
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 336 times.
 358 if (fabs(S(k, k)) < std::numeric_limits<double>::epsilon()) {
95
7/14
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 22 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 22 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 22 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 22 times.
 22 BOOST_CHECK(data->tau_set[k] == false);
96 } else {
97
7/14
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 336 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 336 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 336 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 336 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 336 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 336 times.
 336 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_model = model->cast<float>();
104 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
105
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_data = casted_model->createData();
106
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 Eigen::VectorXf x_f = x.cast<float>();
107
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 const Eigen::VectorXf u_f = u.cast<float>();
108
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 casted_model->calc(casted_data, x_f, u_f);
109
2/2
✓ Branch 0 taken 358 times.
✓ Branch 1 taken 18 times.
 376 for (std::size_t k = 0; k < nv; ++k) {
110
8/16
✓ Branch 1 taken 358 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 358 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 358 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 358 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 358 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 358 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 358 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 358 times.
 358 BOOST_CHECK(data->tau_set[k] == casted_data->tau_set[k]);
111 }
112 18 }
113
114 18 void test_partial_derivatives_against_numdiff(
115 ActuationModelTypes::Type actuation_type,
116 StateModelTypes::Type state_type) {
117 // create the model
118
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 ActuationModelFactory factory;
119 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
120
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 model->createData();
125
126
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 crocoddyl::ActuationModelNumDiff model_num_diff(model);
127 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
128
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 model_num_diff.createData();
129
130 // Generating random values for the state and control
131
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 Eigen::VectorXd x = model->get_state()->rand();
132
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
133
134 // Computing the actuation derivatives
135
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 model->calc(data, x, u);
136
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 model->calcDiff(data, x, u);
137
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 model_num_diff.calc(data_num_diff, x, u);
138
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 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 18 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
142
8/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 18 times.
 18 BOOST_CHECK((data->dtau_dx - data_num_diff->dtau_dx).isZero(tol));
143
8/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 18 times.
 18 BOOST_CHECK((data->dtau_du - data_num_diff->dtau_du).isZero(tol));
144
145 // Computing the actuation derivatives
146
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 x = model->get_state()->rand();
147
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 18 model->calc(data, x);
148
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 18 model->calcDiff(data, x);
149
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 model_num_diff.calc(data_num_diff, x);
150
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 model_num_diff.calcDiff(data_num_diff, x);
151
152 // Checking the partial derivatives against numdiff
153
8/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 18 times.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_model = model->cast<float>();
158 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
159
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_data = casted_model->createData();
160
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 Eigen::VectorXf x_f = x.cast<float>();
161
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 const Eigen::VectorXf u_f = u.cast<float>();
162
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 casted_model->calc(casted_data, x_f, u_f);
163
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 casted_model->calcDiff(casted_data, x_f, u_f);
164 18 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
165
9/18
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 18 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 18 times.
 18 BOOST_CHECK(
166 (data->dtau_dx.cast<float>() - casted_data->dtau_dx).isZero(tol_f));
167
9/18
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 18 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 18 times.
 18 BOOST_CHECK(
168 (data->dtau_du.cast<float>() - casted_data->dtau_du).isZero(tol_f));
169
170
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 18 casted_model->calc(casted_data, x_f);
171
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 18 casted_model->calcDiff(casted_data, x_f);
172
9/18
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 18 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 18 times.
 18 BOOST_CHECK(
173 (data->dtau_dx.cast<float>() - casted_data->dtau_dx).isZero(tol_f));
174 18 }
175
176 18 void test_commands(ActuationModelTypes::Type actuation_type,
177 StateModelTypes::Type state_type) {
178 // create the model
179
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 ActuationModelFactory factory;
180 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
181
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 model->createData();
186
187
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 crocoddyl::ActuationModelNumDiff model_num_diff(model);
188 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
189
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 model_num_diff.createData();
190
191 // Generating random values for the state and control
192
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 Eigen::VectorXd x = model->get_state()->rand();
193 const Eigen::VectorXd tau =
194
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Eigen::VectorXd::Random(model->get_state()->get_nv());
195
196 // Computing the actuation commands
197
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 model->commands(data, x, tau);
198
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 model_num_diff.commands(data_num_diff, x, tau);
199
200 // Checking the joint torques
201 18 double tol = sqrt(model_num_diff.get_disturbance());
202
8/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 18 times.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_model = model->cast<float>();
207 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
208
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_data = casted_model->createData();
209
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 Eigen::VectorXf x_f = x.cast<float>();
210
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 const Eigen::VectorXf tau_f = tau.cast<float>();
211
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 casted_model->commands(casted_data, x_f, tau_f);
212 18 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
213
9/18
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 18 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 18 times.
 18 BOOST_CHECK((data->u.cast<float>() - casted_data->u).isZero(tol_f));
214 18 }
215
216 18 void test_torqueTransform(ActuationModelTypes::Type actuation_type,
217 StateModelTypes::Type state_type) {
218 // create the model
219
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 ActuationModelFactory factory;
220 const std::shared_ptr<crocoddyl::ActuationModelAbstract>& model =
221
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 model->createData();
226
227
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 crocoddyl::ActuationModelNumDiff model_num_diff(model);
228 const std::shared_ptr<crocoddyl::ActuationDataAbstract>& data_num_diff =
229
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 18 model_num_diff.createData();
230
231 // Generating random values for the state and control
232
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 Eigen::VectorXd x = model->get_state()->rand();
233
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
234
235 // Computing the torque transform
236
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 model->torqueTransform(data, x, u);
237
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
 18 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 18 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
243
8/16
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 18 times.
 18 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
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_model = model->cast<float>();
248 const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>&
249
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 casted_data = casted_model->createData();
250
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 Eigen::VectorXf x_f = x.cast<float>();
251
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 const Eigen::VectorXf u_f = u.cast<float>();
252
3/6
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
 18 casted_model->torqueTransform(casted_data, x_f, u_f);
253 18 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
254
9/18
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 18 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 18 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 18 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 18 times.
✗ Branch 29 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 18 times.
 18 BOOST_CHECK((data->Mtau.cast<float>() - casted_data->Mtau).isZero(tol_f));
255 18 }
256
257 //----------------------------------------------------------------------------//
258
259 18 void register_actuation_model_unit_tests(
260 ActuationModelTypes::Type actuation_type,
261 StateModelTypes::Type state_type) {
262
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 boost::test_tools::output_test_stream test_name;
263
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 test_name << "test_" << actuation_type << "_" << state_type;
264
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 std::cout << "Running " << test_name.str() << std::endl;
265
4/8
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
 18 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
266
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 ts->add(BOOST_TEST_CASE(
267 boost::bind(&test_construct_data, actuation_type, state_type)));
268
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 ts->add(BOOST_TEST_CASE(
269 boost::bind(&test_calc_returns_tau, actuation_type, state_type)));
270
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 ts->add(BOOST_TEST_CASE(
271 boost::bind(&test_actuationSet, actuation_type, state_type)));
272
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
273 actuation_type, state_type)));
274 36 ts->add(
275
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 BOOST_TEST_CASE(boost::bind(&test_commands, actuation_type, state_type)));
276
5/10
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 ts->add(BOOST_TEST_CASE(
277 boost::bind(&test_torqueTransform, actuation_type, state_type)));
278
3/6
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
 18 framework::master_test_suite().add(ts);
279 18 }
280
281 1 bool init_function() {
282
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 8 for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
283 7 register_actuation_model_unit_tests(ActuationModelTypes::ActuationModelFull,
284 7 StateModelTypes::all[i]);
285 }
286
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 8 for (size_t i = 0; i < StateModelTypes::all.size(); ++i) {
287
4/4
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 2 times.
 13 if (StateModelTypes::all[i] != StateModelTypes::StateVector &&
288
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 StateModelTypes::all[i] != StateModelTypes::StateMultibody_Hector) {
289 5 register_actuation_model_unit_tests(
290 ActuationModelTypes::ActuationModelFloatingBase,
291 5 StateModelTypes::all[i]);
292 5 register_actuation_model_unit_tests(
293 ActuationModelTypes::ActuationModelSquashingFull,
294 5 StateModelTypes::all[i]);
295 }
296 }
297
298 1 register_actuation_model_unit_tests(
299 ActuationModelTypes::ActuationModelFloatingBaseThrusters,
300 StateModelTypes::StateMultibody_Hector);
301 1 return true;
302 }
303
304 1 int main(int argc, char** argv) {
305 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
306 }
307