GCC Code Coverage Report

Directory: ./
File: unittest/test_cop_support.cpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 129 0.0%
Branches: 0 1034 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, University of Oxford,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #define BOOST_TEST_NO_MAIN
11 #define BOOST_TEST_ALTERNATIVE_INIT_API
12
13 #include <pinocchio/math/quaternion.hpp>
14
15 #include "crocoddyl/core/activations/quadratic-barrier.hpp"
16 #include "crocoddyl/multibody/cop-support.hpp"
17 #include "unittest_common.hpp"
18
19 using namespace boost::unit_test;
20 using namespace crocoddyl::unittest;
21
22 ✗ void test_constructor() {
23 // Common parameters
24 ✗ Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
25 ✗ random_real_in_range(0.01, 0.1));
26
27 // No rotation
28 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
29
30 // Create the CoP support support
31 ✗ crocoddyl::CoPSupport support(R, box);
32 ✗ crocoddyl::CoPSupport casted_support = support.cast<double>();
33
34 ✗ BOOST_CHECK(support.get_R().isApprox(R));
35 ✗ BOOST_CHECK(support.get_box().isApprox(box));
36 ✗ BOOST_CHECK(static_cast<std::size_t>(support.get_A().rows()) == 4);
37 ✗ BOOST_CHECK(static_cast<std::size_t>(support.get_lb().size()) == 4);
38 ✗ BOOST_CHECK(static_cast<std::size_t>(support.get_ub().size()) == 4);
39
40 // Checking that casted computation is the same
41 ✗ BOOST_CHECK(casted_support.get_R().isApprox(R));
42 ✗ BOOST_CHECK(casted_support.get_box().isApprox(box));
43 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_support.get_A().rows()) == 4);
44 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_support.get_lb().size()) == 4);
45 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_support.get_ub().size()) == 4);
46
47 // With rotation
48 ✗ Eigen::Quaterniond q;
49 ✗ pinocchio::quaternion::uniformRandom(q);
50 ✗ R = q.toRotationMatrix();
51
52 // Create the wrench support
53 ✗ support = crocoddyl::CoPSupport(R, box);
54 ✗ casted_support = support.cast<double>();
55
56 ✗ BOOST_CHECK(support.get_R().isApprox(R));
57 ✗ BOOST_CHECK(support.get_box().isApprox(box));
58 ✗ BOOST_CHECK(static_cast<std::size_t>(support.get_A().rows()) == 4);
59 ✗ BOOST_CHECK(static_cast<std::size_t>(support.get_lb().size()) == 4);
60 ✗ BOOST_CHECK(static_cast<std::size_t>(support.get_ub().size()) == 4);
61
62 // Checking that casted computation is the same
63 ✗ BOOST_CHECK(casted_support.get_R().isApprox(R));
64 ✗ BOOST_CHECK(casted_support.get_box().isApprox(box));
65 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_support.get_A().rows()) == 4);
66 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_support.get_lb().size()) == 4);
67 ✗ BOOST_CHECK(static_cast<std::size_t>(casted_support.get_ub().size()) == 4);
68
69 // Create the wrench support from a reference
70 {
71 ✗ crocoddyl::CoPSupport support_from_reference(support);
72 ✗ casted_support = support_from_reference.cast<double>();
73
74 ✗ BOOST_CHECK(support.get_R().isApprox(support_from_reference.get_R()));
75 ✗ BOOST_CHECK(support.get_box().isApprox(support_from_reference.get_box()));
76 ✗ BOOST_CHECK(support.get_A().isApprox(support_from_reference.get_A()));
77 ✗ for (std::size_t i = 0;
78 ✗ i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
79 ✗ BOOST_CHECK(support.get_ub()[i] == support_from_reference.get_ub()[i]);
80 ✗ BOOST_CHECK(support.get_lb()[i] == support_from_reference.get_lb()[i]);
81 }
82
83 // Checking that casted computation is the same
84 ✗ BOOST_CHECK(
85 casted_support.get_R().isApprox(support_from_reference.get_R()));
86 ✗ BOOST_CHECK(
87 casted_support.get_box().isApprox(support_from_reference.get_box()));
88 ✗ BOOST_CHECK(
89 casted_support.get_A().isApprox(support_from_reference.get_A()));
90 ✗ for (std::size_t i = 0;
91 ✗ i < static_cast<std::size_t>(casted_support.get_ub().size()); ++i) {
92 ✗ BOOST_CHECK(casted_support.get_ub()[i] ==
93 support_from_reference.get_ub()[i]);
94 ✗ BOOST_CHECK(casted_support.get_lb()[i] ==
95 support_from_reference.get_lb()[i]);
96 }
97 }
98
99 // Create the wrench support through the copy operator
100 {
101 ✗ crocoddyl::CoPSupport support_from_copy;
102 ✗ support_from_copy = support;
103 ✗ casted_support = support_from_copy.cast<double>();
104
105 ✗ BOOST_CHECK(support.get_R().isApprox(support_from_copy.get_R()));
106 ✗ BOOST_CHECK(support.get_box().isApprox(support_from_copy.get_box()));
107 ✗ BOOST_CHECK(support.get_A().isApprox(support_from_copy.get_A()));
108 ✗ for (std::size_t i = 0;
109 ✗ i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
110 ✗ BOOST_CHECK(support.get_ub()[i] == support_from_copy.get_ub()[i]);
111 ✗ BOOST_CHECK(support.get_lb()[i] == support_from_copy.get_lb()[i]);
112 }
113
114 // Checking that casted computation is the same
115 ✗ BOOST_CHECK(casted_support.get_R().isApprox(support_from_copy.get_R()));
116 ✗ BOOST_CHECK(casted_support.get_box().isApprox(support_from_copy.get_box()));
117 ✗ BOOST_CHECK(casted_support.get_A().isApprox(support_from_copy.get_A()));
118 ✗ for (std::size_t i = 0;
119 ✗ i < static_cast<std::size_t>(support.get_ub().size()); ++i) {
120 ✗ BOOST_CHECK(casted_support.get_ub()[i] == support_from_copy.get_ub()[i]);
121 ✗ BOOST_CHECK(casted_support.get_lb()[i] == support_from_copy.get_lb()[i]);
122 }
123 }
124 }
125
126 ✗ void test_A_matrix_with_rotation_change() {
127 // Common parameters
128 ✗ Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
129 ✗ random_real_in_range(0.01, 0.1));
130
131 // No rotation
132 ✗ Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
133 ✗ crocoddyl::CoPSupport support_1(R, box);
134 ✗ crocoddyl::CoPSupport casted_support_1 = support_1.cast<double>();
135
136 // With rotation
137 ✗ Eigen::Quaterniond q;
138 ✗ pinocchio::quaternion::uniformRandom(q);
139 ✗ R = q.toRotationMatrix();
140 ✗ crocoddyl::CoPSupport support_2(R, box);
141 ✗ crocoddyl::CoPSupport casted_support_2 = support_2.cast<double>();
142
143 ✗ for (std::size_t i = 0; i < 4; ++i) {
144 ✗ BOOST_CHECK((support_1.get_A().row(i).head<3>() -
145 support_2.get_A().row(i).head<3>() * R)
146 .isZero(1e-9));
147 }
148
149 // Checking that casted computation is the same
150 ✗ for (std::size_t i = 0; i < 4; ++i) {
151 ✗ BOOST_CHECK((casted_support_1.get_A().row(i).head<3>() -
152 casted_support_2.get_A().row(i).head<3>() * R)
153 .isZero(1e-9));
154 }
155 }
156
157 ✗ void test_cop_within_the_support_region() {
158 // Create the CoP support with a random orientation
159 ✗ Eigen::Quaterniond q;
160 ✗ pinocchio::quaternion::uniformRandom(q);
161 ✗ Eigen::Matrix3d R = q.toRotationMatrix();
162 ✗ Eigen::Vector2d box = Eigen::Vector2d(random_real_in_range(0.01, 0.1),
163 ✗ random_real_in_range(0.01, 0.1));
164 ✗ crocoddyl::CoPSupport support(R, box);
165
166 // Create the activation for quadratic barrier
167 ✗ crocoddyl::ActivationBounds bounds(support.get_lb(), support.get_ub());
168 ✗ crocoddyl::ActivationModelQuadraticBarrier activation(bounds);
169 crocoddyl::ActivationModelQuadraticBarrier casted_activation =
170 ✗ activation.cast<double>();
171 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& data =
172 ✗ activation.createData();
173 const std::shared_ptr<crocoddyl::ActivationDataAbstract>& casted_data =
174 ✗ casted_activation.createData();
175
176 // Compute the activation value with a force along the contact normal
177 ✗ Eigen::VectorXd wrench(6);
178 ✗ wrench.setZero();
179 ✗ wrench.template head<3>() = random_real_in_range(0., 100.) * R.col(2);
180 ✗ Eigen::VectorXd r = support.get_A() * wrench;
181 ✗ activation.calc(data, r);
182 ✗ BOOST_CHECK(data->a_value == 0.);
183
184 // Checking that casted computation is the same
185 ✗ casted_activation.calc(casted_data, r);
186 ✗ BOOST_CHECK(casted_data->a_value == 0.);
187
188 // Create the CoP support with no rotation
189 ✗ R = Eigen::Matrix3d::Identity();
190 ✗ support = crocoddyl::CoPSupport(R, box);
191
192 // Compute the activation value with a small enough torque in X
193 ✗ wrench.setZero();
194 ✗ wrench(5) = random_real_in_range(0., 100.);
195 ✗ wrench(0) = (box(0) - 1e-9) * wrench(5) / 2.;
196 ✗ r = support.get_A() * wrench;
197 ✗ activation.calc(data, r);
198 ✗ BOOST_CHECK(data->a_value == 0.);
199
200 // Checking that casted computation is the same
201 ✗ casted_activation.calc(casted_data, r);
202 ✗ BOOST_CHECK(casted_data->a_value == 0.);
203
204 // Compute the activation value with a small enough torque in Y
205 ✗ wrench.setZero();
206 ✗ wrench(5) = random_real_in_range(0., 100.);
207 ✗ wrench(1) = (box(1) - 1e-9) * wrench(5) / 2.;
208 ✗ r = support.get_A() * wrench;
209 ✗ activation.calc(data, r);
210 ✗ BOOST_CHECK(data->a_value == 0.);
211
212 // Checking that casted computation is the same
213 ✗ casted_activation.calc(casted_data, r);
214 ✗ BOOST_CHECK(casted_data->a_value == 0.);
215 }
216
217 ✗ void register_unit_tests() {
218 ✗ framework::master_test_suite().add(
219 ✗ BOOST_TEST_CASE(boost::bind(&test_constructor)));
220 ✗ framework::master_test_suite().add(
221 ✗ BOOST_TEST_CASE(boost::bind(&test_A_matrix_with_rotation_change)));
222 ✗ framework::master_test_suite().add(
223 ✗ BOOST_TEST_CASE(boost::bind(&test_cop_within_the_support_region)));
224 }
225
226 ✗ bool init_function() {
227 ✗ register_unit_tests();
228 ✗ return true;
229 }
230
231 ✗ int main(int argc, char* argv[]) {
232 ✗ return ::boost::unit_test::unit_test_main(&init_function, argc, argv);
233 }
234