GCC Code Coverage Report

Directory: ./
File: unittest/test_impulse_costs.cpp
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 48 52 92.3%
Branches: 89 232 38.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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 "factory/impulse_cost.hpp"
13 #include "unittest_common.hpp"
14
15 using namespace boost::unit_test;
16 using namespace crocoddyl::unittest;
17
18 //----------------------------------------------------------------------------//
19
20 42 void test_partial_derivatives_against_impulse_numdiff(
21 ImpulseCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
22 ActivationModelTypes::Type activation_type) {
23 // create the model
24 const std::shared_ptr<crocoddyl::ActionModelAbstract>& model =
25
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 ImpulseCostModelFactory().create(cost_type, model_type, activation_type);
26
27 // create the corresponding data object and set the cost to nan
28 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data =
29
1/2
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 42 model->createData();
30
31
1/2
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 42 crocoddyl::ActionModelNumDiff model_num_diff(model);
32 const std::shared_ptr<crocoddyl::ActionDataAbstract>& data_num_diff =
33
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 model_num_diff.createData();
34
35 // Generating random values for the state and control
36
1/2
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 Eigen::VectorXd x = model->get_state()->rand();
37
2/4
✓ Branch 3 taken 42 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 42 times.
✗ Branch 7 not taken.
 42 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
38
39 // Computing the action derivatives
40
3/6
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
 42 model->calc(data, x, u);
41
3/6
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
 42 model->calcDiff(data, x, u);
42
3/6
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
 42 model_num_diff.calc(data_num_diff, x, u);
43
3/6
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
 42 model_num_diff.calcDiff(data_num_diff, x, u);
44 // Tolerance defined as in
45 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
46 42 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
47
8/16
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 42 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 42 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 42 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 42 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 42 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 42 times.
 42 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
48
8/16
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 42 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 42 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 42 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 42 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 42 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 42 times.
 42 BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol));
49
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 42 times.
 42 if (model_num_diff.get_with_gauss_approx()) {
50 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
51 ✗ BOOST_CHECK((data->Lxu - data_num_diff->Lxu).isZero(tol));
52 ✗ BOOST_CHECK((data->Luu - data_num_diff->Luu).isZero(tol));
53 }
54
55 // Computing the action derivatives
56
1/2
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 x = model->get_state()->rand();
57
2/4
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
 42 model->calc(data, x);
58
2/4
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
 42 model->calcDiff(data, x);
59
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 model_num_diff.calc(data_num_diff, x);
60
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 model_num_diff.calcDiff(data_num_diff, x);
61
62 // Checking the partial derivatives against numerical differentiation
63
8/16
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 42 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 42 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 42 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 42 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 42 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 42 times.
 42 BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol));
64
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 42 times.
 42 if (model_num_diff.get_with_gauss_approx()) {
65 ✗ BOOST_CHECK((data->Lxx - data_num_diff->Lxx).isZero(tol));
66 }
67 42 }
68
69 //----------------------------------------------------------------------------//
70
71 42 void register_impulse_cost_model_unit_tests(
72 ImpulseCostModelTypes::Type cost_type, PinocchioModelTypes::Type model_type,
73 ActivationModelTypes::Type activation_type) {
74
2/4
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
 42 boost::test_tools::output_test_stream test_name;
75
5/10
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 42 times.
✗ Branch 14 not taken.
 42 test_name << "test_" << cost_type << "_" << activation_type << "_"
76
1/2
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
 42 << model_type;
77
4/8
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42 times.
✗ Branch 11 not taken.
 42 std::cout << "Running " << test_name.str() << std::endl;
78
4/8
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 42 times.
✗ Branch 12 not taken.
 42 test_suite* ts = BOOST_TEST_SUITE(test_name.str());
79
5/10
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 42 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 42 times.
✗ Branch 15 not taken.
 42 ts->add(BOOST_TEST_CASE(
80 boost::bind(&test_partial_derivatives_against_impulse_numdiff, cost_type,
81 model_type, activation_type)));
82
3/6
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 42 times.
✗ Branch 7 not taken.
 42 framework::master_test_suite().add(ts);
83 42 }
84
85 1 bool init_function() {
86 // Test all the impulse cost model. Note that we can do it only with humanoids
87 // as it needs to test the contact wrench cone
88
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 for (std::size_t cost_type = 0; cost_type < ImpulseCostModelTypes::all.size();
89 ++cost_type) {
90 35 for (std::size_t activation_type = 0;
91
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 5 times.
 35 activation_type <
92 ActivationModelTypes::ActivationModelQuadraticBarrier;
93 ++activation_type) {
94 30 register_impulse_cost_model_unit_tests(
95 30 ImpulseCostModelTypes::all[cost_type], PinocchioModelTypes::Talos,
96 30 ActivationModelTypes::all[activation_type]);
97 30 if (ImpulseCostModelTypes::all[cost_type] ==
98
4/4
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 18 times.
 54 ImpulseCostModelTypes::CostModelResidualContactForce ||
99
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 18 times.
 24 ImpulseCostModelTypes::all[cost_type] ==
100 ImpulseCostModelTypes::CostModelResidualContactFrictionCone) {
101 12 register_impulse_cost_model_unit_tests(
102 12 ImpulseCostModelTypes::all[cost_type], PinocchioModelTypes::HyQ,
103 12 ActivationModelTypes::all[activation_type]);
104 }
105 }
106 }
107
108 1 return true;
109 }
110
111 1 int main(int argc, char** argv) {
112 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
113 }
114