GCC Code Coverage Report

Directory: ./
File: unittest/test_contact_constraints.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 51 51 100.0%
Branches: 115 220 52.3%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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 "factory/contact_constraint.hpp"
13 #include "unittest_common.hpp"
14
15 using namespace boost::unit_test;
16 using namespace crocoddyl::unittest;
17
18 //----------------------------------------------------------------------------//
19
20 13 void test_partial_derivatives_against_contact_numdiff(
21 ContactConstraintModelTypes::Type constraint_type,
22 PinocchioModelTypes::Type model_type,
23 ActuationModelTypes::Type actuation_type) {
24 // create the model
25 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstract> &model =
26
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 ContactConstraintModelFactory().create(constraint_type, model_type,
27
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 actuation_type);
28
29 // create the corresponding data object and set the constraint to nan
30 const std::shared_ptr<crocoddyl::DifferentialActionDataAbstract> &data =
31
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 model->createData();
32
33
1/2
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
 13 crocoddyl::DifferentialActionModelNumDiff model_num_diff(model);
34 const std::shared_ptr<crocoddyl::DifferentialActionDataAbstract>
35
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 &data_num_diff = model_num_diff.createData();
36
37 // Generating random values for the state and control
38
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 Eigen::VectorXd x = model->get_state()->rand();
39
3/6
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 13 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
40
41 // Computing the action derivatives
42
3/6
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 13 model->calc(data, x, u);
43
3/6
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 13 model->calcDiff(data, x, u);
44
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 model_num_diff.calc(data_num_diff, x, u);
45
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 model_num_diff.calcDiff(data_num_diff, x, u);
46 // Tolerance defined as in
47 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
48
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
49
8/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 13 times.
 13 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
50
8/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 13 times.
 13 BOOST_CHECK((data->Gu - data_num_diff->Gu).isZero(tol));
51
8/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 13 times.
 13 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
52
8/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 13 times.
 13 BOOST_CHECK((data->Hu - data_num_diff->Hu).isZero(tol));
53
54 // Computing the action derivatives
55
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 x = model->get_state()->rand();
56
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 model->calc(data, x);
57
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 model->calcDiff(data, x);
58
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 model_num_diff.calc(data_num_diff, x);
59
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 model_num_diff.calcDiff(data_num_diff, x);
60
8/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 13 times.
 13 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
61
8/16
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 13 times.
 13 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
62
63 // Checking that casted computation is the same
64 #ifdef NDEBUG // Run only in release mode
65 const std::shared_ptr<crocoddyl::DifferentialActionModelAbstractTpl<float>>
66 &casted_model = model->cast<float>();
67 const std::shared_ptr<crocoddyl::DifferentialActionDataAbstractTpl<float>>
68 &casted_data = casted_model->createData();
69 Eigen::VectorXf x_f = x.cast<float>();
70 const Eigen::VectorXf u_f = u.cast<float>();
71 model->calc(data, x, u);
72 model->calcDiff(data, x, u);
73 casted_model->calc(casted_data, x_f, u_f);
74 casted_model->calcDiff(casted_data, x_f, u_f);
75 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
76 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
77 BOOST_CHECK((data->Gu.cast<float>() - casted_data->Gu).isZero(tol_f));
78 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
79 BOOST_CHECK((data->Hu.cast<float>() - casted_data->Hu).isZero(tol_f));
80 model->calc(data, x);
81 model->calcDiff(data, x);
82 casted_model->calc(casted_data, x_f);
83 casted_model->calcDiff(casted_data, x_f);
84 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
85 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
86 #endif
87 13 }
88
89 //----------------------------------------------------------------------------//
90
91 13 void register_contact_constraint_model_unit_tests(
92 ContactConstraintModelTypes::Type constraint_type,
93 PinocchioModelTypes::Type model_type,
94 ActuationModelTypes::Type actuation_type) {
95
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 boost::test_tools::output_test_stream test_name;
96
5/10
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
 13 test_name << "test_" << constraint_type << "_" << actuation_type << "_"
97
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 << model_type;
98
4/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
 13 std::cout << "Running " << test_name.str() << std::endl;
99
4/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
 13 test_suite *ts = BOOST_TEST_SUITE(test_name.str());
100
5/10
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 13 times.
✗ Branch 15 not taken.
 13 ts->add(BOOST_TEST_CASE(
101 boost::bind(&test_partial_derivatives_against_contact_numdiff,
102 constraint_type, model_type, actuation_type)));
103
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 13 framework::master_test_suite().add(ts);
104 13 }
105
106 1 bool init_function() {
107 // Test all the contact constraint model. Note that we can do it only with
108 // humanoids as it needs to test the contact wrench cone
109 6 for (size_t constraint_type = 0;
110
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 constraint_type < ContactConstraintModelTypes::all.size();
111 ++constraint_type) {
112 5 register_contact_constraint_model_unit_tests(
113 5 ContactConstraintModelTypes::all[constraint_type],
114 PinocchioModelTypes::Talos,
115 ActuationModelTypes::ActuationModelFloatingBase);
116 5 register_contact_constraint_model_unit_tests(
117 5 ContactConstraintModelTypes::all[constraint_type],
118 PinocchioModelTypes::RandomHumanoid,
119 ActuationModelTypes::ActuationModelFloatingBase);
120 5 if (ContactConstraintModelTypes::all[constraint_type] ==
121 ContactConstraintModelTypes::
122 4 ConstraintModelResidualContactForceEquality ||
123
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 ContactConstraintModelTypes::all[constraint_type] ==
124 ContactConstraintModelTypes::
125
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2 times.
 9 ConstraintModelResidualContactFrictionConeInequality ||
126
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
 3 ContactConstraintModelTypes::all[constraint_type] ==
127 ContactConstraintModelTypes::
128 ConstraintModelResidualContactControlGravInequality) {
129 3 register_contact_constraint_model_unit_tests(
130 3 ContactConstraintModelTypes::all[constraint_type],
131 PinocchioModelTypes::HyQ,
132 ActuationModelTypes::ActuationModelFloatingBase);
133 }
134 }
135 1 return true;
136 }
137
138 1 int main(int argc, char **argv) {
139 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
140 }
141