From 366c7c81b869507ca54a7b5d01739c002a28e0c8 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 11 Aug 2023 19:18:29 +0100 Subject: [PATCH 1/6] [bugfix] Fixed set_reference in state residual --- include/crocoddyl/multibody/residuals/state.hxx | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/crocoddyl/multibody/residuals/state.hxx b/include/crocoddyl/multibody/residuals/state.hxx index 244681b7d5..864fccf23c 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_) { + 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(nr_) + ")") + << " provided - it should be " + std::to_string(state_->get_nx()) + ")") } xref_ = reference; } From d50c2d450617e7ed28e4720443056a93d9a2dd01 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 11 Aug 2023 18:38:21 +0000 Subject: [PATCH 2/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/crocoddyl/multibody/residuals/state.hxx | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/crocoddyl/multibody/residuals/state.hxx b/include/crocoddyl/multibody/residuals/state.hxx index 864fccf23c..458724ea56 100644 --- a/include/crocoddyl/multibody/residuals/state.hxx +++ b/include/crocoddyl/multibody/residuals/state.hxx @@ -148,10 +148,10 @@ ResidualModelStateTpl::get_reference() const { template void ResidualModelStateTpl::set_reference(const VectorXs& reference) { 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()) + ")") + throw_pretty( + "Invalid argument: " + << "the state reference has wrong dimension (" << reference.size() + << " provided - it should be " + std::to_string(state_->get_nx()) + ")") } xref_ = reference; } From 720468eb3c47ebadaae59ec1d3c76df89ddb58b8 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 11 Aug 2023 20:54:36 +0100 Subject: [PATCH 3/6] [cleanup] Fixed notation in joint acc --- .../crocoddyl/core/residuals/joint-acceleration.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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 From 2c9618de3dd0f84f9000fa7c76d4cbf0321c8722 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 11 Aug 2023 20:55:08 +0100 Subject: [PATCH 4/6] [residual] Added checks in joint-acc residual --- .../core/residuals/joint-acceleration.hxx | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) 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( From 96ecd34879540a7aa2278cd89a5bbbdbed22cbf0 Mon Sep 17 00:00:00 2001 From: Carlos Mastalli Date: Fri, 11 Aug 2023 20:56:06 +0100 Subject: [PATCH 5/6] [unittest] Added set/get_reference tests in residuals --- unittest/test_residuals.cpp | 68 +++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/unittest/test_residuals.cpp b/unittest/test_residuals.cpp index 6e7cfc42a8..d55b32cdb4 100644 --- a/unittest/test_residuals.cpp +++ b/unittest/test_residuals.cpp @@ -14,6 +14,13 @@ #include "factory/residual.hpp" #include "unittest_common.hpp" +#include "crocoddyl/multibody/residuals/state.hpp" +#include "crocoddyl/core/residuals/control.hpp" +#include "crocoddyl/core/residuals/joint-acceleration.hpp" +#include "crocoddyl/core/residuals/joint-effort.hpp" +#include "crocoddyl/multibody/residuals/centroidal-momentum.hpp" +#include "crocoddyl/multibody/residuals/com-position.hpp" + using namespace boost::unit_test; using namespace crocoddyl::unittest; @@ -198,6 +205,57 @@ 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 +277,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 +314,7 @@ bool init_function() { } } } + regiter_residual_reference_unit_tests(); return true; } From 74f8fbe1100bda66fe97e8f9139595ae009eed90 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 11 Aug 2023 19:56:26 +0000 Subject: [PATCH 6/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- unittest/test_residuals.cpp | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/unittest/test_residuals.cpp b/unittest/test_residuals.cpp index d55b32cdb4..3564d87f24 100644 --- a/unittest/test_residuals.cpp +++ b/unittest/test_residuals.cpp @@ -9,17 +9,16 @@ #define BOOST_TEST_NO_MAIN #define BOOST_TEST_ALTERNATIVE_INIT_API -#include "crocoddyl/multibody/data/multibody.hpp" -#include "factory/actuation.hpp" -#include "factory/residual.hpp" -#include "unittest_common.hpp" - -#include "crocoddyl/multibody/residuals/state.hpp" #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" using namespace boost::unit_test; using namespace crocoddyl::unittest; @@ -208,11 +207,13 @@ void test_partial_derivatives_against_numdiff( void test_reference() { ResidualModelFactory factory; StateModelTypes::Type state_type = StateModelTypes::StateMultibody_Talos; - ActuationModelTypes::Type actuation_type = ActuationModelTypes::ActuationModelFloatingBase; + ActuationModelTypes::Type actuation_type = + ActuationModelTypes::ActuationModelFloatingBase; StateModelFactory state_factory; - ActuationModelFactory actuation_factory; + ActuationModelFactory actuation_factory; boost::shared_ptr state = - boost::static_pointer_cast(state_factory.create(state_type)); + boost::static_pointer_cast( + state_factory.create(state_type)); boost::shared_ptr actuation = actuation_factory.create(actuation_type, state_type); @@ -244,13 +245,15 @@ void test_reference() { BOOST_CHECK((tau_ref - jeff_residual.get_reference()).isZero()); // Test reference in centroidal-momentum residual - crocoddyl::ResidualModelCentroidalMomentum cmon_residual(state, Eigen::Matrix::Zero()); + 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()); + 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()); @@ -278,7 +281,7 @@ void register_residual_model_unit_tests( } void regiter_residual_reference_unit_tests() { - boost::test_tools::output_test_stream test_name; + 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());