From ca079cbf7f5be2b2c62c42f78ec50e6f183724cd Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 17 Sep 2024 13:18:52 +0200 Subject: [PATCH 1/2] [modelling/costs] Add getResidual() templated getter for composite cost functions + will attempt a cast to pointer-type to the expected actual type of residual function --- .../modelling/costs/log-residual-cost.hpp | 11 ++++ .../modelling/costs/quad-residual-cost.hpp | 11 ++++ tests/continuous.cpp | 4 +- tests/costs.cpp | 54 +++++++++++-------- 4 files changed, 55 insertions(+), 25 deletions(-) diff --git a/include/aligator/modelling/costs/log-residual-cost.hpp b/include/aligator/modelling/costs/log-residual-cost.hpp index 271ae8b16..387d1446b 100644 --- a/include/aligator/modelling/costs/log-residual-cost.hpp +++ b/include/aligator/modelling/costs/log-residual-cost.hpp @@ -39,6 +39,17 @@ template struct LogResidualCostTpl : CostAbstractTpl { return std::make_shared(this->ndx(), this->nu, residual_->createData()); } + + /// @brief Get a pointer to the underlying type of the residual, by attempting + /// to cast. + template Derived *getResidual() { + return dynamic_cast(&*residual_); + } + + /// @copybrief getResidual(). + template const Derived *getResidual() const { + return dynamic_cast(&*residual_); + } }; extern template struct LogResidualCostTpl; diff --git a/include/aligator/modelling/costs/quad-residual-cost.hpp b/include/aligator/modelling/costs/quad-residual-cost.hpp index 7389e61d3..dc29ecc3c 100644 --- a/include/aligator/modelling/costs/quad-residual-cost.hpp +++ b/include/aligator/modelling/costs/quad-residual-cost.hpp @@ -44,6 +44,17 @@ struct QuadraticResidualCostTpl : CostAbstractTpl<_Scalar> { return std::make_shared(this->ndx(), this->nu, residual_->createData()); } + + /// @brief Get a pointer to the underlying type of the residual, by attempting + /// to cast. + template Derived *getResidual() { + return dynamic_cast(&*residual_); + } + + /// @copybrief getResidual(). + template const Derived *getResidual() const { + return dynamic_cast(&*residual_); + } }; extern template struct QuadraticResidualCostTpl; diff --git a/tests/continuous.cpp b/tests/continuous.cpp index bc7595daf..33964bf84 100644 --- a/tests/continuous.cpp +++ b/tests/continuous.cpp @@ -12,10 +12,10 @@ BOOST_AUTO_TEST_CASE(create_data) { pinocchio::buildModels::humanoidRandom(model); using StateMultibody = proxsuite::nlp::MultibodyPhaseSpace; - auto spaceptr = std::make_shared(model); + const StateMultibody state{model}; Eigen::MatrixXd B(model.nv, model.nv); B.setIdentity(); - dynamics::MultibodyFreeFwdDynamicsTpl contdyn(*spaceptr, B); + dynamics::MultibodyFreeFwdDynamicsTpl contdyn(state, B); using ContDataAbstract = dynamics::ContinuousDynamicsDataTpl; using Data = dynamics::MultibodyFreeFwdDataTpl; diff --git a/tests/costs.cpp b/tests/costs.cpp index 61d34c2b7..3928dd437 100644 --- a/tests/costs.cpp +++ b/tests/costs.cpp @@ -15,6 +15,7 @@ using T = double; using context::MatrixXs; using context::VectorXs; using QuadraticResidualCost = QuadraticResidualCostTpl; +using StateError = StateErrorResidualTpl; void fd_test(VectorXs x0, VectorXs u0, MatrixXs weights, QuadraticResidualCost qres, shared_ptr data) { @@ -42,63 +43,70 @@ void fd_test(VectorXs x0, VectorXs u0, MatrixXs weights, BOOST_AUTO_TEST_CASE(quad_state_se2) { using SE2 = proxsuite::nlp::SETpl<2, T>; - auto space = std::make_shared(); + SE2 space; - const Eigen::Index ndx = space->ndx(); + const Eigen::Index ndx = space.ndx(); const Eigen::Index nu = 1UL; Eigen::VectorXd u0(nu); u0.setZero(); - const auto target = space->rand(); + const auto target = space.rand(); - const auto fun = - std::make_shared>(*space, nu, target); + const StateError fun(space, nu, target); - BOOST_CHECK_EQUAL(fun->nr, ndx); + BOOST_CHECK_EQUAL(fun.nr, ndx); Eigen::MatrixXd weights(ndx, ndx); weights.setIdentity(); - const auto qres = - std::make_shared>(*space, nu, target, weights); + const QuadraticStateCostTpl qres(space, nu, target, weights); - shared_ptr data = qres->createData(); - auto fd = fun->createData(); + shared_ptr data = qres.createData(); + auto fd = fun.createData(); const int nrepeats = 10; for (int k = 0; k < nrepeats; k++) { - Eigen::VectorXd x0 = space->rand(); - fd_test(x0, u0, weights, *qres, data); + Eigen::VectorXd x0 = space.rand(); + fd_test(x0, u0, weights, qres, data); } + + const StateError *fun_cast = qres.getResidual(); + BOOST_CHECK(fun_cast != nullptr); } BOOST_AUTO_TEST_CASE(quad_state_highdim) { using VectorSpace = proxsuite::nlp::VectorSpaceTpl; const Eigen::Index ndx = 56; - const auto space = std::make_shared(ndx); + const VectorSpace space(ndx); const Eigen::Index nu = 1UL; Eigen::VectorXd u0(nu); u0.setZero(); - const auto target = space->rand(); + const auto target = space.rand(); - const auto fun = - std::make_shared>(*space, nu, target); + const StateErrorResidualTpl fun(space, nu, target); - BOOST_CHECK_EQUAL(fun->nr, ndx); + BOOST_CHECK_EQUAL(fun.nr, ndx); Eigen::MatrixXd weights(ndx, ndx); weights.setIdentity(); - const auto qres = - std::make_shared>(*space, nu, target, weights); + const QuadraticStateCostTpl qres(space, nu, target, weights); - shared_ptr data = qres->createData(); - auto fd = fun->createData(); + shared_ptr data = qres.createData(); + auto fd = fun.createData(); const int nrepeats = 10; for (int k = 0; k < nrepeats; k++) { - Eigen::VectorXd x0 = space->rand(); - fd_test(x0, u0, weights, *qres, data); + Eigen::VectorXd x0 = space.rand(); + fd_test(x0, u0, weights, qres, data); + } + + const StateError *fun_cast = qres.getResidual(); + BOOST_CHECK(fun_cast != nullptr); + + { + const auto *try_cast = qres.getResidual>(); + BOOST_CHECK(try_cast == nullptr); } } From b20c82478c45b25ce842b0e4ec2ae31c13445adc Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 17 Sep 2024 13:21:10 +0200 Subject: [PATCH 2/2] Update CHANGELOG --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 010b5c240..d240bf8b6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Added + +- Getter `getResidual()` for composite cost functions ([#198](https://github.com/Simple-Robotics/aligator/pull/198)) + ### Changed - Change storage for `ConstraintStack` to using two `std::vector>` the struct `StageConstraintTpl` is now merely a convenient API shortcut for the end-user.