GCC Code Coverage Report

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