GCC Code Coverage Report

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