GCC Code Coverage Report

Directory: ./
File: unittest/test_impulse_constraints.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 38 38 100.0%
Branches: 69 130 53.1%
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/impulse_constraint.hpp"
13 #include "unittest_common.hpp"
14
15 using namespace boost::unit_test;
16 using namespace crocoddyl::unittest;
17
18 //----------------------------------------------------------------------------//
19
20 12 void test_partial_derivatives_against_impulse_numdiff(
21 ImpulseConstraintModelTypes::Type constraint_type,
22 PinocchioModelTypes::Type model_type) {
23 // create the model
24 const std::shared_ptr<crocoddyl::ActionModelAbstract> &model =
25
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 ImpulseConstraintModelFactory().create(constraint_type, model_type);
26
27 // create the corresponding data object and set the constraint to nan
28 const std::shared_ptr<crocoddyl::ActionDataAbstract> &data =
29
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 model->createData();
30
31
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 crocoddyl::ActionModelNumDiff model_num_diff(model);
32 const std::shared_ptr<crocoddyl::ActionDataAbstract> &data_num_diff =
33
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 model_num_diff.createData();
34
35 // Generating random values for the state and control
36
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 12 const Eigen::VectorXd x = model->get_state()->rand();
37
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
 12 const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu());
38
39 // Computing the action derivatives
40
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
 12 model->calc(data, x, u);
41
3/6
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
 12 model->calcDiff(data, x, u);
42
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 model_num_diff.calc(data_num_diff, x, u);
43
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 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
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.);
47
8/16
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 12 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 12 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 12 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 12 times.
 12 BOOST_CHECK((data->Gx - data_num_diff->Gx).isZero(tol));
48
8/16
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 12 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 12 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 12 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 12 times.
 12 BOOST_CHECK((data->Hx - data_num_diff->Hx).isZero(tol));
49
50 // Checking that casted computation is the same
51 #ifdef NDEBUG // Run only in release mode
52 const std::shared_ptr<crocoddyl::ActionModelAbstractTpl<float>>
53 &casted_model = model->cast<float>();
54 const std::shared_ptr<crocoddyl::ActionDataAbstractTpl<float>> &casted_data =
55 casted_model->createData();
56 Eigen::VectorXf x_f = x.cast<float>();
57 const Eigen::VectorXf u_f = u.cast<float>();
58 model->calc(data, x, u);
59 model->calcDiff(data, x, u);
60 casted_model->calc(casted_data, x_f, u_f);
61 casted_model->calcDiff(casted_data, x_f, u_f);
62 float tol_f = 10.f * std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
63 BOOST_CHECK((data->Gx.cast<float>() - casted_data->Gx).isZero(tol_f));
64 BOOST_CHECK((data->Hx.cast<float>() - casted_data->Hx).isZero(tol_f));
65 #endif
66 12 }
67
68 //----------------------------------------------------------------------------//
69
70 12 void register_impulse_constraint_model_unit_tests(
71 ImpulseConstraintModelTypes::Type constraint_type,
72 PinocchioModelTypes::Type model_type) {
73
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 boost::test_tools::output_test_stream test_name;
74
4/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
 12 test_name << "test_" << constraint_type << "_" << model_type;
75
4/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
 12 std::cout << "Running " << test_name.str() << std::endl;
76
4/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
 12 test_suite *ts = BOOST_TEST_SUITE(test_name.str());
77
5/10
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12 times.
✗ Branch 15 not taken.
 12 ts->add(BOOST_TEST_CASE(
78 boost::bind(&test_partial_derivatives_against_impulse_numdiff,
79 constraint_type, model_type)));
80
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
 12 framework::master_test_suite().add(ts);
81 12 }
82
83 1 bool init_function() {
84 // Test all the impulse constraint model. Note that we can do it only with
85 // humanoids as it needs to test the impulse wrench cone
86 6 for (size_t constraint_type = 0;
87
2/2
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 1 times.
 6 constraint_type < ImpulseConstraintModelTypes::all.size();
88 ++constraint_type) {
89 5 register_impulse_constraint_model_unit_tests(
90 5 ImpulseConstraintModelTypes::all[constraint_type],
91 PinocchioModelTypes::Talos);
92 5 register_impulse_constraint_model_unit_tests(
93 5 ImpulseConstraintModelTypes::all[constraint_type],
94 PinocchioModelTypes::RandomHumanoid);
95 5 if (ImpulseConstraintModelTypes::all[constraint_type] ==
96 ImpulseConstraintModelTypes::
97
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 3 times.
 9 ConstraintModelResidualImpulseForceEquality ||
98
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 3 times.
 4 ImpulseConstraintModelTypes::all[constraint_type] ==
99 ImpulseConstraintModelTypes::
100 ConstraintModelResidualImpulseFrictionConeInequality) {
101 2 register_impulse_constraint_model_unit_tests(
102 2 ImpulseConstraintModelTypes::all[constraint_type],
103 PinocchioModelTypes::HyQ);
104 }
105 }
106
107 1 return true;
108 }
109
110 1 int main(int argc, char **argv) {
111 1 return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
112 }
113