diff --git a/include/crocoddyl/core/residuals/joint-acceleration.hpp b/include/crocoddyl/core/residuals/joint-acceleration.hpp index 04145a7de3..884edce69b 100644 --- a/include/crocoddyl/core/residuals/joint-acceleration.hpp +++ b/include/crocoddyl/core/residuals/joint-acceleration.hpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2022, Heriot-Watt University +// Copyright (C) 2022-2023, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -54,11 +54,11 @@ class ResidualModelJointAccelerationTpl * @brief Initialize the joint-acceleration residual model * * @param[in] state State description - * @param[in] uref Reference joint acceleration + * @param[in] aref Reference joint acceleration * @param[in] nu Dimension of the control vector */ ResidualModelJointAccelerationTpl(boost::shared_ptr state, - const VectorXs& uref, const std::size_t nu); + const VectorXs& aref, const std::size_t nu); /** * @brief Initialize the joint-acceleration residual model @@ -66,10 +66,10 @@ class ResidualModelJointAccelerationTpl * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`. * * @param[in] state State description - * @param[in] uref Reference joint acceleration + * @param[in] aref Reference joint acceleration */ ResidualModelJointAccelerationTpl(boost::shared_ptr state, - const VectorXs& uref); + const VectorXs& aref); /** * @brief Initialize the joint-acceleration residual model diff --git a/include/crocoddyl/core/residuals/joint-acceleration.hxx b/include/crocoddyl/core/residuals/joint-acceleration.hxx index 30285fb2ee..a05eccb09c 100644 --- a/include/crocoddyl/core/residuals/joint-acceleration.hxx +++ b/include/crocoddyl/core/residuals/joint-acceleration.hxx @@ -15,13 +15,25 @@ template ResidualModelJointAccelerationTpl::ResidualModelJointAccelerationTpl( boost::shared_ptr state, const VectorXs& aref, const std::size_t nu) - : Base(state, state->get_nv(), nu, true, true, true), aref_(aref) {} + : Base(state, state->get_nv(), nu, true, true, true), aref_(aref) { + if (static_cast(aref_.size()) != state->get_nv()) { + throw_pretty("Invalid argument: " + << "aref has wrong dimension (it should be " + + std::to_string(state->get_nv()) + ")"); + } +} template ResidualModelJointAccelerationTpl::ResidualModelJointAccelerationTpl( boost::shared_ptr state, const VectorXs& aref) : Base(state, state->get_nv(), state->get_nv(), true, true, true), - aref_(aref) {} + aref_(aref) { + if (static_cast(aref_.size()) != state->get_nv()) { + throw_pretty("Invalid argument: " + << "aref has wrong dimension (it should be " + + std::to_string(state->get_nv()) + ")"); + } +} template ResidualModelJointAccelerationTpl::ResidualModelJointAccelerationTpl( diff --git a/include/crocoddyl/multibody/residuals/state.hxx b/include/crocoddyl/multibody/residuals/state.hxx index 244681b7d5..458724ea56 100644 --- a/include/crocoddyl/multibody/residuals/state.hxx +++ b/include/crocoddyl/multibody/residuals/state.hxx @@ -147,11 +147,11 @@ ResidualModelStateTpl::get_reference() const { template void ResidualModelStateTpl::set_reference(const VectorXs& reference) { - if (static_cast(reference.size()) != nr_) { - throw_pretty("Invalid argument: " - << "the state reference has wrong dimension (" - << reference.size() - << " provided - it should be " + std::to_string(nr_) + ")") + if (static_cast(reference.size()) != state_->get_nx()) { + throw_pretty( + "Invalid argument: " + << "the state reference has wrong dimension (" << reference.size() + << " provided - it should be " + std::to_string(state_->get_nx()) + ")") } xref_ = reference; } diff --git a/unittest/test_residuals.cpp b/unittest/test_residuals.cpp index 6e7cfc42a8..3564d87f24 100644 --- a/unittest/test_residuals.cpp +++ b/unittest/test_residuals.cpp @@ -9,7 +9,13 @@ #define BOOST_TEST_NO_MAIN #define BOOST_TEST_ALTERNATIVE_INIT_API +#include "crocoddyl/core/residuals/control.hpp" +#include "crocoddyl/core/residuals/joint-acceleration.hpp" +#include "crocoddyl/core/residuals/joint-effort.hpp" #include "crocoddyl/multibody/data/multibody.hpp" +#include "crocoddyl/multibody/residuals/centroidal-momentum.hpp" +#include "crocoddyl/multibody/residuals/com-position.hpp" +#include "crocoddyl/multibody/residuals/state.hpp" #include "factory/actuation.hpp" #include "factory/residual.hpp" #include "unittest_common.hpp" @@ -198,6 +204,61 @@ void test_partial_derivatives_against_numdiff( BOOST_CHECK((data->Rx - data_num_diff->Rx).isZero(tol)); } +void test_reference() { + ResidualModelFactory factory; + StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos; + ActuationModelTypes::Type actuation_type = + ActuationModelTypes::ActuationModelFloatingBase; + StateModelFactory state_factory; + ActuationModelFactory actuation_factory; + boost::shared_ptr state = + boost::static_pointer_cast( + state_factory.create(state_type)); + boost::shared_ptr actuation = + actuation_factory.create(actuation_type, state_type); + + const std::size_t nu = actuation->get_nu(); + const std::size_t nv = state->get_nv(); + + // Test reference in state residual + crocoddyl::ResidualModelState state_residual(state, state->rand(), nu); + Eigen::VectorXd x_ref = state_residual.get_state()->rand(); + state_residual.set_reference(x_ref); + BOOST_CHECK((x_ref - state_residual.get_reference()).isZero()); + + // Test reference in control residual + crocoddyl::ResidualModelControl control_residual(state, nu); + Eigen::VectorXd u_ref = Eigen::VectorXd::Random(nu); + control_residual.set_reference(u_ref); + BOOST_CHECK((u_ref - control_residual.get_reference()).isZero()); + + // Test reference in joint-acceleration residual + crocoddyl::ResidualModelJointAcceleration jacc_residual(state, nu); + Eigen::VectorXd a_ref = Eigen::VectorXd::Random(nv); + jacc_residual.set_reference(a_ref); + BOOST_CHECK((a_ref - jacc_residual.get_reference()).isZero()); + + // Test reference in joint-effort residual + crocoddyl::ResidualModelJointEffort jeff_residual(state, actuation, nu); + Eigen::VectorXd tau_ref = Eigen::VectorXd::Random(nu); + jeff_residual.set_reference(tau_ref); + BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero()); + + // Test reference in centroidal-momentum residual + crocoddyl::ResidualModelCentroidalMomentum cmon_residual( + state, Eigen::Matrix::Zero()); + Eigen::Matrix h_ref = Eigen::Matrix::Random(); + cmon_residual.set_reference(h_ref); + BOOST_CHECK((h_ref - cmon_residual.get_reference()).isZero()); + + // Test reference in com-position residual + crocoddyl::ResidualModelCoMPosition c_residual(state, + Eigen::Vector3d::Zero()); + Eigen::Vector3d c_ref = Eigen::Vector3d::Random(); + c_residual.set_reference(c_ref); + BOOST_CHECK((c_ref - c_residual.get_reference()).isZero()); +} + //----------------------------------------------------------------------------// void register_residual_model_unit_tests( @@ -219,6 +280,15 @@ void register_residual_model_unit_tests( framework::master_test_suite().add(ts); } +void regiter_residual_reference_unit_tests() { + boost::test_tools::output_test_stream test_name; + test_name << "test_reference"; + std::cout << "Running " << test_name.str() << std::endl; + test_suite* ts = BOOST_TEST_SUITE(test_name.str()); + ts->add(BOOST_TEST_CASE(boost::bind(&test_reference))); + framework::master_test_suite().add(ts); +} + bool init_function() { // Test all residuals available with all the activation types with all // available states types. @@ -247,6 +317,7 @@ bool init_function() { } } } + regiter_residual_reference_unit_tests(); return true; }