GCC Code Coverage Report

Directory: ./
File: unittest/test_constraints.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 133 133 100.0%
Branches: 290 576 50.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2023, 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 boost::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 boost::shared_ptr<crocoddyl::StateMultibody>& state =
30 70 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
31 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 boost::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
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 const Eigen::VectorXd x = model->get_state()->rand();
41
2/4
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 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 70 }
53
54 70 void test_calc_against_numdiff(ConstraintModelTypes::Type constraint_type,
55 StateModelTypes::Type state_type) {
56 // create the model
57
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
58 const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
59
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
60
61 // create the corresponding data object
62 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
63 70 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
64 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
65
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
66
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
67 const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
68
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
69
70 // Create the equivalent num diff model and data.
71
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::ConstraintModelNumDiff model_num_diff(model);
72 const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
73
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 model_num_diff.createData(&shared_data);
74
75 // Generating random values for the state and control
76
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 const Eigen::VectorXd x = model->get_state()->rand();
77
2/4
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
78
79 // Compute all the pinocchio function needed for the models.
80
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);
81
82 // Computing the cost derivatives
83
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);
84
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);
85
86 // Checking the partial derivatives against NumDiff
87
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());
88
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());
89 70 }
90
91 70 void test_partial_derivatives_against_numdiff(
92 ConstraintModelTypes::Type constraint_type,
93 StateModelTypes::Type state_type) {
94 // create the model
95
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
96 const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
97
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
98
99 // create the corresponding data object
100 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
101 70 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
102 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
103
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
104
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
105 const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
106
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
107
108 // Create the equivalent num diff model and data.
109
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::ConstraintModelNumDiff model_num_diff(model);
110 const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data_num_diff =
111
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 model_num_diff.createData(&shared_data);
112
113 // Generating random values for the state and control
114
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 Eigen::VectorXd x = model->get_state()->rand();
115
2/4
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
116
117 // Compute all the pinocchio function needed for the models.
118
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);
119
120 // set the function that needs to be called at every step of the numdiff
121 using namespace boost::placeholders;
122
123 70 std::vector<crocoddyl::ConstraintModelNumDiff::ReevaluationFunction> reevals;
124
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(boost::bind(&crocoddyl::unittest::updateAllPinocchio,
125 &pinocchio_model, &pinocchio_data, _1, _2));
126
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 model_num_diff.set_reevals(reevals);
127
128 // Computing the cost derivatives
129
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);
130
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);
131
132 // Computing the cost derivatives via numerical differentiation
133
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);
134
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);
135
136 // Checking the partial derivatives against numdiff
137 70 double tol = sqrt(model_num_diff.get_disturbance());
138
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));
139
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));
140
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));
141
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));
142
143 // Computing the cost derivatives
144
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 x = model->get_state()->rand();
145
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);
146
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);
147
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);
148
149 // Computing the cost derivatives via numerical differentiation
150
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);
151
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);
152
153 // Checking the partial derivatives against numdiff
154
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));
155
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));
156 70 }
157
158 70 void test_dimensions_in_constraint_manager(
159 ConstraintModelTypes::Type constraint_type,
160 StateModelTypes::Type state_type) {
161 // create the model
162
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
163 const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
164
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
165
166 // create the corresponding data object
167 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
168 70 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
169 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
170
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
171
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
172
173 // create the constraint manager model
174
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
175
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);
176
177 // Generating random values for the state and control
178
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 const Eigen::VectorXd x = state->rand();
179
180 // Compute all the pinocchio function needed for the models.
181
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);
182
183
6/12
✓ 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 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_state()->get_nx() ==
184 constraint_man.get_state()->get_nx());
185
6/12
✓ 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 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_state()->get_ndx() ==
186 constraint_man.get_state()->get_ndx());
187
6/12
✓ 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 17 taken 70 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 70 times.
 70 BOOST_CHECK(model->get_nu() == constraint_man.get_nu());
188
6/12
✓ 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 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_state()->get_nq() ==
189 constraint_man.get_state()->get_nq());
190
6/12
✓ 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 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_state()->get_nv() ==
191 constraint_man.get_state()->get_nv());
192
6/12
✓ 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 17 taken 70 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 70 times.
 70 BOOST_CHECK(model->get_ng() == constraint_man.get_ng());
193
6/12
✓ 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 17 taken 70 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 70 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 70 times.
 70 BOOST_CHECK(model->get_nh() == constraint_man.get_nh());
194 70 }
195
196 70 void test_partial_derivatives_in_constraint_manager(
197 ConstraintModelTypes::Type constraint_type,
198 StateModelTypes::Type state_type) {
199 // create the model
200
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 ConstraintModelFactory factory;
201 const boost::shared_ptr<crocoddyl::ConstraintModelAbstract>& model =
202
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 factory.create(constraint_type, state_type);
203
204 // create the corresponding data object
205 const boost::shared_ptr<crocoddyl::StateMultibody>& state =
206 70 boost::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state());
207 70 pinocchio::Model& pinocchio_model = *state->get_pinocchio().get();
208
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 pinocchio::Data pinocchio_data(pinocchio_model);
209
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 crocoddyl::DataCollectorMultibody shared_data(&pinocchio_data);
210 const boost::shared_ptr<crocoddyl::ConstraintDataAbstract>& data =
211
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 model->createData(&shared_data);
212
213 // create the constraint manager model
214
1/2
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
 70 crocoddyl::ConstraintModelManager constraint_man(state, model->get_nu());
215
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.);
216 const boost::shared_ptr<crocoddyl::ConstraintDataManager>& data_man =
217
1/2
✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
 70 constraint_man.createData(&shared_data);
218
219 // Generating random values for the state and control
220
1/2
✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
 70 const Eigen::VectorXd x = state->rand();
221
2/4
✓ Branch 3 taken 70 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 70 times.
✗ Branch 7 not taken.
 70 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
222
223 // Compute all the pinocchio function needed for the models.
224
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);
225
226 // Computing the constraint derivatives
227
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);
228
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);
229
230 // Computing the constraint-manager derivatives
231
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);
232
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);
233
234
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());
235
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());
236 70 }
237
238 //----------------------------------------------------------------------------//
239
240 70 void register_constraint_model_unit_tests(
241 ConstraintModelTypes::Type constraint_type,
242 StateModelTypes::Type state_type) {
243
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;
244
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;
245
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;
246
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());
247
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(
248 boost::bind(&test_calc_returns_a_residual, constraint_type, state_type)));
249
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(
250 boost::bind(&test_calc_against_numdiff, constraint_type, state_type)));
251
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,
252 constraint_type, state_type)));
253
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,
254 constraint_type, state_type)));
255
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(
256 boost::bind(&test_partial_derivatives_in_constraint_manager,
257 constraint_type, state_type)));
258
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);
259 70 }
260
261 1 bool init_function() {
262 // Test all constraints available with all available states types.
263 15 for (size_t constraint_type = 0;
264
2/2
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 1 times.
 15 constraint_type < ConstraintModelTypes::all.size(); ++constraint_type) {
265 70 for (size_t state_type =
266 14 StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm];
267
2/2
✓ Branch 1 taken 70 times.
✓ Branch 2 taken 14 times.
 84 state_type < StateModelTypes::all.size(); ++state_type) {
268 70 register_constraint_model_unit_tests(
269 70 ConstraintModelTypes::all[constraint_type],
270 70 StateModelTypes::all[state_type]);
271 }
272 }
273 1 return true;
274 }
275
276 1 int main(int argc, char** argv) {
277 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
278 }
279