| 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 |