GCC Code Coverage Report

Directory: ./
File: unittest/test_controls.cpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 102 0.0%
Functions: 0 6 0.0%
Branches: 0 432 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, LAAS-CNRS, New York University,
5 // Max Planck Gesellschaft,
6 // University of Edinburgh, INRIA,
7 // University of Trento, Heriot-Watt University
8 // Copyright note valid unless otherwise controld in individual files.
9 // All rights reserved.
10 ///////////////////////////////////////////////////////////////////////////////
11
12 #define BOOST_TEST_NO_MAIN
13 #define BOOST_TEST_ALTERNATIVE_INIT_API
14
15 #include "crocoddyl/core/numdiff/control.hpp"
16 #include "factory/control.hpp"
17 #include "unittest_common.hpp"
18
19 using namespace boost::unit_test;
20 using namespace crocoddyl::unittest;
21
22 //----------------------------------------------------------------------------//
23
24 ✗ void test_calcDiff_num_diff(ControlTypes::Type control_type) {
25 ✗ ControlFactory factory;
26 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>&
27 ✗ control = factory.create(control_type, 10);
28
29 const std::shared_ptr<crocoddyl::ControlParametrizationDataAbstract>& data =
30 ✗ control->createData();
31
32 // Generating random values for the control parameters
33 ✗ const Eigen::VectorXd p = Eigen::VectorXd::Random(control->get_nu());
34 ✗ double t = Eigen::VectorXd::Random(1)(0) * 0.5 + 1.; // random in [0, 1]
35
36 // Get the num diff control
37 ✗ crocoddyl::ControlParametrizationModelNumDiff control_num_diff(control);
38 std::shared_ptr<crocoddyl::ControlParametrizationDataAbstract> data_num_diff =
39 ✗ control_num_diff.createData();
40
41 // Computing the partial derivatives of the value function
42 ✗ control->calc(data, t, p);
43 ✗ control_num_diff.calc(data_num_diff, t, p);
44 ✗ control->calcDiff(data, t, p);
45 ✗ control_num_diff.calcDiff(data_num_diff, t, p);
46 // Tolerance defined as in
47 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
48 ✗ double tol = std::pow(control_num_diff.get_disturbance(), 1. / 3.);
49 ✗ BOOST_CHECK((data->dw_du - data_num_diff->dw_du).isZero(tol));
50
51 // Checking that casted computation is the same
52 const std::shared_ptr<
53 crocoddyl::ControlParametrizationModelAbstractTpl<float>>&
54 ✗ casted_control = control->cast<float>();
55 const std::shared_ptr<
56 crocoddyl::ControlParametrizationDataAbstractTpl<float>>& casted_data =
57 ✗ casted_control->createData();
58 ✗ const Eigen::VectorXf p_f = p.cast<float>();
59 ✗ float t_f = float(t);
60 ✗ casted_control->calc(casted_data, t_f, p_f);
61 ✗ casted_control->calcDiff(casted_data, t_f, p_f);
62 ✗ float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
63 ✗ BOOST_CHECK((data->dw_du.cast<float>() - casted_data->dw_du).isZero(tol_f));
64 ✗ }
65
66 ✗ void test_multiplyByJacobian_num_diff(ControlTypes::Type control_type) {
67 ✗ ControlFactory factory;
68 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>&
69 ✗ control = factory.create(control_type, 10);
70
71 std::shared_ptr<crocoddyl::ControlParametrizationDataAbstract> data =
72 ✗ control->createData();
73
74 // Generating random values for the control parameters, the time, and the
75 // matrix to multiply
76 ✗ const Eigen::VectorXd p = Eigen::VectorXd::Random(control->get_nu());
77 ✗ double t = Eigen::VectorXd::Random(1)(0) * 0.5 + 1.; // random in [0, 1]
78 ✗ const Eigen::MatrixXd A = Eigen::MatrixXd::Random(5, control->get_nw());
79
80 // Get the num diff control and datas
81 ✗ crocoddyl::ControlParametrizationModelNumDiff control_num_diff(control);
82 std::shared_ptr<crocoddyl::ControlParametrizationDataAbstract> data_num_diff =
83 ✗ control_num_diff.createData();
84
85 // Checking the operator
86 ✗ Eigen::MatrixXd A_J(Eigen::MatrixXd::Zero(A.rows(), control->get_nu()));
87 Eigen::MatrixXd A_J_num_diff(
88 ✗ Eigen::MatrixXd::Zero(A.rows(), control->get_nu()));
89 ✗ control->calc(data, t, p);
90 ✗ control->calcDiff(data, t, p);
91 ✗ control_num_diff.calc(data_num_diff, t, p);
92 ✗ control_num_diff.calcDiff(data_num_diff, t, p);
93 ✗ control->multiplyByJacobian(data, A, A_J);
94 ✗ control_num_diff.multiplyByJacobian(data_num_diff, A, A_J_num_diff);
95 // Tolerance defined as in
96 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
97 ✗ double tol = std::pow(control_num_diff.get_disturbance(), 1. / 3.);
98 ✗ BOOST_CHECK((A_J - A_J_num_diff).isZero(tol));
99
100 // Checking that casted computation is the same
101 const std::shared_ptr<
102 crocoddyl::ControlParametrizationModelAbstractTpl<float>>&
103 ✗ casted_control = control->cast<float>();
104 const std::shared_ptr<
105 crocoddyl::ControlParametrizationDataAbstractTpl<float>>& casted_data =
106 ✗ casted_control->createData();
107 Eigen::MatrixXf A_J_f(
108 ✗ Eigen::MatrixXf::Zero(A.rows(), casted_control->get_nu()));
109 ✗ const Eigen::VectorXf p_f = p.cast<float>();
110 ✗ float t_f = float(t);
111 ✗ const Eigen::MatrixXf A_f = A.cast<float>();
112 ✗ casted_control->calc(casted_data, t_f, p_f);
113 ✗ casted_control->calcDiff(casted_data, t_f, p_f);
114 ✗ casted_control->multiplyByJacobian(casted_data, A_f, A_J_f);
115 ✗ float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
116 ✗ BOOST_CHECK((data->dw_du.cast<float>() - casted_data->dw_du).isZero(tol_f));
117 ✗ BOOST_CHECK((A_J.cast<float>() - A_J_f).isZero(tol_f));
118 ✗ }
119
120 ✗ void test_multiplyJacobianTransposeBy_num_diff(
121 ControlTypes::Type control_type) {
122 ✗ ControlFactory factory;
123 const std::shared_ptr<crocoddyl::ControlParametrizationModelAbstract>&
124 ✗ control = factory.create(control_type, 10);
125
126 std::shared_ptr<crocoddyl::ControlParametrizationDataAbstract> data =
127 ✗ control->createData();
128
129 // Generating random values for the control parameters, the time, and the
130 // matrix to multiply
131 ✗ const Eigen::VectorXd p = Eigen::VectorXd::Random(control->get_nu());
132 ✗ double t = Eigen::VectorXd::Random(1)(0) * 0.5 + 1.; // random in [0, 1]
133 ✗ const Eigen::MatrixXd A = Eigen::MatrixXd::Random(control->get_nw(), 5);
134
135 // Get the num diff control and datas
136 ✗ crocoddyl::ControlParametrizationModelNumDiff control_num_diff(control);
137 std::shared_ptr<crocoddyl::ControlParametrizationDataAbstract> data_num_diff =
138 ✗ control_num_diff.createData();
139
140 // Checking the operator
141 ✗ Eigen::MatrixXd JT_A(Eigen::MatrixXd::Zero(control->get_nu(), A.cols()));
142 Eigen::MatrixXd JT_A_num_diff(
143 ✗ Eigen::MatrixXd::Zero(control->get_nu(), A.cols()));
144 ✗ control->calc(data, t, p);
145 ✗ control->calcDiff(data, t, p);
146 ✗ control_num_diff.calc(data_num_diff, t, p);
147 ✗ control_num_diff.calcDiff(data_num_diff, t, p);
148 ✗ control->multiplyJacobianTransposeBy(data, A, JT_A);
149 ✗ control_num_diff.multiplyJacobianTransposeBy(data_num_diff, A, JT_A_num_diff);
150 // Tolerance defined as in
151 // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf
152 ✗ double tol = std::pow(control_num_diff.get_disturbance(), 1. / 3.);
153 ✗ BOOST_CHECK((JT_A - JT_A_num_diff).isZero(tol));
154
155 // Checking that casted computation is the same
156 const std::shared_ptr<
157 crocoddyl::ControlParametrizationModelAbstractTpl<float>>&
158 ✗ casted_control = control->cast<float>();
159 const std::shared_ptr<
160 crocoddyl::ControlParametrizationDataAbstractTpl<float>>& casted_data =
161 ✗ casted_control->createData();
162 ✗ const Eigen::VectorXf p_f = p.cast<float>();
163 ✗ float t_f = float(t);
164 ✗ const Eigen::MatrixXf A_f = A.cast<float>();
165 Eigen::MatrixXf JT_A_f(
166 ✗ Eigen::MatrixXf::Zero(casted_control->get_nu(), A_f.cols()));
167 ✗ casted_control->calc(casted_data, t_f, p_f);
168 ✗ casted_control->calcDiff(casted_data, t_f, p_f);
169 ✗ casted_control->multiplyJacobianTransposeBy(casted_data, A_f, JT_A_f);
170 ✗ float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon());
171 ✗ BOOST_CHECK((JT_A.cast<float>() - JT_A_f).isZero(tol_f));
172 ✗ }
173
174 //----------------------------------------------------------------------------//
175
176 ✗ void register_control_unit_tests(ControlTypes::Type control_type) {
177 ✗ boost::test_tools::output_test_stream test_name;
178 ✗ test_name << "test_" << control_type;
179 ✗ std::cout << "Running " << test_name.str() << std::endl;
180 ✗ test_suite* ts = BOOST_TEST_SUITE(test_name.str());
181 ✗ ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_num_diff, control_type)));
182 ✗ ts->add(BOOST_TEST_CASE(
183 boost::bind(&test_multiplyByJacobian_num_diff, control_type)));
184 ✗ ts->add(BOOST_TEST_CASE(
185 boost::bind(&test_multiplyJacobianTransposeBy_num_diff, control_type)));
186 ✗ framework::master_test_suite().add(ts);
187 ✗ }
188
189 ✗ bool init_function() {
190 ✗ for (size_t i = 0; i < ControlTypes::all.size(); ++i) {
191 ✗ register_control_unit_tests(ControlTypes::all[i]);
192 }
193 ✗ return true;
194 }
195
196 ✗ int main(int argc, char** argv) {
197 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
198 }
199