GCC Code Coverage Report

Directory: ./
File: unittest/test_constraints.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 133 133 100.0%
Branches: 332 660 50.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2025, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #define BOOST_TEST_NO_MAIN
10 #define BOOST_TEST_ALTERNATIVE_INIT_API
11
12 #include "crocoddyl/multibody/data/multibody.hpp"
13 #include "factory/constraint.hpp"
14 #include "unittest_common.hpp"
15
16 using namespace boost::unit_test;
17 using namespace crocoddyl::unittest;
18
19 //----------------------------------------------------------------------------//
20
21 70 void test_calc_returns_a_residual(ConstraintModelTypes::Type constraint_type,
22 StateModelTypes::Type state_type) {
23 // create the model
24
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
25 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
26
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
27
28 // create the corresponding data object
29 const std::shared_ptr<crocoddyl::StateMultibody>& state =
30
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
31
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
32
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
33
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
34 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
35
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
36
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 data->g *= nan("");
37
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 data->h *= nan("");
38
39 // Generating random values for the state and control
40
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 const Eigen::VectorXd x = model->get_state()->rand();
41
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
42
43 // Compute all the pinocchio function needed for the models.
44
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
45
46 // Getting the constraint residual computed by calc()
47
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 model->calc(data, x, u);
48
49 // Checking that calc returns a residual vector
50
7/14
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 70 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 70 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 70 times.
 70 BOOST_CHECK(!data->g.hasNaN());
51
7/14
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 70 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 70 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 70 times.
 70 BOOST_CHECK(!data->h.hasNaN());
52
53 // Checking that casted computation is the same
54 #ifdef NDEBUG // Run only in release mode
55 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
56 casted_model = model->cast<float>();
57 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
58 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
59 casted_model->get_state());
60 pinocchio::ModelTpl<float>& casted_pinocchio_model =
61 *casted_state->get_pinocchio().get();
62 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
63 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
64 &casted_pinocchio_data);
65 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
66 casted_data = casted_model->createData(&casted_shared_data);
67 casted_data->g *= float(nan(""));
68 casted_data->h *= float(nan(""));
69 const Eigen::VectorXf x_f = x.cast<float>();
70 const Eigen::VectorXf u_f = u.cast<float>();
71 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
72 &casted_pinocchio_data, x_f);
73 casted_model->calc(casted_data, x_f, u_f);
74 BOOST_CHECK(!casted_data->g.hasNaN());
75 BOOST_CHECK(!casted_data->h.hasNaN());
76 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
77 BOOST_CHECK((data->g.cast<float>() - casted_data->g).isZero(tol_f));
78 BOOST_CHECK((data->h.cast<float>() - casted_data->h).isZero(tol_f));
79 #endif
80 70 }
81
82 70 void test_calc_against_numdiff(ConstraintModelTypes::Type constraint_type,
83 StateModelTypes::Type state_type) {
84 // create the model
85
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
86 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
87
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
88
89 // create the corresponding data object
90 const std::shared_ptr<crocoddyl::StateMultibody>& state =
91
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
92
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
93
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
94
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
95 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
96
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
97
98 // Create the equivalent num diff model and data.
99
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::ConstraintModelNumDiff model_num_diff(model);
100 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
101
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 model_num_diff.createData(&shared_data);
102
103 // Generating random values for the state and control
104
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 const Eigen::VectorXd x = model->get_state()->rand();
105
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
106
107 // Compute all the pinocchio function needed for the models.
108
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
109
110 // Computing the cost derivatives
111
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 model->calc(data, x, u);
112
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 70 model_num_diff.calc(data_num_diff, x, u);
113
114 // Checking the partial derivatives against NumDiff
115
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 70 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 70 times.
 70 BOOST_CHECK((data->g - data_num_diff->g).isZero());
116
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 70 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 70 times.
 70 BOOST_CHECK((data->h - data_num_diff->h).isZero());
117
118 // Checking that casted computation is the same
119 #ifdef NDEBUG // Run only in release mode
120 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
121 casted_model = model->cast<float>();
122 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
123 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
124 casted_model->get_state());
125 pinocchio::ModelTpl<float>& casted_pinocchio_model =
126 *casted_state->get_pinocchio().get();
127 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
128 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
129 &casted_pinocchio_data);
130 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
131 casted_data = casted_model->createData(&casted_shared_data);
132 crocoddyl::ConstraintModelNumDiffTpl<float> casted_model_num_diff =
133 model_num_diff.cast<float>();
134 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
135 casted_data_num_diff =
136 casted_model_num_diff.createData(&casted_shared_data);
137 const Eigen::VectorXf x_f = x.cast<float>();
138 const Eigen::VectorXf u_f = u.cast<float>();
139 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
140 &casted_pinocchio_data, x_f);
141 casted_model->calc(casted_data, x_f, u_f);
142 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
143 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
144 BOOST_CHECK((casted_data->g - casted_data_num_diff->g).isZero(tol_f));
145 BOOST_CHECK((casted_data->h - casted_data_num_diff->h).isZero(tol_f));
146 #endif
147 70 }
148
149 70 void test_partial_derivatives_against_numdiff(
150 ConstraintModelTypes::Type constraint_type,
151 StateModelTypes::Type state_type) {
152 // create the model
153
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
154 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
155
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
156
157 // create the corresponding data object
158 const std::shared_ptr<crocoddyl::StateMultibody>& state =
159
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
160
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
161
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
162
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
163 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
164
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
165
166 // Create the equivalent num diff model and data.
167
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::ConstraintModelNumDiff model_num_diff(model);
168 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
169
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 model_num_diff.createData(&shared_data);
170
171 // Generating random values for the state and control
172
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 Eigen::VectorXd x = model->get_state()->rand();
173
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
174
175 // Compute all the pinocchio function needed for the models.
176
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
177
178 // set the function that needs to be called at every step of the numdiff
179 using namespace boost::placeholders;
180
181 70 std::vector<crocoddyl::ConstraintModelNumDiff::ReevaluationFunction> reevals;
182
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 70 reevals.push_back(
183 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
184 double, 0, pinocchio::JointCollectionDefaultTpl>,
185 &pinocchio_model, &pinocchio_data, _1, _2));
186
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 model_num_diff.set_reevals(reevals);
187
188 // Computing the cost derivatives
189
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 model->calc(data, x, u);
190
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 model->calcDiff(data, x, u);
191
192 // Computing the cost derivatives via numerical differentiation
193
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 70 model_num_diff.calc(data_num_diff, x, u);
194
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 70 model_num_diff.calcDiff(data_num_diff, x, u);
195
196 // Checking the partial derivatives against numdiff
197 70 double tol = sqrt(model_num_diff.get_disturbance());
198
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 70 times.
 70 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
199
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 70 times.
 70 BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
200
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 70 times.
 70 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
201
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 70 times.
 70 BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
202
203 // Computing the cost derivatives
204
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 x = model->get_state()->rand();
205
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
206
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
 70 model->calc(data, x);
207
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
 70 model->calcDiff(data, x);
208
209 // Computing the cost derivatives via numerical differentiation
210
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 model_num_diff.calc(data_num_diff, x);
211
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 model_num_diff.calcDiff(data_num_diff, x);
212
213 // Checking the partial derivatives against numdiff
214
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 70 times.
 70 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
215
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 70 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 70 times.
 70 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
216
217 // Checking that casted computation is the same
218 #ifdef NDEBUG // Run only in release mode
219 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
220 casted_model = model->cast<float>();
221 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
222 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
223 casted_model->get_state());
224 pinocchio::ModelTpl<float>& casted_pinocchio_model =
225 *casted_state->get_pinocchio().get();
226 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
227 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
228 &casted_pinocchio_data);
229 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
230 casted_data = casted_model->createData(&casted_shared_data);
231 crocoddyl::ConstraintModelNumDiffTpl<float> casted_model_num_diff =
232 model_num_diff.cast<float>();
233 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
234 casted_data_num_diff =
235 casted_model_num_diff.createData(&casted_shared_data);
236 std::vector<crocoddyl::ConstraintModelNumDiffTpl<float>::ReevaluationFunction>
237 casted_reevals;
238 casted_reevals.push_back(
239 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
240 float, 0, pinocchio::JointCollectionDefaultTpl>,
241 &casted_pinocchio_model, &casted_pinocchio_data, _1, _2));
242 casted_model_num_diff.set_reevals(casted_reevals);
243 const Eigen::VectorXf x_f = x.cast<float>();
244 const Eigen::VectorXf u_f = u.cast<float>();
245 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
246 crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model,
247 &casted_pinocchio_data, x_f);
248 model->calc(data, x, u);
249 model->calcDiff(data, x, u);
250 casted_model->calc(casted_data, x_f, u_f);
251 casted_model->calcDiff(casted_data, x_f, u_f);
252 casted_model_num_diff.calc(casted_data_num_diff, x_f, u_f);
253 casted_model_num_diff.calcDiff(casted_data_num_diff, x_f, u_f);
254 float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
255 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
256 BOOST_CHECK((data->Gu.cast<float>() - casted_data->Gu).isZero(tol_f));
257 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
258 BOOST_CHECK((data->Hu.cast<float>() - casted_data->Hu).isZero(tol_f));
259 tol_f = sqrt(casted_model_num_diff.get_disturbance());
260 BOOST_CHECK((casted_data->Gx - casted_data_num_diff->Gx).isZero(tol_f));
261 BOOST_CHECK((casted_data->Gu - casted_data_num_diff->Gu).isZero(tol_f));
262 BOOST_CHECK((casted_data->Hx - casted_data_num_diff->Hx).isZero(tol_f));
263 BOOST_CHECK((casted_data->Hu - casted_data_num_diff->Hu).isZero(tol_f));
264 #endif
265 70 }
266
267 70 void test_dimensions_in_constraint_manager(
268 ConstraintModelTypes::Type constraint_type,
269 StateModelTypes::Type state_type) {
270 // create the model
271
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
272 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
273
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
274
275 // create the corresponding data object
276 const std::shared_ptr<crocoddyl::StateMultibody>& state =
277
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
278
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
279
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
280
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
281
282 // create the constraint manager model
283
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
284
2/4
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 constraint_man.addConstraint("myConstraint", model);
285
286 // Generating random values for the state and control
287
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 const Eigen::VectorXd x = state->rand();
288
289 // Compute all the pinocchio function needed for the models.
290
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
291
292
10/20
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 70 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 70 times.
 70 BOOST_CHECK(model->get_state()->get_nx() ==
293 constraint_man.get_state()->get_nx());
294
10/20
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 70 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 70 times.
 70 BOOST_CHECK(model->get_state()->get_ndx() ==
295 constraint_man.get_state()->get_ndx());
296
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 70 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 70 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 70 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 70 times.
 70 BOOST_CHECK(model->get_nu() == constraint_man.get_nu());
297
10/20
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 70 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 70 times.
 70 BOOST_CHECK(model->get_state()->get_nq() ==
298 constraint_man.get_state()->get_nq());
299
10/20
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 70 times.
✗ Branch 33 not taken.
✗ Branch 37 not taken.
✓ Branch 38 taken 70 times.
 70 BOOST_CHECK(model->get_state()->get_nv() ==
300 constraint_man.get_state()->get_nv());
301
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 70 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 70 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 70 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 70 times.
 70 BOOST_CHECK(model->get_ng() == constraint_man.get_ng());
302
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 70 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 70 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 70 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 70 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 70 times.
 70 BOOST_CHECK(model->get_nh() == constraint_man.get_nh());
303
304 // Checking that casted computation is the same
305 #ifdef NDEBUG // Run only in release mode
306 crocoddyl::ConstraintModelManagerTpl<float> casted_constraint_man =
307 constraint_man.cast<float>();
308 BOOST_CHECK(model->get_state()->get_nx() ==
309 casted_constraint_man.get_state()->get_nx());
310 BOOST_CHECK(model->get_state()->get_ndx() ==
311 casted_constraint_man.get_state()->get_ndx());
312 BOOST_CHECK(model->get_nu() == casted_constraint_man.get_nu());
313 BOOST_CHECK(model->get_state()->get_nq() ==
314 casted_constraint_man.get_state()->get_nq());
315 BOOST_CHECK(model->get_state()->get_nv() ==
316 casted_constraint_man.get_state()->get_nv());
317 BOOST_CHECK(model->get_ng() == casted_constraint_man.get_ng());
318 BOOST_CHECK(model->get_nh() == casted_constraint_man.get_nh());
319 #endif
320 70 }
321
322 70 void test_partial_derivatives_in_constraint_manager(
323 ConstraintModelTypes::Type constraint_type,
324 StateModelTypes::Type state_type) {
325 // create the model
326
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
327 const std::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
328
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
329
330 // create the corresponding data object
331 const std::shared_ptr<crocoddyl::StateMultibody>& state =
332
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
333
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
334
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
335
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
336 const std::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
337
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
338
339 // create the constraint manager model
340
2/4
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
341
2/4
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 constraint_man.addConstraint("myConstraint", model, 1.);
342 const std::shared_ptr<crocoddyl::ConstraintDataManager>& data_man =
343
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 constraint_man.createData(&shared_data);
344
345 // Generating random values for the state and control
346
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 const Eigen::VectorXd x = state->rand();
347
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
348
349 // Compute all the pinocchio function needed for the models.
350
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
351
352 // Computing the constraint derivatives
353
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 model->calc(data, x, u);
354
3/6
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
 70 model->calcDiff(data, x, u);
355
356 // Computing the constraint-manager derivatives
357
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 70 constraint_man.calc(data_man, x, u);
358
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
 70 constraint_man.calcDiff(data_man, x, u);
359
360
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 70 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 70 times.
 70 BOOST_CHECK((data->Hx - data_man->Hx).isZero());
361
8/16
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 70 times.
✗ Branch 17 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 70 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 70 times.
 70 BOOST_CHECK((data->Hu - data_man->Hu).isZero());
362
363 // Checking that casted computation is the same
364 #ifdef NDEBUG // Run only in release mode
365 const std::shared_ptr<crocoddyl::ConstraintModelAbstractTpl<float>>&
366 casted_model = model->cast<float>();
367 const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state =
368 std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>(
369 casted_model->get_state());
370 pinocchio::ModelTpl<float>& casted_pinocchio_model =
371 *casted_state->get_pinocchio().get();
372 pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model);
373 crocoddyl::DataCollectorMultibodyTpl<float> casted_shared_data(
374 &casted_pinocchio_data);
375 const std::shared_ptr<crocoddyl::ConstraintDataAbstractTpl<float>>&
376 casted_data = casted_model->createData(&casted_shared_data);
377 crocoddyl::ConstraintModelManagerTpl<float> casted_constraint_man =
378 constraint_man.cast<float>();
379 const std::shared_ptr<crocoddyl::ConstraintDataManagerTpl<float>>&
380 casted_data_man = casted_constraint_man.createData(&casted_shared_data);
381 const Eigen::VectorXf x_f = x.cast<float>();
382 const Eigen::VectorXf u_f = u.cast<float>();
383 casted_model->calc(casted_data, x_f, u_f);
384 casted_model->calcDiff(casted_data, x_f, u_f);
385 casted_constraint_man.calc(casted_data_man, x_f, u_f);
386 casted_constraint_man.calcDiff(casted_data_man, x_f, u_f);
387 BOOST_CHECK((casted_data->Hx - casted_data_man->Hx).isZero());
388 BOOST_CHECK((casted_data->Hu - casted_data_man->Hu).isZero());
389 #endif
390 70 }
391
392 //----------------------------------------------------------------------------//
393
394 70 void register_constraint_model_unit_tests(
395 ConstraintModelTypes::Type constraint_type,
396 StateModelTypes::Type state_type) {
397
2/4
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 boost::test_tools::output_test_stream test_name;
398
4/8
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
 70 test_name << "test_" << constraint_type << "_" << state_type;
399
4/8
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 70 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 70 times.
✗ Branch 11 not taken.
 70 std::cout << "Running " << test_name.str() << std::endl;
400
4/8
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
 70 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
401
5/10
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 ts->add(BOOST_TEST_CASE(
402 boost::bind(&test_calc_returns_a_residual, constraint_type, state_type)));
403
5/10
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 ts->add(BOOST_TEST_CASE(
404 boost::bind(&test_calc_against_numdiff, constraint_type, state_type)));
405
5/10
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
406 constraint_type, state_type)));
407
5/10
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 ts->add(BOOST_TEST_CASE(boost::bind(&test_dimensions_in_constraint_manager,
408 constraint_type, state_type)));
409
5/10
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 70 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 70 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 70 times.
✗ Branch 15 not taken.
 70 ts->add(BOOST_TEST_CASE(
410 boost::bind(&test_partial_derivatives_in_constraint_manager,
411 constraint_type, state_type)));
412
3/6
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 framework::master_test_suite().add(ts);
413 70 }
414
415 1 bool init_function() {
416 // Test all constraints available with all available states types.
417 15 for (size_t constraint_type = 0;
418
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 1 times.
 15 constraint_type < ConstraintModelTypes::all.size(); ++constraint_type) {
419 70 for (size_t state_type =
420 14 StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
421
2/2
✓ Branch 1 taken 70 times.
✓ Branch 2 taken 14 times.
 84 state_type < StateModelTypes::all.size(); ++state_type) {
422 70 register_constraint_model_unit_tests(
423 70 ConstraintModelTypes::all[constraint_type],
424 70 StateModelTypes::all[state_type]);
425 }
426 }
427 1 return true;
428 }
429
430 1 int main(int argc, char** argv) {
431 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
432 }
433