GCC Code Coverage Report

Directory: ./
File: unittest/test_contacts.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 118 118 100.0%
Branches: 347 686 50.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 <memory>
13 #include <pinocchio/algorithm/frames.hpp>
14 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
15
16 #include "crocoddyl/multibody/contacts/contact-1d.hpp"
17 #include "crocoddyl/multibody/contacts/contact-2d.hpp"
18 #include "crocoddyl/multibody/contacts/contact-3d.hpp"
19 #include "crocoddyl/multibody/contacts/contact-6d.hpp"
20 #include "factory/contact.hpp"
21 #include "unittest_common.hpp"
22
23 using namespace crocoddyl::unittest;
24 using namespace boost::unit_test;
25
26 //----------------------------------------------------------------------------//
27
28 50 void test_construct_data(ContactModelTypes::Type contact_type,
29 PinocchioModelTypes::Type model_type) {
30 // create the model
31
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
32 std::shared_ptr<crocoddyl::ContactModelAbstract> model =
33
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
34
35 // Run the print function
36
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 std::ostringstream tmp;
37
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 tmp << *model;
38
39 // create the corresponding data object
40 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
41
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 model->get_state()->get_pinocchio();
42
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
43 std::shared_ptr<crocoddyl::ContactDataAbstract> data =
44
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
45 50 }
46
47 50 void test_calc_fetch_jacobians(ContactModelTypes::Type contact_type,
48 PinocchioModelTypes::Type model_type) {
49 // create the model
50
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
51 std::shared_ptr<crocoddyl::ContactModelAbstract> model =
52
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
53
54 // create the corresponding data object
55 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
56
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 model->get_state()->get_pinocchio();
57
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
58 const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
59
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
60
61 // Compute the jacobian and check that the impulse model fetch it.
62
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 Eigen::VectorXd x = model->get_state()->rand();
63
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
64 &pinocchio_data, x);
65
66 // Getting the jacobian from the model
67
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calc(data, x);
68
69 // Check that only the Jacobian has been filled
70
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->Jc.isZero());
71
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 10 times.
 50 if (model_type !=
72 PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
73 // body system.
74
7/14
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 40 times.
 40 BOOST_CHECK(!data->a0.isZero());
75 }
76
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->da0_dx.isZero());
77
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->f.toVector().isZero());
78
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_dx.isZero());
79
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_du.isZero());
80
81 // Checking that casted computation is the same
82 #ifdef NDEBUG // Run only in release mode
83 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
84 model->cast<float>();
85 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
86 casted_model->get_state()->get_pinocchio();
87 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
88 const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
89 casted_model->createData(&pinocchio_data_f);
90 Eigen::VectorXf x_f = x.cast<float>();
91 crocoddyl::unittest::updateAllPinocchio(pinocchio_model_f.get(),
92 &pinocchio_data_f, x_f);
93 casted_model->calc(casted_data, x_f);
94 BOOST_CHECK(!casted_data->Jc.isZero());
95 if (model_type !=
96 PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
97 // body system.
98 BOOST_CHECK(!casted_data->a0.isZero());
99 }
100 BOOST_CHECK(casted_data->da0_dx.isZero());
101 BOOST_CHECK(casted_data->f.toVector().isZero());
102 BOOST_CHECK(casted_data->df_dx.isZero());
103 BOOST_CHECK(casted_data->df_du.isZero());
104 #endif
105 50 }
106
107 50 void test_calc_diff_fetch_derivatives(ContactModelTypes::Type contact_type,
108 PinocchioModelTypes::Type model_type) {
109 // create the model
110
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
111 std::shared_ptr<crocoddyl::ContactModelAbstract> model =
112
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
113
114 // create the corresponding data object
115 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
116
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 model->get_state()->get_pinocchio();
117
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
118 const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
119
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
120
121 // Compute the jacobian and check that the impulse model fetch it.
122
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 Eigen::VectorXd x = model->get_state()->rand();
123
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 crocoddyl::unittest::updateAllPinocchio(pinocchio_model.get(),
124 &pinocchio_data, x);
125
126 // Getting the jacobian from the model
127
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calc(data, x);
128
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calcDiff(data, x);
129
130 // Check that nothing has been computed and that all value are initialized to
131 // 0
132
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->Jc.isZero());
133
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 10 times.
 50 if (model_type !=
134 PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
135 // body system.
136
7/14
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 40 times.
 40 BOOST_CHECK(!data->a0.isZero());
137
7/14
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 40 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 40 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 40 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 40 times.
 40 BOOST_CHECK(!data->da0_dx.isZero());
138 }
139
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->f.toVector().isZero());
140
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_dx.isZero());
141
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_du.isZero());
142
143 // Checking that casted computation is the same
144 #ifdef NDEBUG // Run only in release mode
145 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
146 model->cast<float>();
147 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
148 casted_model->get_state()->get_pinocchio();
149 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
150 const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
151 casted_model->createData(&pinocchio_data_f);
152 Eigen::VectorXf x_f = x.cast<float>();
153 crocoddyl::unittest::updateAllPinocchio(pinocchio_model_f.get(),
154 &pinocchio_data_f, x_f);
155 casted_model->calc(casted_data, x_f);
156 casted_model->calcDiff(casted_data, x_f);
157 BOOST_CHECK(!casted_data->Jc.isZero());
158 if (model_type !=
159 PinocchioModelTypes::Hector) { // this is due to Hector is a single rigid
160 // body system.
161 float tol_f =
162 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
163 BOOST_CHECK(!casted_data->a0.isZero());
164 BOOST_CHECK(!casted_data->da0_dx.isZero());
165 BOOST_CHECK((data->a0.cast<float>() - casted_data->a0).isZero());
166 BOOST_CHECK(
167 (data->da0_dx.cast<float>() - casted_data->da0_dx).isZero(tol_f));
168 }
169 BOOST_CHECK(casted_data->f.toVector().isZero());
170 BOOST_CHECK(casted_data->df_dx.isZero());
171 BOOST_CHECK(casted_data->df_du.isZero());
172 #endif
173 50 }
174
175 50 void test_update_force(ContactModelTypes::Type contact_type,
176 PinocchioModelTypes::Type model_type) {
177 // create the model
178
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
179 std::shared_ptr<crocoddyl::ContactModelAbstract> model =
180
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
181
182 // create the corresponding data object
183 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
184
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 model->get_state()->get_pinocchio();
185
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
186 const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
187
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
188
189 // Create a random force and update it
190
2/4
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 Eigen::VectorXd f = Eigen::VectorXd::Random(data->Jc.rows());
191
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->updateForce(data, f);
192
193 // Check that nothing has been computed and that all value are initialized to
194 // 0
195
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->Jc.isZero());
196
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->a0.isZero());
197
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->da0_dx.isZero());
198
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(!data->f.toVector().isZero());
199
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(!data->fext.toVector().isZero());
200
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_dx.isZero());
201
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->df_du.isZero());
202
203 // Checking that casted computation is the same
204 #ifdef NDEBUG // Run only in release mode
205 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
206 model->cast<float>();
207 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
208 casted_model->get_state()->get_pinocchio();
209 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
210 const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
211 casted_model->createData(&pinocchio_data_f);
212 Eigen::VectorXf f_f = f.cast<float>();
213 casted_model->updateForce(casted_data, f_f);
214 BOOST_CHECK(casted_data->Jc.isZero());
215 BOOST_CHECK(casted_data->a0.isZero());
216 BOOST_CHECK(casted_data->da0_dx.isZero());
217 BOOST_CHECK(!casted_data->f.toVector().isZero());
218 BOOST_CHECK(!casted_data->fext.toVector().isZero());
219 BOOST_CHECK(
220 (data->f.toVector().cast<float>() - casted_data->f.toVector()).isZero());
221 BOOST_CHECK(
222 (data->fext.toVector().cast<float>() - casted_data->fext.toVector())
223 .isZero());
224 BOOST_CHECK(casted_data->df_dx.isZero());
225 BOOST_CHECK(casted_data->df_du.isZero());
226 #endif
227 50 }
228
229 50 void test_update_force_diff(ContactModelTypes::Type contact_type,
230 PinocchioModelTypes::Type model_type) {
231 // create the model
232
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
233 std::shared_ptr<crocoddyl::ContactModelAbstract> model =
234
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
235
236 // create the corresponding data object
237 const std::shared_ptr<pinocchio::Model>& pinocchio_model =
238
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 model->get_state()->get_pinocchio();
239
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 pinocchio::Data pinocchio_data(*pinocchio_model.get());
240 const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
241
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
242
243 // Create a random force and update it
244 Eigen::MatrixXd df_dx =
245
2/4
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
 50 Eigen::MatrixXd::Random(data->df_dx.rows(), data->df_dx.cols());
246 Eigen::MatrixXd df_du =
247
2/4
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
 50 Eigen::MatrixXd::Random(data->df_du.rows(), data->df_du.cols());
248
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->updateForceDiff(data, df_dx, df_du);
249
250 // Check that nothing has been computed and that all value are initialized to
251 // 0
252
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->Jc.isZero());
253
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->a0.isZero());
254
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(data->da0_dx.isZero());
255
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->f.toVector().isZero());
256
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK(data->fext.toVector().isZero());
257
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->df_dx.isZero());
258
7/14
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✗ Branch 27 not taken.
✓ Branch 28 taken 50 times.
 50 BOOST_CHECK(!data->df_du.isZero());
259
260 // Checking that casted computation is the same
261 #ifdef NDEBUG // Run only in release mode
262 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
263 model->cast<float>();
264 const std::shared_ptr<pinocchio::ModelTpl<float>>& pinocchio_model_f =
265 casted_model->get_state()->get_pinocchio();
266 pinocchio::DataTpl<float> pinocchio_data_f(*pinocchio_model_f.get());
267 const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
268 casted_model->createData(&pinocchio_data_f);
269 Eigen::MatrixXf df_dx_f = df_dx.cast<float>();
270 Eigen::MatrixXf df_du_f = df_du.cast<float>();
271 casted_model->updateForceDiff(casted_data, df_dx_f, df_du_f);
272 BOOST_CHECK(casted_data->Jc.isZero());
273 BOOST_CHECK(casted_data->a0.isZero());
274 BOOST_CHECK(casted_data->da0_dx.isZero());
275 BOOST_CHECK(casted_data->f.toVector().isZero());
276 BOOST_CHECK(casted_data->fext.toVector().isZero());
277 BOOST_CHECK(!casted_data->df_dx.isZero());
278 BOOST_CHECK(!casted_data->df_du.isZero());
279 BOOST_CHECK((data->df_dx.cast<float>() - casted_data->df_dx).isZero());
280 BOOST_CHECK((data->df_du.cast<float>() - casted_data->df_du).isZero());
281 #endif
282 50 }
283
284 50 void test_partial_derivatives_against_numdiff(
285 ContactModelTypes::Type contact_type,
286 PinocchioModelTypes::Type model_type) {
287 using namespace boost::placeholders;
288
289 // create the model
290
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ContactModelFactory factory;
291 std::shared_ptr<crocoddyl::ContactModelAbstract> model =
292
4/8
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 50 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 50 times.
✗ Branch 13 not taken.
 50 factory.create(contact_type, model_type, Eigen::Vector2d::Random());
293
294 // create the corresponding data object
295 pinocchio::Model& pinocchio_model =
296
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 *model->get_state()->get_pinocchio().get();
297
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 pinocchio::Data pinocchio_data(pinocchio_model);
298 const std::shared_ptr<crocoddyl::ContactDataAbstract>& data =
299
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 model->createData(&pinocchio_data);
300
301 // Create the equivalent num diff model and data.
302
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 crocoddyl::ContactModelNumDiff model_num_diff(model);
303 const std::shared_ptr<crocoddyl::ContactDataAbstract>& data_num_diff =
304
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 model_num_diff.createData(&pinocchio_data);
305
306 // Generating random values for the state
307
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 const Eigen::VectorXd x = model->get_state()->rand();
308
309 // Compute all the pinocchio function needed for the models.
310
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
311
312 // set the function that needs to be called at every step of the numdiff
313 50 std::vector<crocoddyl::ContactModelNumDiff::ReevaluationFunction> reevals;
314
3/6
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
 50 reevals.push_back(
315 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
316 double, 0, pinocchio::JointCollectionDefaultTpl>,
317 &pinocchio_model, &pinocchio_data, _1, _2));
318
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 model_num_diff.set_reevals(reevals);
319
320 // Computing the contact derivatives
321
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calc(data, x);
322
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 model->calcDiff(data, x);
323
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 model_num_diff.calc(data_num_diff, x);
324
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 model_num_diff.calcDiff(data_num_diff, x);
325 // Tolerance defined as in
326 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
327 50 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
328
8/16
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 50 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 50 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 50 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 50 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 50 times.
 50 BOOST_CHECK((data->da0_dx - data_num_diff->da0_dx).isZero(tol));
329
330 // Checking that casted computation is the same
331 #ifdef NDEBUG // Run only in release mode
332 std::shared_ptr<crocoddyl::ContactModelAbstractTpl<float>> casted_model =
333 model->cast<float>();
334 pinocchio::ModelTpl<float>& pinocchio_model_f =
335 *casted_model->get_state()->get_pinocchio().get();
336 pinocchio::DataTpl<float> pinocchio_data_f(pinocchio_model_f);
337 const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>& casted_data =
338 casted_model->createData(&pinocchio_data_f);
339 const Eigen::VectorXf x_f = x.cast<float>();
340 crocoddyl::ContactModelNumDiffTpl<float> casted_model_num_diff =
341 model_num_diff.cast<float>();
342 const std::shared_ptr<crocoddyl::ContactDataAbstractTpl<float>>&
343 casted_data_num_diff =
344 casted_model_num_diff.createData(&pinocchio_data_f);
345 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x);
346 crocoddyl::unittest::updateAllPinocchio(&pinocchio_model_f, &pinocchio_data_f,
347 x_f);
348 std::vector<crocoddyl::ContactModelNumDiffTpl<float>::ReevaluationFunction>
349 reevals_f;
350 reevals_f.push_back(
351 boost::bind(&crocoddyl::unittest::updateAllPinocchio<
352 float, 0, pinocchio::JointCollectionDefaultTpl>,
353 &pinocchio_model_f, &pinocchio_data_f, _1, _2));
354 casted_model_num_diff.set_reevals(reevals_f);
355 model->calc(data, x);
356 model->calcDiff(data, x);
357 casted_model->calc(casted_data, x_f);
358 casted_model->calcDiff(casted_data, x_f);
359 casted_model_num_diff.calc(casted_data_num_diff, x_f);
360 casted_model_num_diff.calcDiff(casted_data_num_diff, x_f);
361 float tol_f = 80.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
362 BOOST_CHECK((data->da0_dx.cast<float>() - casted_data->da0_dx).isZero(tol_f));
363 BOOST_CHECK((casted_data->da0_dx - casted_data_num_diff->da0_dx)
364 .isZero(30.f * tol_f));
365 #endif
366 50 }
367
368 //----------------------------------------------------------------------------//
369
370 50 void register_contact_model_unit_tests(ContactModelTypes::Type contact_type,
371 PinocchioModelTypes::Type model_type) {
372
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 boost::test_tools::output_test_stream test_name;
373
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 50 test_name << "test_" << contact_type << "_" << model_type;
374
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 50 std::cout << "Running " << test_name.str() << std::endl;
375
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
 50 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
376
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
377 boost::bind(&test_construct_data, contact_type, model_type)));
378
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
379 boost::bind(&test_calc_fetch_jacobians, contact_type, model_type)));
380
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_diff_fetch_derivatives,
381 contact_type, model_type)));
382
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
383 boost::bind(&test_update_force, contact_type, model_type)));
384
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(
385 boost::bind(&test_update_force_diff, contact_type, model_type)));
386
5/10
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 50 times.
✗ Branch 15 not taken.
 50 ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff,
387 contact_type, model_type)));
388
3/6
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 50 times.
✗ Branch 7 not taken.
 50 framework::master_test_suite().add(ts);
389 50 }
390
391 1 bool init_function() {
392
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
 11 for (size_t contact_type = 0; contact_type < ContactModelTypes::all.size();
393 ++contact_type) {
394
2/2
✓ Branch 1 taken 50 times.
✓ Branch 2 taken 10 times.
 60 for (size_t model_type = 0; model_type < PinocchioModelTypes::all.size();
395 ++model_type) {
396 50 register_contact_model_unit_tests(ContactModelTypes::all[contact_type],
397 50 PinocchioModelTypes::all[model_type]);
398 }
399 }
400 1 return true;
401 }
402
403 1 int main(int argc, char** argv) {
404 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
405 }
406