GCC Code Coverage Report

Directory: ./
File: unittest/test_friction_cone.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 197 0.0%
Branches: 0 1588 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, University of Oxford,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #define BOOST_TEST_NO_MAIN
11 #define BOOST_TEST_ALTERNATIVE_INIT_API
12
13 #include <pinocchio/math/quaternion.hpp>
14
15 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
16 #include "crocoddyl/multibody/friction-cone.hpp"
17 #include "unittest_common.hpp"
18
19 using namespace boost::unit_test;
20 using namespace crocoddyl::unittest;
21
22 ✗ void test_constructor() {
23 // Common parameters
24 ✗ double mu = random_real_in_range(0.01, 1.);
25 ✗ std::size_t nf = 2 * random_int_in_range(2, 16);
26 ✗ bool inner_appr = false;
27
28 // No rotation
29 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
30
31 // Create the friction cone with rotation and surface normal
32 ✗ crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
33 ✗ crocoddyl::FrictionCone casted_cone = cone.cast<double>();
34
35 ✗ BOOST_CHECK(cone.get_R().isApprox(R));
36 ✗ BOOST_CHECK(cone.get_mu() == mu);
37 ✗ BOOST_CHECK(cone.get_nf() == nf);
38 ✗ BOOST_CHECK(cone.get_inner_appr() == inner_appr);
39 ✗ BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
40 ✗ BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
41 ✗ BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);
42
43 // Checking that casted computation is the same
44 ✗ BOOST_CHECK(casted_cone.get_R().isApprox(R));
45 ✗ BOOST_CHECK(casted_cone.get_mu() == mu);
46 ✗ BOOST_CHECK(casted_cone.get_nf() == nf);
47 ✗ BOOST_CHECK(casted_cone.get_inner_appr() == inner_appr);
48 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_A().rows()) == nf + 1);
49 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_lb().size()) == nf + 1);
50 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_ub().size()) == nf + 1);
51
52 // With rotation
53 ✗ Eigen::Quaterniond q;
54 ✗ pinocchio::quaternion::uniformRandom(q);
55 ✗ R = q.toRotationMatrix();
56
57 // Create the friction cone
58 ✗ cone = crocoddyl::FrictionCone(R, mu, nf, inner_appr);
59 ✗ casted_cone = cone.cast<double>();
60
61 ✗ BOOST_CHECK(cone.get_R().isApprox(R));
62 ✗ BOOST_CHECK(cone.get_mu() == mu);
63 ✗ BOOST_CHECK(cone.get_nf() == nf);
64 ✗ BOOST_CHECK(cone.get_inner_appr() == inner_appr);
65 ✗ BOOST_CHECK(static_cast<std::size_t>(cone.get_A().rows()) == nf + 1);
66 ✗ BOOST_CHECK(static_cast<std::size_t>(cone.get_lb().size()) == nf + 1);
67 ✗ BOOST_CHECK(static_cast<std::size_t>(cone.get_ub().size()) == nf + 1);
68
69 // Checking that casted computation is the same
70 ✗ BOOST_CHECK(casted_cone.get_R().isApprox(R));
71 ✗ BOOST_CHECK(casted_cone.get_mu() == mu);
72 ✗ BOOST_CHECK(casted_cone.get_nf() == nf);
73 ✗ BOOST_CHECK(casted_cone.get_inner_appr() == inner_appr);
74 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_A().rows()) == nf + 1);
75 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_lb().size()) == nf + 1);
76 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_cone.get_ub().size()) == nf + 1);
77
78 // Create the friction cone from a reference
79 {
80 ✗ crocoddyl::FrictionCone cone_reference(cone);
81 ✗ casted_cone = cone_reference.cast<double>();
82
83 ✗ BOOST_CHECK(cone.get_nf() == cone_reference.get_nf());
84 ✗ BOOST_CHECK(cone.get_A().isApprox(cone_reference.get_A()));
85 ✗ for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
86 ++i) {
87 ✗ BOOST_CHECK(cone.get_ub()[i] == cone_reference.get_ub()[i]);
88 ✗ BOOST_CHECK(cone.get_lb()[i] == cone_reference.get_lb()[i]);
89 }
90 ✗ BOOST_CHECK(cone.get_R().isApprox(cone_reference.get_R()));
91 ✗ BOOST_CHECK(std::abs(cone.get_mu() - cone_reference.get_mu()) < 1e-9);
92 ✗ BOOST_CHECK(cone.get_inner_appr() == cone_reference.get_inner_appr());
93 ✗ BOOST_CHECK(std::abs(cone.get_min_nforce() -
94 cone_reference.get_min_nforce()) < 1e-9);
95 ✗ BOOST_CHECK(cone.get_max_nforce() == cone_reference.get_max_nforce());
96
97 // Checking that casted computation is the same
98 ✗ BOOST_CHECK(casted_cone.get_nf() == cone_reference.get_nf());
99 ✗ BOOST_CHECK(casted_cone.get_A().isApprox(cone_reference.get_A()));
100 ✗ for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
101 ++i) {
102 ✗ BOOST_CHECK(casted_cone.get_ub()[i] == cone_reference.get_ub()[i]);
103 ✗ BOOST_CHECK(casted_cone.get_lb()[i] == cone_reference.get_lb()[i]);
104 }
105 ✗ BOOST_CHECK(casted_cone.get_R().isApprox(cone_reference.get_R()));
106 ✗ BOOST_CHECK(std::abs(casted_cone.get_mu() - cone_reference.get_mu()) <
107 1e-9);
108 ✗ BOOST_CHECK(cone.get_inner_appr() == cone_reference.get_inner_appr());
109 ✗ BOOST_CHECK(std::abs(casted_cone.get_min_nforce() -
110 cone_reference.get_min_nforce()) < 1e-9);
111 ✗ BOOST_CHECK(casted_cone.get_max_nforce() ==
112 cone_reference.get_max_nforce());
113 }
114
115 // Create the friction cone through the copy operator
116 {
117 ✗ crocoddyl::FrictionCone cone_copy;
118 ✗ cone_copy = cone;
119 ✗ casted_cone = cone_copy.cast<double>();
120
121 ✗ BOOST_CHECK(cone.get_nf() == cone_copy.get_nf());
122 ✗ BOOST_CHECK(cone.get_A().isApprox(cone_copy.get_A()));
123 ✗ for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
124 ++i) {
125 ✗ BOOST_CHECK(cone.get_ub()[i] == cone_copy.get_ub()[i]);
126 ✗ BOOST_CHECK(cone.get_lb()[i] == cone_copy.get_lb()[i]);
127 }
128 ✗ BOOST_CHECK(cone.get_R().isApprox(cone_copy.get_R()));
129 ✗ BOOST_CHECK(std::abs(cone.get_mu() - cone_copy.get_mu()) < 1e-9);
130 ✗ BOOST_CHECK(cone.get_inner_appr() == cone_copy.get_inner_appr());
131 ✗ BOOST_CHECK(std::abs(cone.get_min_nforce() - cone_copy.get_min_nforce()) <
132 1e-9);
133 ✗ BOOST_CHECK(cone.get_max_nforce() == cone_copy.get_max_nforce());
134
135 // Checking that casted computation is the same
136 ✗ BOOST_CHECK(casted_cone.get_nf() == cone_copy.get_nf());
137 ✗ BOOST_CHECK(casted_cone.get_A().isApprox(cone_copy.get_A()));
138 ✗ for (std::size_t i = 0; i < static_cast<std::size_t>(cone.get_ub().size());
139 ++i) {
140 ✗ BOOST_CHECK(casted_cone.get_ub()[i] == cone_copy.get_ub()[i]);
141 ✗ BOOST_CHECK(casted_cone.get_lb()[i] == cone_copy.get_lb()[i]);
142 }
143 ✗ BOOST_CHECK(casted_cone.get_R().isApprox(cone_copy.get_R()));
144 ✗ BOOST_CHECK(std::abs(casted_cone.get_mu() - cone_copy.get_mu()) < 1e-9);
145 ✗ BOOST_CHECK(casted_cone.get_inner_appr() == cone_copy.get_inner_appr());
146 ✗ BOOST_CHECK(std::abs(casted_cone.get_min_nforce() -
147 cone_copy.get_min_nforce()) < 1e-9);
148 ✗ BOOST_CHECK(casted_cone.get_max_nforce() == cone_copy.get_max_nforce());
149 }
150 }
151
152 ✗ void test_inner_approximation_of_friction_cone() {
153 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
154 ✗ double mu = random_real_in_range(0.01, 1.);
155 ✗ std::size_t nf = 2 * random_int_in_range(2, 16);
156 ✗ bool inner_appr = true;
157 ✗ crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
158 ✗ crocoddyl::FrictionCone casted_cone = cone.cast<double>();
159 ✗ const Eigen::VectorXd A_mu = -cone.get_A().col(2);
160 ✗ for (std::size_t i = 0; i < nf; ++i) {
161 ✗ BOOST_CHECK_CLOSE(
162 A_mu(i),
163 mu * cos((2 * crocoddyl::pi<double>() / static_cast<double>(nf)) / 2.),
164 1e-9);
165 }
166
167 // Checking that casted computation is the same
168 ✗ const Eigen::VectorXd A_mu_casted = -cone.get_A().col(2);
169 ✗ for (std::size_t i = 0; i < nf; ++i) {
170 ✗ BOOST_CHECK_CLOSE(
171 A_mu_casted(i),
172 mu * cos((2 * crocoddyl::pi<double>() / static_cast<double>(nf)) / 2.),
173 1e-9);
174 }
175 }
176
177 ✗ void test_A_matrix_with_rotation_change() {
178 // Common parameters
179 ✗ double mu = random_real_in_range(0.01, 1.);
180 ✗ std::size_t nf = 2 * random_int_in_range(2, 16);
181 ✗ bool inner_appr = false;
182
183 // No rotation
184 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
185 ✗ crocoddyl::FrictionCone cone_1(R, mu, nf, inner_appr);
186 ✗ crocoddyl::FrictionCone casted_cone_1 = cone_1.cast<double>();
187
188 // With rotation
189 ✗ Eigen::Quaterniond q;
190 ✗ pinocchio::quaternion::uniformRandom(q);
191 ✗ R = q.toRotationMatrix();
192 ✗ crocoddyl::FrictionCone cone_2(R, mu, nf, inner_appr);
193 ✗ crocoddyl::FrictionCone casted_cone_2 = cone_2.cast<double>();
194
195 ✗ for (std::size_t i = 0; i < 5; ++i) {
196 ✗ BOOST_CHECK(
197 (cone_1.get_A().row(i) - cone_2.get_A().row(i) * R).isZero(1e-9));
198 }
199
200 // Checking that casted computation is the same
201 ✗ for (std::size_t i = 0; i < 5; ++i) {
202 ✗ BOOST_CHECK(
203 (casted_cone_1.get_A().row(i) - casted_cone_2.get_A().row(i) * R)
204 .isZero(1e-9));
205 }
206 }
207
208 ✗ void test_force_along_friction_cone_normal() {
209 // Create the friction cone
210 ✗ Eigen::Quaterniond q;
211 ✗ pinocchio::quaternion::uniformRandom(q);
212 ✗ Eigen::Matrix3d R = q.toRotationMatrix();
213 ✗ double mu = random_real_in_range(0.01, 1.);
214 ✗ std::size_t nf = 2 * random_int_in_range(2, 16);
215 ✗ bool inner_appr = false;
216 ✗ crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
217
218 // Create the activation for quadratic barrier
219 ✗ crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
220 ✗ crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
221 crocoddyl::ActivationModelQuadraticBarrier casted_activation =
222 ✗ activation.cast<double>();
223 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data =
224 ✗ activation.createData();
225 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data =
226 ✗ casted_activation.createData();
227
228 // Compute the activation value
229 ✗ Eigen::Vector3d force = random_real_in_range(0., 100.) * R.col(2);
230 ✗ Eigen::VectorXd r = cone.get_A() * force;
231 ✗ activation.calc(data, r);
232
233 // The activation value has to be zero since the force is inside the friction
234 // cone
235 ✗ BOOST_CHECK(data->a_value == 0.);
236
237 // Checking that casted computation is the same
238 ✗ casted_activation.calc(casted_data, r);
239 ✗ BOOST_CHECK(casted_data->a_value == 0.);
240 }
241
242 ✗ void test_negative_force_along_friction_cone_normal() {
243 // Create the friction cone
244 ✗ Eigen::Quaterniond q;
245 ✗ pinocchio::quaternion::uniformRandom(q);
246 ✗ Eigen::Matrix3d R = q.toRotationMatrix();
247 ✗ double mu = random_real_in_range(0.01, 1.);
248 ✗ std::size_t nf = 2 * random_int_in_range(2, 16);
249 ✗ bool inner_appr = false;
250 ✗ crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
251 ✗ crocoddyl::FrictionCone casted_cone = cone.cast<double>();
252
253 // Create the activation for quadratic barrier
254 ✗ crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
255 ✗ crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
256 crocoddyl::ActivationModelQuadraticBarrier casted_activation =
257 ✗ activation.cast<double>();
258 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data =
259 ✗ activation.createData();
260 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data =
261 ✗ casted_activation.createData();
262
263 // Compute the activation value
264 ✗ Eigen::Vector3d force = -random_real_in_range(0., 100.) * R.col(2);
265 ✗ Eigen::VectorXd r = cone.get_A() * force;
266
267 // The first nf elements of the residual has to be positive since the force is
268 // outside the friction cone. Additionally, the last value has to be equals to
269 // the force norm but with negative value since the forces is aligned and in
270 // opposite direction to the friction cone orientation
271 ✗ for (std::size_t i = 0; i < nf; ++i) {
272 ✗ BOOST_CHECK(r(i) > 0.);
273 }
274 ✗ BOOST_CHECK_CLOSE(r(nf), -force.norm(), 1e-9);
275
276 // The activation value has to be positive since the force is outside the
277 // friction cone
278 ✗ activation.calc(data, r);
279 ✗ BOOST_CHECK(data->a_value > 0.);
280
281 // Checking that casted computation is the same
282 ✗ r = casted_cone.get_A() * force;
283 ✗ for (std::size_t i = 0; i < nf; ++i) {
284 ✗ BOOST_CHECK(r(i) > 0.);
285 }
286 ✗ BOOST_CHECK_CLOSE(r(nf), -force.norm(), 1e-9);
287 ✗ casted_activation.calc(casted_data, r);
288 ✗ BOOST_CHECK(casted_data->a_value > 0.);
289 }
290
291 ✗ void test_force_parallel_to_friction_cone_normal() {
292 // Create the friction cone
293 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
294 ✗ double mu = random_real_in_range(0.01, 1.);
295 ✗ std::size_t nf = 2 * random_int_in_range(2, 16);
296 ✗ bool inner_appr = false;
297 ✗ crocoddyl::FrictionCone cone(R, mu, nf, inner_appr);
298 ✗ crocoddyl::FrictionCone casted_cone = cone.cast<double>();
299
300 // Create the activation for quadratic barrier
301 ✗ crocoddyl::ActivationBounds bounds(cone.get_lb(), cone.get_ub());
302 ✗ crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
303 crocoddyl::ActivationModelQuadraticBarrier casted_activation =
304 ✗ activation.cast<double>();
305 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data =
306 ✗ activation.createData();
307 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data =
308 ✗ casted_activation.createData();
309
310 // Compute the activation value
311 Eigen::Vector3d force =
312 ✗ -random_real_in_range(0., 100.) * Eigen::Vector3d::UnitX();
313 ✗ Eigen::VectorXd r = cone.get_A() * force;
314
315 // The last value of the residual is equals to zero since the force is
316 // parallel to the friction cone orientation
317 ✗ BOOST_CHECK_CLOSE(r(nf), 0., 1e-9);
318
319 // The activation value has to be positive since the force is outside the
320 // friction cone
321 ✗ activation.calc(data, r);
322 ✗ BOOST_CHECK(data->a_value > 0.);
323
324 // Checking that casted computation is the same
325 ✗ r = casted_cone.get_A() * force;
326 ✗ BOOST_CHECK_CLOSE(r(nf), 0., 1e-9);
327 ✗ casted_activation.calc(casted_data, r);
328 ✗ BOOST_CHECK(casted_data->a_value > 0.);
329 }
330
331 ✗ void register_unit_tests() {
332 ✗ framework::master_test_suite().add(
333 ✗ BOOST_TEST_CASE(boost::bind(&test_constructor)));
334 ✗ framework::master_test_suite().add(
335 ✗ BOOST_TEST_CASE(boost::bind(&test_inner_approximation_of_friction_cone)));
336 ✗ framework::master_test_suite().add(
337 ✗ BOOST_TEST_CASE(boost::bind(&test_A_matrix_with_rotation_change)));
338 ✗ framework::master_test_suite().add(
339 ✗ BOOST_TEST_CASE(boost::bind(&test_force_along_friction_cone_normal)));
340 ✗ framework::master_test_suite().add(BOOST_TEST_CASE(
341 boost::bind(&test_negative_force_along_friction_cone_normal)));
342 ✗ framework::master_test_suite().add(BOOST_TEST_CASE(
343 boost::bind(&test_force_parallel_to_friction_cone_normal)));
344 }
345
346 ✗ bool init_function() {
347 ✗ register_unit_tests();
348 ✗ return true;
349 }
350
351 ✗ int main(int argc, char* argv[]) {
352 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
353 }
354