GCC Code Coverage Report

Directory: ./
File: unittest/test_residuals.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 165 165 100.0%
Branches: 292 568 51.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2024, 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 "crocoddyl/core/residuals/control.hpp"
13 #include "crocoddyl/core/residuals/joint-acceleration.hpp"
14 #include "crocoddyl/core/residuals/joint-effort.hpp"
15 #include "crocoddyl/multibody/data/multibody.hpp"
16 #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp"
17 #include "crocoddyl/multibody/residuals/com-position.hpp"
18 #include "crocoddyl/multibody/residuals/state.hpp"
19 #include "factory/actuation.hpp"
20 #include "factory/residual.hpp"
21 #include "unittest_common.hpp"
22
23 using namespace boost::unit_test;
24 using namespace crocoddyl::unittest;
25
26 //----------------------------------------------------------------------------//
27
28 180 void test_calc_returns_a_residual(ResidualModelTypes::Type residual_type,
29 StateModelTypes::Type state_type,
30 ActuationModelTypes::Type actuation_type) {
31 // Create the model
32
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 ResidualModelFactory residual_factory;
33
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 ActuationModelFactory actuation_factory;
34 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
35
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 actuation_factory.create(actuation_type, state_type);
36 const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
37 residual_factory.create(residual_type, state_type,
38
1/2
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
 180 actuation_model->get_nu());
39
40 // Run the print function
41
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 std::ostringstream tmp;
42
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 tmp << *model;
43
44 // Create the corresponding shared data
45 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
46 180 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
47 180 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
48
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 pinocchio::Data pinocchio_data(pinocchio_model);
49 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
50
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_model->createData();
51 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
52
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_data);
53
54 // create the residual data
55 const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
56
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 model->createData(&shared_data);
57
58 // Generating random values for the state and control
59
1/2
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 const Eigen::VectorXd x = model->get_state()->rand();
60
2/4
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
 180 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
61
62 // Compute all the pinocchio function needed for the models.
63
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
64
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u);
65
66 // Getting the residual value computed by calc()
67
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 data->r *= nan("");
68
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 model->calc(data, x, u);
69
70 // Checking that calc returns a residual value
71
2/2
✓ Branch 2 taken 2090 times.
✓ Branch 3 taken 180 times.
 2270 for (std::size_t i = 0; i < model->get_nr(); ++i)
72
7/14
✓ Branch 1 taken 2090 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2090 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2090 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2090 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 2090 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2090 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 2090 times.
 2090 BOOST_CHECK(!std::isnan(data->r(i)));
73 180 }
74
75 180 void test_calc_against_numdiff(ResidualModelTypes::Type residual_type,
76 StateModelTypes::Type state_type,
77 ActuationModelTypes::Type actuation_type) {
78 using namespace boost::placeholders;
79
80 // Create the model
81
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 ResidualModelFactory residual_factory;
82
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 ActuationModelFactory actuation_factory;
83 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
84
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 actuation_factory.create(actuation_type, state_type);
85 const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
86 residual_factory.create(residual_type, state_type,
87
1/2
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
 180 actuation_model->get_nu());
88
89 // Create the corresponding shared data
90 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
91 180 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
92 180 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
93
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 pinocchio::Data pinocchio_data(pinocchio_model);
94 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
95
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_model->createData();
96 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
97
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_data);
98
99 // Create the residual data
100 const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
101
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 model->createData(&shared_data);
102
103 // Create the equivalent num diff model and data.
104
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 crocoddyl::ResidualModelNumDiff model_num_diff(model);
105 const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff =
106
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 model_num_diff.createData(&shared_data);
107
108 // Generating random values for the state and control
109
1/2
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 const Eigen::VectorXd x = model->get_state()->rand();
110
2/4
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
 180 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
111
112 // Computing the residual
113
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
114
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 actuation_model->calc(actuation_data, x, u);
115
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 model->calc(data, x, u);
116
117 // Computing the residual from num diff
118 180 std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals;
119
3/6
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
 180 reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
120 &pinocchio_model, &pinocchio_data, _1, _2));
121
3/6
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 180 times.
✗ Branch 10 not taken.
 180 reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation,
122 actuation_model, actuation_data, _1, _2));
123
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 model_num_diff.set_reevals(reevals);
124
3/6
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
 180 model_num_diff.calc(data_num_diff, x, u);
125
126 // Checking the partial derivatives against NumDiff
127
7/14
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 180 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 180 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 180 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 180 times.
 180 BOOST_CHECK(data->r == data_num_diff->r);
128 180 }
129
130 180 void test_partial_derivatives_against_numdiff(
131 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
132 ActuationModelTypes::Type actuation_type) {
133 using namespace boost::placeholders;
134
135 // Create the model
136
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 ResidualModelFactory residual_factory;
137
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 ActuationModelFactory actuation_factory;
138 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
139
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 actuation_factory.create(actuation_type, state_type);
140 const boost::shared_ptr<crocoddyl::ResidualModelAbstract>& model =
141 residual_factory.create(residual_type, state_type,
142
1/2
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
 180 actuation_model->get_nu());
143
144 // Create the corresponding shared data
145 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
146 180 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
147 180 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
148
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 pinocchio::Data pinocchio_data(pinocchio_model);
149 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
150
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_model->createData();
151 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
152
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_data);
153
154 // Create the residual data
155 const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data =
156
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 model->createData(&shared_data);
157
158 // Create the equivalent num diff model and data.
159
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 crocoddyl::ResidualModelNumDiff model_num_diff(model);
160 const boost::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff =
161
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 model_num_diff.createData(&shared_data);
162
163 // Generating random values for the state and control
164
1/2
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 Eigen::VectorXd x = model->get_state()->rand();
165
2/4
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
 180 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
166
167 // Computing the residual derivatives
168
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
169
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 actuation_model->calc(actuation_data, x, u);
170
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 actuation_model->calcDiff(actuation_data, x, u);
171
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 model->calc(data, x, u);
172
3/6
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
 180 model->calcDiff(data, x, u);
173
174 // Computing the residual derivatives via numerical differentiation
175 180 std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals;
176
3/6
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
 180 reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
177 &pinocchio_model, &pinocchio_data, _1, _2));
178
3/6
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 180 times.
✗ Branch 10 not taken.
 180 reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation,
179 actuation_model, actuation_data, _1, _2));
180
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 model_num_diff.set_reevals(reevals);
181
3/6
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
 180 model_num_diff.calc(data_num_diff, x, u);
182
3/6
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
 180 model_num_diff.calcDiff(data_num_diff, x, u);
183
184 // Checking the partial derivatives against numdiff
185 // Tolerance defined as in
186 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
187 180 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
188
8/16
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 180 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 180 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 180 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 180 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 180 times.
 180 BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol));
189
8/16
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 180 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 180 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 180 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 180 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 180 times.
 180 BOOST_CHECK((data->Ru - data_num_diff->Ru).isZero(tol));
190
191 // Computing the residual derivatives
192
1/2
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 x = model->get_state()->rand();
193
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
194
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_model->calc(actuation_data, x);
195
1/2
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
 180 actuation_model->calcDiff(actuation_data, x);
196
197 // Computing the residual derivatives via numerical differentiation
198
2/4
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
 180 model->calc(data, x);
199
2/4
✓ Branch 2 taken 180 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
 180 model->calcDiff(data, x);
200
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 model_num_diff.calc(data_num_diff, x);
201
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 model_num_diff.calcDiff(data_num_diff, x);
202
203 // Checking the partial derivatives against numdiff
204
8/16
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 180 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 180 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 180 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 180 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 180 times.
 180 BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol));
205 180 }
206
207 1 void test_reference() {
208
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ResidualModelFactory factory;
209 1 StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos;
210 1 ActuationModelTypes::Type actuation_type =
211 ActuationModelTypes::ActuationModelFloatingBase;
212
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 StateModelFactory state_factory;
213
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ActuationModelFactory actuation_factory;
214 boost::shared_ptr<crocoddyl::StateMultibody> state =
215 boost::static_pointer_cast<crocoddyl::StateMultibody>(
216
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_factory.create(state_type));
217 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
218
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 actuation_factory.create(actuation_type, state_type);
219
220 1 const std::size_t nu = actuation->get_nu();
221 1 const std::size_t nv = state->get_nv();
222
223 // Test reference in state residual
224
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 crocoddyl::ResidualModelState state_residual(state, state->rand(), nu);
225
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 Eigen::VectorXd x_ref = state_residual.get_state()->rand();
226
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 state_residual.set_reference(x_ref);
227
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 1 BOOST_CHECK((x_ref - state_residual.get_reference()).isZero());
228
229 // Test reference in control residual
230
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 crocoddyl::ResidualModelControl control_residual(state, nu);
231
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::VectorXd u_ref = Eigen::VectorXd::Random(nu);
232
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 control_residual.set_reference(u_ref);
233
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 1 BOOST_CHECK((u_ref - control_residual.get_reference()).isZero());
234
235 // Test reference in joint-acceleration residual
236
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 crocoddyl::ResidualModelJointAcceleration jacc_residual(state, nu);
237
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::VectorXd a_ref = Eigen::VectorXd::Random(nv);
238
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jacc_residual.set_reference(a_ref);
239
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 1 BOOST_CHECK((a_ref - jacc_residual.get_reference()).isZero());
240
241 // Test reference in joint-effort residual
242
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 crocoddyl::ResidualModelJointEffort jeff_residual(state, actuation, nu);
243
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::VectorXd tau_ref = Eigen::VectorXd::Random(nu);
244
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 jeff_residual.set_reference(tau_ref);
245
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 1 BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero());
246
247 // Test reference in centroidal-momentum residual
248 crocoddyl::ResidualModelCentroidalMomentum cmon_residual(
249
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 state, Eigen::Matrix<double, 6, 1>::Zero());
250
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::Matrix<double, 6, 1> h_ref = Eigen::Matrix<double, 6, 1>::Random();
251
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 cmon_residual.set_reference(h_ref);
252
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 1 BOOST_CHECK((h_ref - cmon_residual.get_reference()).isZero());
253
254 // Test reference in com-position residual
255 crocoddyl::ResidualModelCoMPosition c_residual(state,
256
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 1 Eigen::Vector3d::Zero());
257
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Eigen::Vector3d c_ref = Eigen::Vector3d::Random();
258
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 c_residual.set_reference(c_ref);
259
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 1 times.
 1 BOOST_CHECK((c_ref - c_residual.get_reference()).isZero());
260 1 }
261
262 //----------------------------------------------------------------------------//
263
264 180 void register_residual_model_unit_tests(
265 ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type,
266 ActuationModelTypes::Type actuation_type) {
267
2/4
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
 180 boost::test_tools::output_test_stream test_name;
268
5/10
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 180 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 180 times.
✗ Branch 14 not taken.
 180 test_name << "test_" << residual_type << "_" << state_type << "_"
269
1/2
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
 180 << actuation_type;
270
4/8
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 180 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 180 times.
✗ Branch 11 not taken.
 180 std::cout << "Running " << test_name.str() << std::endl;
271
4/8
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
 180 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
272 360 ts->add(
273
5/10
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 180 times.
✗ Branch 15 not taken.
 180 BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_residual, residual_type,
274 state_type, actuation_type)));
275
5/10
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 180 times.
✗ Branch 15 not taken.
 180 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_against_numdiff, residual_type,
276 state_type, actuation_type)));
277 360 ts->add(
278
5/10
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 180 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 180 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 180 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 180 times.
✗ Branch 15 not taken.
 180 BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
279 residual_type, state_type, actuation_type)));
280
3/6
✓ Branch 1 taken 180 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 180 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 180 times.
✗ Branch 7 not taken.
 180 framework::master_test_suite().add(ts);
281 180 }
282
283 1 void regiter_residual_reference_unit_tests() {
284
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 boost::test_tools::output_test_stream test_name;
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 test_name << "test_reference";
286
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 std::cout << "Running " << test_name.str() << std::endl;
287
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
288
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 ts->add(BOOST_TEST_CASE(boost::bind(&test_reference)));
289
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 framework::master_test_suite().add(ts);
290 1 }
291
292 1 bool init_function() {
293 // Test all residuals available with all the activation types with all
294 // available states types.
295
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
 11 for (size_t residual_type = 0; residual_type < ResidualModelTypes::all.size();
296 ++residual_type) {
297 50 for (size_t state_type =
298 10 StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
299
2/2
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 10 times.
 60 state_type < StateModelTypes::all.size(); ++state_type) {
300 250 for (size_t actuation_type = 0;
301
2/2
✓ Branch 1 taken 200 times.
✓ Branch 2 taken 50 times.
 250 actuation_type < ActuationModelTypes::all.size(); ++actuation_type) {
302
2/2
✓ Branch 1 taken 150 times.
✓ Branch 2 taken 50 times.
 200 if (ActuationModelTypes::all[actuation_type] !=
303 ActuationModelTypes::ActuationModelFloatingBaseThrusters) {
304 150 register_residual_model_unit_tests(
305 150 ResidualModelTypes::all[residual_type],
306 150 StateModelTypes::all[state_type],
307 150 ActuationModelTypes::all[actuation_type]);
308 50 } else if (StateModelTypes::all[state_type] !=
309
4/4
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 30 times.
✓ Branch 3 taken 20 times.
 90 StateModelTypes::StateMultibody_TalosArm &&
310
2/2
✓ Branch 1 taken 30 times.
✓ Branch 2 taken 10 times.
 40 StateModelTypes::all[state_type] !=
311 StateModelTypes::StateMultibodyContact2D_TalosArm) {
312 30 register_residual_model_unit_tests(
313 30 ResidualModelTypes::all[residual_type],
314 30 StateModelTypes::all[state_type],
315 30 ActuationModelTypes::all[actuation_type]);
316 }
317 }
318 }
319 }
320 1 regiter_residual_reference_unit_tests();
321 1 return true;
322 }
323
324 1 int main(int argc, char** argv) {
325 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
326 }
327