| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 boost::shared_ptr<crocoddyl::ActionModelAbstract>& model = |
||
25 |
✓✗✓✗ |
84 |
ImpulseCostModelFactory().create(cost_type, model_type, activation_type); |
26 |
|||
27 |
// create the corresponding data object and set the cost to nan |
||
28 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = |
||
29 |
✓✗ | 84 |
model->createData(); |
30 |
|||
31 |
✓✗ | 84 |
crocoddyl::ActionModelNumDiff model_num_diff(model); |
32 |
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data_num_diff = |
||
33 |
✓✗ | 84 |
model_num_diff.createData(); |
34 |
|||
35 |
// Generating random values for the state and control |
||
36 |
✓✗ | 84 |
Eigen::VectorXd x = model->get_state()->rand(); |
37 |
✓✗✓✗ |
84 |
const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu()); |
38 |
|||
39 |
// Computing the action derivatives |
||
40 |
✓✗✓✗ ✓✗ |
42 |
model->calc(data, x, u); |
41 |
✓✗✓✗ ✓✗ |
42 |
model->calcDiff(data, x, u); |
42 |
✓✗✓✗ ✓✗ |
42 |
model_num_diff.calc(data_num_diff, x, u); |
43 |
✓✗✓✗ ✓✗ |
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 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol)); |
48 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(tol)); |
49 |
✗✓ | 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 |
✓✗ | 42 |
x = model->get_state()->rand(); |
57 |
✓✗✓✗ |
42 |
model->calc(data, x); |
58 |
✓✗✓✗ |
42 |
model->calcDiff(data, x); |
59 |
✓✗✓✗ |
42 |
model_num_diff.calc(data_num_diff, x); |
60 |
✓✗✓✗ |
42 |
model_num_diff.calcDiff(data_num_diff, x); |
61 |
|||
62 |
// Checking the partial derivatives against numerical differentiation |
||
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
42 |
BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(tol)); |
64 |
✗✓ | 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 |
✓✗✓✗ |
84 |
boost::test_tools::output_test_stream test_name; |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
42 |
test_name << "test_" << cost_type << "_" << activation_type << "_" |
76 |
✓✗ | 42 |
<< model_type; |
77 |
✓✗✓✗ ✓✗✓✗ |
42 |
std::cout << "Running " << test_name.str() << std::endl; |
78 |
✓✗✓✗ ✓✗✓✗ |
42 |
test_suite* ts = BOOST_TEST_SUITE(test_name.str()); |
79 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
42 |
ts->add(BOOST_TEST_CASE( |
80 |
boost::bind(&test_partial_derivatives_against_impulse_numdiff, cost_type, |
||
81 |
model_type, activation_type))); |
||
82 |
✓✗✓✗ ✓✗ |
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 |
✓✓ | 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 |
✓✓ | 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 |
✓✓✓✓ |
54 |
ImpulseCostModelTypes::CostModelResidualContactForce || |
99 |
✓✓ | 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 |
} |
| Generated by: GCOVR (Version 4.2) |