GCC Code Coverage Report

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