GCC Code Coverage Report

Directory: ./
File: unittest/test_costs_noFF.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 151 155 97.4%
Branches: 322 696 46.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, LAAS-CNRS, New York University,
5 // Max Planck Gesellschaft, University of Edinburgh,
6 // INRIA
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 "crocoddyl/core/actuation-base.hpp"
15 #include "crocoddyl/multibody/actuations/full.hpp"
16 #include "crocoddyl/multibody/data/multibody.hpp"
17 #include "factory/actuation.hpp"
18 #include "factory/cost.hpp"
19 #include "unittest_common.hpp"
20
21 using namespace boost::unit_test;
22 using namespace crocoddyl::unittest;
23
24 //----------------------------------------------------------------------------//
25
26 9 void test_calc_returns_a_cost(CostModelNoFFTypes::Type cost_type,
27 ActivationModelTypes::Type activation_type) {
28 // create the model
29
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 CostModelFactory factory;
30 const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
31
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 factory.create(cost_type, activation_type);
32
33 // Run the print function
34
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 std::ostringstream tmp;
35
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 tmp << *model;
36
37 // create the corresponding data object
38 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
39 9 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
40 9 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
41
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 pinocchio::Data pinocchio_data(pinocchio_model);
42
43 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
44
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 boost::make_shared<crocoddyl::ActuationModelFull>(state);
45 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
46
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation->createData();
47 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
48
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_data);
49 const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
50
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model->createData(&shared_data);
51 9 data->cost = nan("");
52
53 // Generating random values for the state and control
54
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 const Eigen::VectorXd x = model->get_state()->rand();
55
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
56
57 // Compute all the pinocchio function needed for the models.
58
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
59
60 // Getting the cost value computed by calc()
61
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 model->calc(data, x, u);
62
63 // Checking that calc returns a cost value
64
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 9 times.
 9 BOOST_CHECK(!std::isnan(data->cost));
65 9 }
66
67 9 void test_calc_against_numdiff(CostModelNoFFTypes::Type cost_type,
68 ActivationModelTypes::Type activation_type) {
69 // create the model
70
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 CostModelFactory factory;
71 const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
72
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 factory.create(cost_type, activation_type);
73
74 // create the corresponding data object
75 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
76 9 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
77 9 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
78
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 pinocchio::Data pinocchio_data(pinocchio_model);
79
80 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
81
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 boost::make_shared<crocoddyl::ActuationModelFull>(state);
82 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
83
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation->createData();
84 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
85
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_data);
86 const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
87
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model->createData(&shared_data);
88
89 // Create the equivalent num diff model and data.
90
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 crocoddyl::CostModelNumDiff model_num_diff(model);
91 const boost::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
92
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 model_num_diff.createData(&shared_data);
93
94 // Generating random values for the state and control
95
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 const Eigen::VectorXd x = model->get_state()->rand();
96
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
97
98 // Compute all the pinocchio function needed for the models.
99
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
100
101 // Computing the cost derivatives
102
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 model->calc(data, x, u);
103
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 model_num_diff.calc(data_num_diff, x, u);
104
105 // Checking the partial derivatives against NumDiff
106
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 9 times.
 9 BOOST_CHECK(data->cost == data_num_diff->cost);
107 9 }
108
109 9 void test_partial_derivatives_against_numdiff(
110 CostModelNoFFTypes::Type cost_type,
111 ActivationModelTypes::Type activation_type) {
112 using namespace boost::placeholders;
113
114 // create the model
115
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 CostModelFactory factory;
116 const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
117
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 factory.create(cost_type, activation_type);
118
119 // create the corresponding data object
120 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
121 9 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
122 9 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
123
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 pinocchio::Data pinocchio_data(pinocchio_model);
124
125 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model =
126
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 boost::make_shared<crocoddyl::ActuationModelFull>(state);
127 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
128
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_model->createData();
129 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
130
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_data);
131 const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
132
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model->createData(&shared_data);
133
134 // Create the equivalent num diff model and data.
135
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 crocoddyl::CostModelNumDiff model_num_diff(model);
136 const boost::shared_ptr<crocoddyl::CostDataAbstract>& data_num_diff =
137
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 model_num_diff.createData(&shared_data);
138
139 // Generating random values for the state and control
140
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 Eigen::VectorXd x = model->get_state()->rand();
141
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
142
143 // Compute all the pinocchio function needed for the models.
144
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
145
146 // set the function that needs to be called at every step of the numdiff
147 9 std::vector<crocoddyl::CostModelNumDiff::ReevaluationFunction> reevals;
148
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 reevals.push_back(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
149 &pinocchio_model, &pinocchio_data, _1, _2));
150
3/6
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9 times.
✗ Branch 10 not taken.
 9 reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation,
151 actuation_model, actuation_data, _1, _2));
152
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 model_num_diff.set_reevals(reevals);
153
154 // Computing the cost derivatives
155
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 actuation_model->calc(actuation_data, x, u);
156
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 actuation_model->calcDiff(actuation_data, x, u);
157
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 model->calc(data, x, u);
158
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 model->calcDiff(data, x, u);
159
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 model_num_diff.calc(data_num_diff, x, u);
160
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 model_num_diff.calcDiff(data_num_diff, x, u);
161 // Tolerance defined as in
162 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
163 9 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
164
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
165
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
166
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 if (model_num_diff.get_with_gauss_approx()) {
167
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
168
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
169
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
170 } else {
171 ✗ BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
172 ✗ BOOST_CHECK((data_num_diff->Lxu).isZero(tol));
173 ✗ BOOST_CHECK((data_num_diff->Luu).isZero(tol));
174 }
175
176 // Computing the cost derivatives
177
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 x = model->get_state()->rand();
178
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
179
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_model->calc(actuation_data, x);
180
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_model->calcDiff(actuation_data, x);
181
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 9 model->calc(data, x);
182
2/4
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
 9 model->calcDiff(data, x);
183
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 model_num_diff.calc(data_num_diff, x);
184
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 model_num_diff.calcDiff(data_num_diff, x);
185
186 // Checking the partial derivatives against numdiff
187
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
188
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 if (model_num_diff.get_with_gauss_approx()) {
189
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 9 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 9 times.
 9 BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
190 } else {
191 ✗ BOOST_CHECK((data_num_diff->Lxx).isZero(tol));
192 }
193 9 }
194
195 9 void test_dimensions_in_cost_sum(CostModelNoFFTypes::Type cost_type,
196 ActivationModelTypes::Type activation_type) {
197 // create the model
198
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 CostModelFactory factory;
199 const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
200
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 factory.create(cost_type, activation_type);
201
202 // create the corresponding data object
203 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
204 9 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
205 9 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
206
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 pinocchio::Data pinocchio_data(pinocchio_model);
207
208 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
209
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 boost::make_shared<crocoddyl::ActuationModelFull>(state);
210 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
211
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation->createData();
212 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
213
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_data);
214
215 // create the cost sum model
216 9 crocoddyl::CostModelSum cost_sum(state, model->get_nu());
217
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 cost_sum.addCost("myCost", model, 1.);
218
219 // Generating random values for the state and control
220
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 const Eigen::VectorXd& x = state->rand();
221
222 // Compute all the pinocchio function needed for the models.
223
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
224
225
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 21 taken 9 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 9 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 9 times.
 9 BOOST_CHECK(model->get_state()->get_nx() == cost_sum.get_state()->get_nx());
226
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 21 taken 9 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 9 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 9 times.
 9 BOOST_CHECK(model->get_state()->get_ndx() == cost_sum.get_state()->get_ndx());
227
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 9 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 9 times.
 9 BOOST_CHECK(model->get_nu() == cost_sum.get_nu());
228
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 21 taken 9 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 9 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 9 times.
 9 BOOST_CHECK(model->get_state()->get_nq() == cost_sum.get_state()->get_nq());
229
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 21 taken 9 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 9 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 9 times.
 9 BOOST_CHECK(model->get_state()->get_nv() == cost_sum.get_state()->get_nv());
230
6/12
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 19 taken 9 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 9 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 9 times.
 9 BOOST_CHECK(model->get_activation()->get_nr() == cost_sum.get_nr());
231 9 }
232
233 9 void test_partial_derivatives_in_cost_sum(
234 CostModelNoFFTypes::Type cost_type,
235 ActivationModelTypes::Type activation_type) {
236 // create the model
237
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 CostModelFactory factory;
238 const boost::shared_ptr<crocoddyl::CostModelAbstract>& model =
239
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 factory.create(cost_type, activation_type);
240
241 // create the corresponding data object
242 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
243 9 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
244 9 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
245
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 pinocchio::Data pinocchio_data(pinocchio_model);
246
247 boost::shared_ptr<crocoddyl::ActuationModelAbstract> actuation =
248
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 boost::make_shared<crocoddyl::ActuationModelFull>(state);
249 const boost::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data =
250
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation->createData();
251 crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data,
252
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 actuation_data);
253 const boost::shared_ptr<crocoddyl::CostDataAbstract>& data =
254
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 model->createData(&shared_data);
255
256 // create the cost sum model
257 9 crocoddyl::CostModelSum cost_sum(state, model->get_nu());
258
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 cost_sum.addCost("myCost", model, 1.);
259 const boost::shared_ptr<crocoddyl::CostDataSum>& data_sum =
260
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 cost_sum.createData(&shared_data);
261
262 // Generating random values for the state and control
263
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 const Eigen::VectorXd& x = state->rand();
264
2/4
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 const Eigen::VectorXd& u = Eigen::VectorXd::Random(model->get_nu());
265
266 // Compute all the pinocchio function needed for the models.
267
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
268
269 // Computing the cost derivatives
270
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 model->calc(data, x, u);
271
3/6
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
 9 model->calcDiff(data, x, u);
272
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 cost_sum.calc(data_sum, x, u);
273
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
 9 cost_sum.calcDiff(data_sum, x, u);
274
275
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 9 times.
 9 BOOST_CHECK((data->Lx - data_sum->Lx).isZero());
276
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 9 times.
 9 BOOST_CHECK((data->Lu - data_sum->Lu).isZero());
277
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 9 times.
 9 BOOST_CHECK((data->Lxx - data_sum->Lxx).isZero());
278
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 9 times.
 9 BOOST_CHECK((data->Lxu - data_sum->Lxu).isZero());
279
8/16
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 9 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 9 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 9 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 9 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 9 times.
 9 BOOST_CHECK((data->Luu - data_sum->Luu).isZero());
280 9 }
281
282 //----------------------------------------------------------------------------//
283
284 9 void register_cost_model_unit_tests(
285 CostModelNoFFTypes::Type cost_type,
286 ActivationModelTypes::Type activation_type) {
287
2/4
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 boost::test_tools::output_test_stream test_name;
288
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 9 test_name << "test_" << cost_type << "_" << activation_type
289
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 << "_StateMultibody_TalosArm";
290
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
 9 std::cout << "Running " << test_name.str() << std::endl;
291
4/8
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
 9 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
292
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
 9 ts->add(BOOST_TEST_CASE(
293 boost::bind(&test_calc_returns_a_cost, cost_type, activation_type)));
294
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
 9 ts->add(BOOST_TEST_CASE(
295 boost::bind(&test_calc_against_numdiff, cost_type, activation_type)));
296
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
 9 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
297 cost_type, activation_type)));
298
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
 9 ts->add(BOOST_TEST_CASE(
299 boost::bind(&test_dimensions_in_cost_sum, cost_type, activation_type)));
300
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 9 times.
✗ Branch 15 not taken.
 9 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_in_cost_sum,
301 cost_type, activation_type)));
302
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
 9 framework::master_test_suite().add(ts);
303 9 }
304
305 1 bool init_function() {
306 // Test all costs available with all the activation types with state type
307 // TalosArm.
308
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
 2 for (size_t cost_type = 0; cost_type < CostModelNoFFTypes::all.size();
309 ++cost_type) {
310 10 for (size_t activation_type = 0;
311
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 1 times.
 10 activation_type < ActivationModelTypes::all.size();
312 ++activation_type) {
313 9 register_cost_model_unit_tests(
314 9 CostModelNoFFTypes::all[cost_type],
315 9 ActivationModelTypes::all[activation_type]);
316 }
317 }
318 1 return true;
319 }
320
321 1 int main(int argc, char** argv) {
322 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
323 }
324