| 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 "crocoddyl/core/residuals/control.hpp" | ||
| 13 | #include "crocoddyl/core/residuals/joint-acceleration.hpp" | ||
| 14 | #include "crocoddyl/core/residuals/joint-effort.hpp" | ||
| 15 | #include "crocoddyl/multibody/data/multibody.hpp" | ||
| 16 | #include "crocoddyl/multibody/residuals/centroidal-momentum.hpp" | ||
| 17 | #include "crocoddyl/multibody/residuals/com-position.hpp" | ||
| 18 | #include "crocoddyl/multibody/residuals/state.hpp" | ||
| 19 | #include "factory/actuation.hpp" | ||
| 20 | #include "factory/residual.hpp" | ||
| 21 | #include "unittest_common.hpp" | ||
| 22 | |||
| 23 | using namespace boost::unit_test; | ||
| 24 | using namespace crocoddyl::unittest; | ||
| 25 | |||
| 26 | //----------------------------------------------------------------------------// | ||
| 27 | |||
| 28 | ✗ | void test_calc_returns_a_residual(ResidualModelTypes::Type residual_type, | |
| 29 | StateModelTypes::Type state_type, | ||
| 30 | ActuationModelTypes::Type actuation_type) { | ||
| 31 | // Create the model | ||
| 32 | ✗ | ResidualModelFactory residual_factory; | |
| 33 | ✗ | ActuationModelFactory actuation_factory; | |
| 34 | std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model = | ||
| 35 | ✗ | actuation_factory.create(actuation_type, state_type); | |
| 36 | const std::shared_ptr<crocoddyl::ResidualModelAbstract>& model = | ||
| 37 | residual_factory.create(residual_type, state_type, | ||
| 38 | ✗ | actuation_model->get_nu()); | |
| 39 | |||
| 40 | // Run the print function | ||
| 41 | ✗ | std::ostringstream tmp; | |
| 42 | ✗ | tmp << *model; | |
| 43 | |||
| 44 | // Create the corresponding shared data | ||
| 45 | const std::shared_ptr<crocoddyl::StateMultibody>& state = | ||
| 46 | ✗ | std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state()); | |
| 47 | ✗ | pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); | |
| 48 | ✗ | pinocchio::Data pinocchio_data(pinocchio_model); | |
| 49 | const std::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data = | ||
| 50 | ✗ | actuation_model->createData(); | |
| 51 | crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data, | ||
| 52 | ✗ | actuation_data); | |
| 53 | |||
| 54 | // create the residual data | ||
| 55 | const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data = | ||
| 56 | ✗ | model->createData(&shared_data); | |
| 57 | |||
| 58 | // Generating random values for the state and control | ||
| 59 | ✗ | const Eigen::VectorXd x = model->get_state()->rand(); | |
| 60 | ✗ | const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu()); | |
| 61 | |||
| 62 | // Compute all the pinocchio function needed for the models. | ||
| 63 | ✗ | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); | |
| 64 | ✗ | crocoddyl::unittest::updateActuation(actuation_model, actuation_data, x, u); | |
| 65 | |||
| 66 | // Getting the residual value computed by calc() | ||
| 67 | ✗ | data->r *= nan(""); | |
| 68 | ✗ | model->calc(data, x, u); | |
| 69 | |||
| 70 | // Checking that calc returns a residual value | ||
| 71 | ✗ | for (std::size_t i = 0; i < model->get_nr(); ++i) | |
| 72 | ✗ | BOOST_CHECK(!std::isnan(data->r(i))); | |
| 73 | |||
| 74 | // Checking that casted computation is the same | ||
| 75 | #ifdef NDEBUG // Run only in release mode | ||
| 76 | const std::shared_ptr<crocoddyl::ResidualModelAbstractTpl<float>>& | ||
| 77 | casted_model = model->cast<float>(); | ||
| 78 | const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>& | ||
| 79 | casted_actuation_model = actuation_model->cast<float>(); | ||
| 80 | const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state = | ||
| 81 | std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>( | ||
| 82 | casted_model->get_state()); | ||
| 83 | pinocchio::ModelTpl<float>& pinocchio_model_f = | ||
| 84 | *casted_state->get_pinocchio().get(); | ||
| 85 | pinocchio::DataTpl<float> pinocchio_data_f(pinocchio_model_f); | ||
| 86 | const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>& | ||
| 87 | casted_actuation_data = casted_actuation_model->createData(); | ||
| 88 | crocoddyl::DataCollectorActMultibodyTpl<float> casted_shared_data( | ||
| 89 | &pinocchio_data_f, casted_actuation_data); | ||
| 90 | const std::shared_ptr<crocoddyl::ResidualDataAbstractTpl<float>>& | ||
| 91 | casted_data = casted_model->createData(&casted_shared_data); | ||
| 92 | const Eigen::VectorXf x_f = x.cast<float>(); | ||
| 93 | const Eigen::VectorXf u_f = u.cast<float>(); | ||
| 94 | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model_f, &pinocchio_data_f, | ||
| 95 | x_f); | ||
| 96 | crocoddyl::unittest::updateActuation(casted_actuation_model, | ||
| 97 | casted_actuation_data, x_f, u_f); | ||
| 98 | casted_data->r *= float(nan("")); | ||
| 99 | casted_model->calc(casted_data, x_f, u_f); | ||
| 100 | for (std::size_t i = 0; i < casted_model->get_nr(); ++i) | ||
| 101 | BOOST_CHECK(!std::isnan(casted_data->r(i))); | ||
| 102 | float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon()); | ||
| 103 | BOOST_CHECK((data->r.cast<float>() - casted_data->r).isZero(tol_f)); | ||
| 104 | #endif | ||
| 105 | ✗ | } | |
| 106 | |||
| 107 | ✗ | void test_calc_against_numdiff(ResidualModelTypes::Type residual_type, | |
| 108 | StateModelTypes::Type state_type, | ||
| 109 | ActuationModelTypes::Type actuation_type) { | ||
| 110 | using namespace boost::placeholders; | ||
| 111 | |||
| 112 | // Create the model | ||
| 113 | ✗ | ResidualModelFactory residual_factory; | |
| 114 | ✗ | ActuationModelFactory actuation_factory; | |
| 115 | std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model = | ||
| 116 | ✗ | actuation_factory.create(actuation_type, state_type); | |
| 117 | const std::shared_ptr<crocoddyl::ResidualModelAbstract>& model = | ||
| 118 | residual_factory.create(residual_type, state_type, | ||
| 119 | ✗ | actuation_model->get_nu()); | |
| 120 | |||
| 121 | // Create the corresponding shared data | ||
| 122 | const std::shared_ptr<crocoddyl::StateMultibody>& state = | ||
| 123 | ✗ | std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state()); | |
| 124 | ✗ | pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); | |
| 125 | ✗ | pinocchio::Data pinocchio_data(pinocchio_model); | |
| 126 | const std::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data = | ||
| 127 | ✗ | actuation_model->createData(); | |
| 128 | crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data, | ||
| 129 | ✗ | actuation_data); | |
| 130 | |||
| 131 | // Create the residual data | ||
| 132 | const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data = | ||
| 133 | ✗ | model->createData(&shared_data); | |
| 134 | |||
| 135 | // Create the equivalent num diff model and data. | ||
| 136 | ✗ | crocoddyl::ResidualModelNumDiff model_num_diff(model); | |
| 137 | const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff = | ||
| 138 | ✗ | model_num_diff.createData(&shared_data); | |
| 139 | |||
| 140 | // Generating random values for the state and control | ||
| 141 | ✗ | const Eigen::VectorXd x = model->get_state()->rand(); | |
| 142 | ✗ | const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu()); | |
| 143 | |||
| 144 | // Computing the residual | ||
| 145 | ✗ | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); | |
| 146 | ✗ | actuation_model->calc(actuation_data, x, u); | |
| 147 | ✗ | model->calc(data, x, u); | |
| 148 | |||
| 149 | // Computing the residual from num diff | ||
| 150 | ✗ | std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals; | |
| 151 | ✗ | reevals.push_back( | |
| 152 | boost::bind(&crocoddyl::unittest::updateAllPinocchio< | ||
| 153 | double, 0, pinocchio::JointCollectionDefaultTpl>, | ||
| 154 | &pinocchio_model, &pinocchio_data, _1, _2)); | ||
| 155 | ✗ | reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation<double>, | |
| 156 | actuation_model, actuation_data, _1, _2)); | ||
| 157 | ✗ | model_num_diff.set_reevals(reevals); | |
| 158 | ✗ | model_num_diff.calc(data_num_diff, x, u); | |
| 159 | |||
| 160 | // Checking the partial derivatives against NumDiff | ||
| 161 | ✗ | BOOST_CHECK(data->r == data_num_diff->r); | |
| 162 | |||
| 163 | // Checking that casted computation is the same | ||
| 164 | #ifdef NDEBUG // Run only in release mode | ||
| 165 | const std::shared_ptr<crocoddyl::ResidualModelAbstractTpl<float>>& | ||
| 166 | casted_model = model->cast<float>(); | ||
| 167 | const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>& | ||
| 168 | casted_actuation_model = actuation_model->cast<float>(); | ||
| 169 | const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state = | ||
| 170 | std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>( | ||
| 171 | casted_model->get_state()); | ||
| 172 | pinocchio::ModelTpl<float>& casted_pinocchio_model = | ||
| 173 | *casted_state->get_pinocchio().get(); | ||
| 174 | pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model); | ||
| 175 | const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>& | ||
| 176 | casted_actuation_data = casted_actuation_model->createData(); | ||
| 177 | crocoddyl::DataCollectorActMultibodyTpl<float> casted_shared_data( | ||
| 178 | &casted_pinocchio_data, casted_actuation_data); | ||
| 179 | const std::shared_ptr<crocoddyl::ResidualDataAbstractTpl<float>>& | ||
| 180 | casted_data = casted_model->createData(&casted_shared_data); | ||
| 181 | const Eigen::VectorXf x_f = x.cast<float>(); | ||
| 182 | const Eigen::VectorXf u_f = u.cast<float>(); | ||
| 183 | crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model, | ||
| 184 | &casted_pinocchio_data, x_f); | ||
| 185 | casted_actuation_model->calc(casted_actuation_data, x_f, u_f); | ||
| 186 | casted_model->calc(casted_data, x_f, u_f); | ||
| 187 | float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon()); | ||
| 188 | BOOST_CHECK((data->r.cast<float>() - casted_data->r).isZero(tol_f)); | ||
| 189 | #endif | ||
| 190 | ✗ | } | |
| 191 | |||
| 192 | ✗ | void test_partial_derivatives_against_numdiff( | |
| 193 | ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type, | ||
| 194 | ActuationModelTypes::Type actuation_type) { | ||
| 195 | using namespace boost::placeholders; | ||
| 196 | |||
| 197 | // Create the model | ||
| 198 | ✗ | ResidualModelFactory residual_factory; | |
| 199 | ✗ | ActuationModelFactory actuation_factory; | |
| 200 | std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation_model = | ||
| 201 | ✗ | actuation_factory.create(actuation_type, state_type); | |
| 202 | const std::shared_ptr<crocoddyl::ResidualModelAbstract>& model = | ||
| 203 | residual_factory.create(residual_type, state_type, | ||
| 204 | ✗ | actuation_model->get_nu()); | |
| 205 | |||
| 206 | // Create the corresponding shared data | ||
| 207 | const std::shared_ptr<crocoddyl::StateMultibody>& state = | ||
| 208 | ✗ | std::static_pointer_cast<crocoddyl::StateMultibody>(model->get_state()); | |
| 209 | ✗ | pinocchio::Model& pinocchio_model = *state->get_pinocchio().get(); | |
| 210 | ✗ | pinocchio::Data pinocchio_data(pinocchio_model); | |
| 211 | const std::shared_ptr<crocoddyl::ActuationDataAbstract>& actuation_data = | ||
| 212 | ✗ | actuation_model->createData(); | |
| 213 | crocoddyl::DataCollectorActMultibody shared_data(&pinocchio_data, | ||
| 214 | ✗ | actuation_data); | |
| 215 | |||
| 216 | // Create the residual data | ||
| 217 | const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data = | ||
| 218 | ✗ | model->createData(&shared_data); | |
| 219 | |||
| 220 | // Create the equivalent num diff model and data. | ||
| 221 | ✗ | crocoddyl::ResidualModelNumDiff model_num_diff(model); | |
| 222 | const std::shared_ptr<crocoddyl::ResidualDataAbstract>& data_num_diff = | ||
| 223 | ✗ | model_num_diff.createData(&shared_data); | |
| 224 | |||
| 225 | // Generating random values for the state and control | ||
| 226 | ✗ | Eigen::VectorXd x = model->get_state()->rand(); | |
| 227 | ✗ | const Eigen::VectorXd u = Eigen::VectorXd::Random(model->get_nu()); | |
| 228 | |||
| 229 | // Computing the residual derivatives | ||
| 230 | ✗ | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); | |
| 231 | ✗ | actuation_model->calc(actuation_data, x, u); | |
| 232 | ✗ | actuation_model->calcDiff(actuation_data, x, u); | |
| 233 | ✗ | model->calc(data, x, u); | |
| 234 | ✗ | model->calcDiff(data, x, u); | |
| 235 | |||
| 236 | // Computing the residual derivatives via numerical differentiation | ||
| 237 | ✗ | std::vector<crocoddyl::ResidualModelNumDiff::ReevaluationFunction> reevals; | |
| 238 | ✗ | reevals.push_back( | |
| 239 | boost::bind(&crocoddyl::unittest::updateAllPinocchio< | ||
| 240 | double, 0, pinocchio::JointCollectionDefaultTpl>, | ||
| 241 | &pinocchio_model, &pinocchio_data, _1, _2)); | ||
| 242 | ✗ | reevals.push_back(boost::bind(&crocoddyl::unittest::updateActuation<double>, | |
| 243 | actuation_model, actuation_data, _1, _2)); | ||
| 244 | ✗ | model_num_diff.set_reevals(reevals); | |
| 245 | ✗ | model_num_diff.calc(data_num_diff, x, u); | |
| 246 | ✗ | model_num_diff.calcDiff(data_num_diff, x, u); | |
| 247 | |||
| 248 | // Checking the partial derivatives against numdiff | ||
| 249 | // Tolerance defined as in | ||
| 250 | // http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf | ||
| 251 | ✗ | double tol = std::pow(model_num_diff.get_disturbance(), 1. / 3.); | |
| 252 | ✗ | BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol)); | |
| 253 | ✗ | BOOST_CHECK((data->Ru - data_num_diff->Ru).isZero(tol)); | |
| 254 | |||
| 255 | // Computing the residual derivatives | ||
| 256 | ✗ | x = model->get_state()->rand(); | |
| 257 | ✗ | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); | |
| 258 | ✗ | actuation_model->calc(actuation_data, x); | |
| 259 | ✗ | actuation_model->calcDiff(actuation_data, x); | |
| 260 | |||
| 261 | // Computing the residual derivatives via numerical differentiation | ||
| 262 | ✗ | model->calc(data, x); | |
| 263 | ✗ | model->calcDiff(data, x); | |
| 264 | ✗ | model_num_diff.calc(data_num_diff, x); | |
| 265 | ✗ | model_num_diff.calcDiff(data_num_diff, x); | |
| 266 | |||
| 267 | // Checking the partial derivatives against numdiff | ||
| 268 | ✗ | BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol)); | |
| 269 | |||
| 270 | // Checking that casted computation is the same | ||
| 271 | #ifdef NDEBUG // Run only in release mode | ||
| 272 | const std::shared_ptr<crocoddyl::ResidualModelAbstractTpl<float>>& | ||
| 273 | casted_model = model->cast<float>(); | ||
| 274 | const std::shared_ptr<crocoddyl::ActuationModelAbstractTpl<float>>& | ||
| 275 | casted_actuation_model = actuation_model->cast<float>(); | ||
| 276 | const std::shared_ptr<crocoddyl::StateMultibodyTpl<float>>& casted_state = | ||
| 277 | std::static_pointer_cast<crocoddyl::StateMultibodyTpl<float>>( | ||
| 278 | casted_model->get_state()); | ||
| 279 | pinocchio::ModelTpl<float>& casted_pinocchio_model = | ||
| 280 | *casted_state->get_pinocchio().get(); | ||
| 281 | pinocchio::DataTpl<float> casted_pinocchio_data(casted_pinocchio_model); | ||
| 282 | const std::shared_ptr<crocoddyl::ActuationDataAbstractTpl<float>>& | ||
| 283 | casted_actuation_data = casted_actuation_model->createData(); | ||
| 284 | crocoddyl::DataCollectorActMultibodyTpl<float> casted_shared_data( | ||
| 285 | &casted_pinocchio_data, casted_actuation_data); | ||
| 286 | const std::shared_ptr<crocoddyl::ResidualDataAbstractTpl<float>>& | ||
| 287 | casted_data = casted_model->createData(&casted_shared_data); | ||
| 288 | const Eigen::VectorXf x_f = x.cast<float>(); | ||
| 289 | const Eigen::VectorXf u_f = u.cast<float>(); | ||
| 290 | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); | ||
| 291 | crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model, | ||
| 292 | &casted_pinocchio_data, x_f); | ||
| 293 | actuation_model->calc(actuation_data, x, u); | ||
| 294 | actuation_model->calcDiff(actuation_data, x, u); | ||
| 295 | model->calc(data, x, u); | ||
| 296 | model->calcDiff(data, x, u); | ||
| 297 | casted_actuation_model->calc(casted_actuation_data, x_f, u_f); | ||
| 298 | casted_actuation_model->calcDiff(casted_actuation_data, x_f, u_f); | ||
| 299 | casted_model->calc(casted_data, x_f, u_f); | ||
| 300 | casted_model->calcDiff(casted_data, x_f, u_f); | ||
| 301 | float tol_f = std::sqrt(2.0f * std::numeric_limits<float>::epsilon()); | ||
| 302 | BOOST_CHECK((data->Rx.cast<float>() - casted_data->Rx).isZero(tol_f)); | ||
| 303 | BOOST_CHECK((data->Ru.cast<float>() - casted_data->Ru).isZero(tol_f)); | ||
| 304 | crocoddyl::unittest::updateAllPinocchio(&pinocchio_model, &pinocchio_data, x); | ||
| 305 | crocoddyl::unittest::updateAllPinocchio(&casted_pinocchio_model, | ||
| 306 | &casted_pinocchio_data, x_f); | ||
| 307 | actuation_model->calc(actuation_data, x); | ||
| 308 | actuation_model->calcDiff(actuation_data, x); | ||
| 309 | model->calc(data, x); | ||
| 310 | model->calcDiff(data, x); | ||
| 311 | casted_actuation_model->calc(casted_actuation_data, x_f); | ||
| 312 | casted_actuation_model->calcDiff(casted_actuation_data, x_f); | ||
| 313 | casted_model->calc(casted_data, x_f); | ||
| 314 | casted_model->calcDiff(casted_data, x_f); | ||
| 315 | BOOST_CHECK((data->Rx.cast<float>() - casted_data->Rx).isZero(tol_f)); | ||
| 316 | #endif | ||
| 317 | ✗ | } | |
| 318 | |||
| 319 | ✗ | void test_reference() { | |
| 320 | ✗ | ResidualModelFactory factory; | |
| 321 | ✗ | StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos; | |
| 322 | ✗ | ActuationModelTypes::Type actuation_type = | |
| 323 | ActuationModelTypes::ActuationModelFloatingBase; | ||
| 324 | ✗ | StateModelFactory state_factory; | |
| 325 | ✗ | ActuationModelFactory actuation_factory; | |
| 326 | std::shared_ptr<crocoddyl::StateMultibody> state = | ||
| 327 | std::static_pointer_cast<crocoddyl::StateMultibody>( | ||
| 328 | ✗ | state_factory.create(state_type)); | |
| 329 | std::shared_ptr<crocoddyl::ActuationModelAbstract> actuation = | ||
| 330 | ✗ | actuation_factory.create(actuation_type, state_type); | |
| 331 | |||
| 332 | ✗ | const std::size_t nu = actuation->get_nu(); | |
| 333 | ✗ | const std::size_t nv = state->get_nv(); | |
| 334 | |||
| 335 | // Test reference in state residual | ||
| 336 | ✗ | crocoddyl::ResidualModelState state_residual(state, state->rand(), nu); | |
| 337 | ✗ | Eigen::VectorXd x_ref = state_residual.get_state()->rand(); | |
| 338 | ✗ | state_residual.set_reference(x_ref); | |
| 339 | ✗ | BOOST_CHECK((x_ref - state_residual.get_reference()).isZero()); | |
| 340 | // Checking that casted computation is the same | ||
| 341 | #ifdef NDEBUG // Run only in release mode | ||
| 342 | crocoddyl::ResidualModelStateTpl<float> casted_state_residual = | ||
| 343 | state_residual.cast<float>(); | ||
| 344 | Eigen::VectorXf x_ref_f = casted_state_residual.get_state()->rand(); | ||
| 345 | casted_state_residual.set_reference(x_ref_f); | ||
| 346 | BOOST_CHECK((x_ref_f - casted_state_residual.get_reference()).isZero()); | ||
| 347 | #endif | ||
| 348 | // Test reference in control residual | ||
| 349 | ✗ | crocoddyl::ResidualModelControl control_residual(state, nu); | |
| 350 | ✗ | Eigen::VectorXd u_ref = Eigen::VectorXd::Random(nu); | |
| 351 | ✗ | control_residual.set_reference(u_ref); | |
| 352 | ✗ | BOOST_CHECK((u_ref - control_residual.get_reference()).isZero()); | |
| 353 | // Checking that casted computation is the same | ||
| 354 | #ifdef NDEBUG // Run only in release mode | ||
| 355 | crocoddyl::ResidualModelControlTpl<float> casted_control_residual = | ||
| 356 | control_residual.cast<float>(); | ||
| 357 | Eigen::VectorXf u_ref_f = Eigen::VectorXf::Random(nu); | ||
| 358 | casted_control_residual.set_reference(u_ref_f); | ||
| 359 | BOOST_CHECK((u_ref_f - casted_control_residual.get_reference()).isZero()); | ||
| 360 | #endif | ||
| 361 | // Test reference in joint-acceleration residual | ||
| 362 | ✗ | crocoddyl::ResidualModelJointAcceleration jacc_residual(state, nu); | |
| 363 | ✗ | Eigen::VectorXd a_ref = Eigen::VectorXd::Random(nv); | |
| 364 | ✗ | jacc_residual.set_reference(a_ref); | |
| 365 | ✗ | BOOST_CHECK((a_ref - jacc_residual.get_reference()).isZero()); | |
| 366 | // Checking that casted computation is the same | ||
| 367 | #ifdef NDEBUG // Run only in release mode | ||
| 368 | crocoddyl::ResidualModelJointAccelerationTpl<float> casted_jacc_residual = | ||
| 369 | jacc_residual.cast<float>(); | ||
| 370 | Eigen::VectorXf a_ref_f = Eigen::VectorXf::Random(nv); | ||
| 371 | casted_jacc_residual.set_reference(a_ref_f); | ||
| 372 | BOOST_CHECK((a_ref_f - casted_jacc_residual.get_reference()).isZero()); | ||
| 373 | #endif | ||
| 374 | // Test reference in joint-effort residual | ||
| 375 | ✗ | crocoddyl::ResidualModelJointEffort jeff_residual(state, actuation, nu); | |
| 376 | ✗ | Eigen::VectorXd tau_ref = Eigen::VectorXd::Random(nu); | |
| 377 | ✗ | jeff_residual.set_reference(tau_ref); | |
| 378 | ✗ | BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero()); | |
| 379 | // Checking that casted computation is the same | ||
| 380 | #ifdef NDEBUG // Run only in release mode | ||
| 381 | crocoddyl::ResidualModelJointEffortTpl<float> casted_jeff_residual = | ||
| 382 | jeff_residual.cast<float>(); | ||
| 383 | Eigen::VectorXf tau_ref_f = Eigen::VectorXf::Random(nu); | ||
| 384 | casted_jeff_residual.set_reference(tau_ref_f); | ||
| 385 | BOOST_CHECK((tau_ref_f - casted_jeff_residual.get_reference()).isZero()); | ||
| 386 | #endif | ||
| 387 | // Test reference in centroidal-momentum residual | ||
| 388 | crocoddyl::ResidualModelCentroidalMomentum cmon_residual( | ||
| 389 | ✗ | state, Eigen::Matrix<double, 6, 1>::Zero()); | |
| 390 | ✗ | Eigen::Matrix<double, 6, 1> h_ref = Eigen::Matrix<double, 6, 1>::Random(); | |
| 391 | ✗ | cmon_residual.set_reference(h_ref); | |
| 392 | ✗ | BOOST_CHECK((h_ref - cmon_residual.get_reference()).isZero()); | |
| 393 | // Checking that casted computation is the same | ||
| 394 | #ifdef NDEBUG // Run only in release mode | ||
| 395 | crocoddyl::ResidualModelCentroidalMomentumTpl<float> casted_cmon_residual = | ||
| 396 | cmon_residual.cast<float>(); | ||
| 397 | Eigen::Matrix<float, 6, 1> h_ref_f = Eigen::Matrix<float, 6, 1>::Random(); | ||
| 398 | casted_cmon_residual.set_reference(h_ref_f); | ||
| 399 | BOOST_CHECK((h_ref_f - casted_cmon_residual.get_reference()).isZero()); | ||
| 400 | #endif | ||
| 401 | // Test reference in com-position residual | ||
| 402 | crocoddyl::ResidualModelCoMPosition c_residual(state, | ||
| 403 | ✗ | Eigen::Vector3d::Zero()); | |
| 404 | ✗ | Eigen::Vector3d c_ref = Eigen::Vector3d::Random(); | |
| 405 | ✗ | c_residual.set_reference(c_ref); | |
| 406 | ✗ | BOOST_CHECK((c_ref - c_residual.get_reference()).isZero()); | |
| 407 | // Checking that casted computation is the same | ||
| 408 | #ifdef NDEBUG // Run only in release mode | ||
| 409 | crocoddyl::ResidualModelCoMPositionTpl<float> casted_c_residual = | ||
| 410 | c_residual.cast<float>(); | ||
| 411 | Eigen::Vector3f c_ref_f = Eigen::Vector3f::Random(); | ||
| 412 | casted_c_residual.set_reference(c_ref_f); | ||
| 413 | BOOST_CHECK((c_ref_f - casted_c_residual.get_reference()).isZero()); | ||
| 414 | #endif | ||
| 415 | ✗ | } | |
| 416 | |||
| 417 | //----------------------------------------------------------------------------// | ||
| 418 | |||
| 419 | ✗ | void register_residual_model_unit_tests( | |
| 420 | ResidualModelTypes::Type residual_type, StateModelTypes::Type state_type, | ||
| 421 | ActuationModelTypes::Type actuation_type) { | ||
| 422 | ✗ | boost::test_tools::output_test_stream test_name; | |
| 423 | ✗ | test_name << "test_" << residual_type << "_" << state_type << "_" | |
| 424 | ✗ | << actuation_type; | |
| 425 | ✗ | std::cout << "Running " << test_name.str() << std::endl; | |
| 426 | ✗ | test_suite* ts = BOOST_TEST_SUITE(test_name.str()); | |
| 427 | ✗ | ts->add( | |
| 428 | ✗ | BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_residual, residual_type, | |
| 429 | state_type, actuation_type))); | ||
| 430 | ✗ | ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_against_numdiff, residual_type, | |
| 431 | state_type, actuation_type))); | ||
| 432 | ✗ | ts->add( | |
| 433 | ✗ | BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_against_numdiff, | |
| 434 | residual_type, state_type, actuation_type))); | ||
| 435 | ✗ | framework::master_test_suite().add(ts); | |
| 436 | ✗ | } | |
| 437 | |||
| 438 | ✗ | void regiter_residual_reference_unit_tests() { | |
| 439 | ✗ | boost::test_tools::output_test_stream test_name; | |
| 440 | ✗ | test_name << "test_reference"; | |
| 441 | ✗ | std::cout << "Running " << test_name.str() << std::endl; | |
| 442 | ✗ | test_suite* ts = BOOST_TEST_SUITE(test_name.str()); | |
| 443 | ✗ | ts->add(BOOST_TEST_CASE(boost::bind(&test_reference))); | |
| 444 | ✗ | framework::master_test_suite().add(ts); | |
| 445 | ✗ | } | |
| 446 | |||
| 447 | ✗ | bool init_function() { | |
| 448 | // Test all residuals available with all the activation types with all | ||
| 449 | // available states types. | ||
| 450 | ✗ | for (size_t residual_type = 0; residual_type < ResidualModelTypes::all.size(); | |
| 451 | ++residual_type) { | ||
| 452 | // size_t residual_type = ResidualModelTypes::ResidualModelCoMPosition; | ||
| 453 | ✗ | for (size_t state_type = | |
| 454 | ✗ | StateModelTypes::all[StateModelTypes::StateMultibody_TalosArm]; | |
| 455 | ✗ | state_type < StateModelTypes::all.size(); ++state_type) { | |
| 456 | ✗ | for (size_t actuation_type = 0; | |
| 457 | ✗ | actuation_type < ActuationModelTypes::all.size(); ++actuation_type) { | |
| 458 | ✗ | if (ActuationModelTypes::all[actuation_type] != | |
| 459 | ActuationModelTypes::ActuationModelFloatingBaseThrusters) { | ||
| 460 | ✗ | register_residual_model_unit_tests( | |
| 461 | ✗ | ResidualModelTypes::all[residual_type], | |
| 462 | ✗ | StateModelTypes::all[state_type], | |
| 463 | ✗ | ActuationModelTypes::all[actuation_type]); | |
| 464 | ✗ | } else if (StateModelTypes::all[state_type] != | |
| 465 | ✗ | StateModelTypes::StateMultibody_TalosArm && | |
| 466 | ✗ | StateModelTypes::all[state_type] != | |
| 467 | StateModelTypes::StateMultibodyContact2D_TalosArm) { | ||
| 468 | ✗ | register_residual_model_unit_tests( | |
| 469 | ✗ | ResidualModelTypes::all[residual_type], | |
| 470 | ✗ | StateModelTypes::all[state_type], | |
| 471 | ✗ | ActuationModelTypes::all[actuation_type]); | |
| 472 | } | ||
| 473 | } | ||
| 474 | } | ||
| 475 | } | ||
| 476 | ✗ | regiter_residual_reference_unit_tests(); | |
| 477 | ✗ | return true; | |
| 478 | } | ||
| 479 | |||
| 480 | ✗ | int main(int argc, char** argv) { | |
| 481 | ✗ | return ::boost::unit_test::unit_test_main(&init_function, argc, argv); | |
| 482 | } | ||
| 483 |