From 46a1e3844dba72f2888c63d27bdb73bf727e9bd9 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Fri, 9 Jan 2026 23:40:21 +0100 Subject: [PATCH 1/9] pinocchio 4 API fixes --- .../multibody/expose-pinocchio-functions.cpp | 7 ++-- examples/talos-walk-utils.cpp | 42 +++++++++++-------- examples/talos-walk-utils.hpp | 3 +- .../modelling/multibody/constrained-rnea.hpp | 37 ++++++++++++---- 4 files changed, 58 insertions(+), 31 deletions(-) diff --git a/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp b/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp index 9ef88a25a..0d25bef3e 100644 --- a/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp +++ b/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp @@ -197,10 +197,9 @@ auto underactuatedConstraintInvDyn_proxy(const PinModel &model, PinData &data, const RCMVector &constraint_models, RCDVector &constraint_datas) { long nu = actMatrix.cols(); - int d = 0; - for (size_t k = 0; k < constraint_models.size(); ++k) { - d += (int)constraint_models[k].size(); - } + const int d = details::computeRigidConstraintsTotalSize(constraint_models, + constraint_datas); + context::VectorXs out(nu + d); underactuatedConstrainedInverseDynamics( model, data, q, v, actMatrix, constraint_models, constraint_datas, out); diff --git a/examples/talos-walk-utils.cpp b/examples/talos-walk-utils.cpp index f80009207..be2a379fe 100644 --- a/examples/talos-walk-utils.cpp +++ b/examples/talos-walk-utils.cpp @@ -9,7 +9,7 @@ #include void makeTalosReduced(Model &model_complete, Model &model, - Eigen::VectorXd &q0) { + Eigen::Ref q0) { const std::string talos_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/robots/talos_reduced.urdf"; const std::string srdf_path = @@ -57,16 +57,17 @@ void makeTalosReduced(Model &model_complete, Model &model, q0 = model.referenceConfigurations["half_sitting"]; } -void foot_traj(Eigen::Vector3d &translation_init, const int &t_ss, - const int &ts) { - double swing_apex = 0.05; - translation_init[2] += swing_apex * sin(ts * M_PI / (double)t_ss); +void getFootTraj(std::size_t t_ss, std::size_t ts, + Eigen::Ref translation_init) { + constexpr double swing_apex = 0.05; + translation_init[2] += swing_apex * std::sin(ts * M_PI / (double)t_ss); } IntegratorSemiImplEuler -create_dynamics(MultibodyPhaseSpace &stage_space, Support &support, - MatrixXd &actuation_matrix, ProximalSettings &proximal_settings, - std::vector &constraint_models) { +createDynamics(MultibodyPhaseSpace &stage_space, Support &support, + MatrixXd &actuation_matrix, ProximalSettings &proximal_settings, + PINOCCHIO_ALIGNED_STD_VECTOR(pin::RigidConstraintModel) & + constraint_models) { pinocchio::context::RigidConstraintModelVector cms; switch (support) { case LEFT: @@ -144,18 +145,25 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, foot_joint_ids.push_back(rmodel.frames[foot_frame_ids[0]].parentJoint); foot_joint_ids.push_back(rmodel.frames[foot_frame_ids[1]].parentJoint); - std::vector constraint_models; + PINOCCHIO_ALIGNED_STD_VECTOR(pin::RigidConstraintModel) constraint_models; for (std::size_t i = 0; i < 2; i++) { pin::SE3 pl1 = rmodel.frames[foot_frame_ids[i]].placement; pin::SE3 pl2 = rdata.oMf[foot_frame_ids[i]]; - pin::RigidConstraintModel constraint_model = pin::RigidConstraintModel( + pin::RigidConstraintModel constraint_model( pin::ContactType::CONTACT_6D, rmodel, foot_joint_ids[i], pl1, 0, pl2, pin::LOCAL_WORLD_ALIGNED); - constraint_model.corrector.Kp << 0, 0, 100, 0, 0, 0; - constraint_model.corrector.Kd << 50, 50, 50, 50, 50, 50; +#ifdef ALIGATOR_PINOCCHIO_V4 + auto &corrector = constraint_model.baumgarte_corrector_parameters(); + corrector.Kp = 100; + corrector.Kd = 50; +#else + auto &corrector = constraint_model.corrector; + corrector.Kp << 0, 0, 100, 0, 0, 0; + corrector.Kd << 50, 50, 50, 50, 50, 50; +#endif constraint_model.name = foot_frame_name[i]; - constraint_models.push_back(constraint_model); + constraint_models.push_back(std::move(constraint_model)); } std::vector double_phase; @@ -195,14 +203,14 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, std::shared_ptr frame_fn_LF; switch (ph) { case LEFT: - foot_traj(RF_placement.translation(), T_ss, ts); + getFootTraj(T_ss, ts, RF_placement.translation()); frame_fn_RF = std::make_shared( stage_space.ndx(), nu, rmodel, RF_placement, foot_frame_ids[1]); rcost.addCost("frame_fn_RF", QuadraticResidualCost(stage_space, *frame_fn_RF, w_LFRF)); break; case RIGHT: - foot_traj(LF_placement.translation(), T_ss, ts); + getFootTraj(T_ss, ts, LF_placement.translation()); frame_fn_LF = std::make_shared( stage_space.ndx(), nu, rmodel, LF_placement, foot_frame_ids[0]); rcost.addCost("frame_fn_LF", @@ -213,8 +221,8 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, break; } stage_models.push_back( - StageModel(rcost, create_dynamics(stage_space, ph, actuation_matrix, - prox_settings, constraint_models))); + StageModel(rcost, createDynamics(stage_space, ph, actuation_matrix, + prox_settings, constraint_models))); } auto ter_space = MultibodyPhaseSpace(rmodel); auto term_cost = CostStack(ter_space, nu); diff --git a/examples/talos-walk-utils.hpp b/examples/talos-walk-utils.hpp index ab773b219..ceceacc92 100644 --- a/examples/talos-walk-utils.hpp +++ b/examples/talos-walk-utils.hpp @@ -38,7 +38,8 @@ using TrajOptProblem = aligator::TrajOptProblemTpl; enum Support { LEFT, RIGHT, DOUBLE }; -void makeTalosReduced(Model &model_complete, Model &model, Eigen::VectorXd &q0); +void makeTalosReduced(Model &model_complete, Model &model, + Eigen::Ref q0); TrajOptProblem defineLocomotionProblem(const std::size_t T_ss, const std::size_t T_ds); diff --git a/include/aligator/modelling/multibody/constrained-rnea.hpp b/include/aligator/modelling/multibody/constrained-rnea.hpp index ba3e1a200..6e2825f48 100644 --- a/include/aligator/modelling/multibody/constrained-rnea.hpp +++ b/include/aligator/modelling/multibody/constrained-rnea.hpp @@ -16,6 +16,28 @@ using pinocchio::RigidConstraintDataTpl; using pinocchio::RigidConstraintModelTpl; } // namespace +namespace details { +template +int computeRigidConstraintsTotalSize( + const pinocchio::container::aligned_vector< + RigidConstraintModelTpl> &constraint_models, + [[maybe_unused]] const pinocchio::container::aligned_vector< + RigidConstraintDataTpl> &constraint_datas) { + + int d = 0; + for (size_t k = 0; k < constraint_models.size(); ++k) { + const int constraint_size = +#ifdef ALIGATOR_PINOCCHIO_V4 + constraint_models[k].residualSize(constraint_datas[k]); +#else + static_cast(constraint_models[k].size()); +#endif + d += constraint_size; + } + return d; +} +} // namespace details + template void underactuatedConstrainedInverseDynamics( @@ -34,17 +56,15 @@ void underactuatedConstrainedInverseDynamics( OutType &res = res_.const_cast_derived(); - long nu = actMatrix.cols(); - long nv = model.nv; + const long nu = actMatrix.cols(); + const long nv = model.nv; assert(nv == actMatrix.rows() && "Actuation matrix dimension inconsistent."); pin::computeAllTerms(model, data, q, v); const auto &nle = data.nle; - int d = 0; - for (size_t k = 0; k < constraint_models.size(); ++k) { - d += (int)constraint_models[k].size(); - } + const int d = details::computeRigidConstraintsTotalSize(constraint_models, + constraint_datas); assert(res.size() == nu + d); MatrixXs work(nv, nu + d); @@ -53,10 +73,9 @@ void underactuatedConstrainedInverseDynamics( auto JacT = work.rightCols(d); pin::getConstraintsJacobian(model, data, constraint_models, constraint_datas, JacT.transpose()); - JacT = -JacT; - - Eigen::ColPivHouseholderQR qr(work); + JacT *= -1; + Eigen::ColPivHouseholderQR> qr(work); res = qr.solve(nle); } From a1171143d490244dd2ce20ea9f348ae447dd4886 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 13 Jan 2026 11:45:34 +0100 Subject: [PATCH 2/9] bindings : use .eval() here --- .../src/modelling/multibody/expose-pinocchio-functions.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp b/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp index 0d25bef3e..7741faa01 100644 --- a/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp +++ b/bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp @@ -204,8 +204,7 @@ auto underactuatedConstraintInvDyn_proxy(const PinModel &model, PinData &data, underactuatedConstrainedInverseDynamics( model, data, q, v, actMatrix, constraint_models, constraint_datas, out); - return bp::make_tuple((context::VectorXs)out.head(nu), - (context::VectorXs)out.tail(d)); + return bp::make_tuple(out.head(nu).eval(), out.tail(d).eval()); } void exposePinocchioFunctions() { From 604f0cf561909e200433c30a16f680eaeea0dfe2 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Thu, 15 Jan 2026 14:21:31 +0100 Subject: [PATCH 3/9] [dynamics] remove some .txx files --- CMakeLists.txt | 2 +- .../modelling/dynamics/linear-ode.hpp | 11 +++---- .../modelling/dynamics/linear-ode.txx | 13 --------- .../dynamics/multibody-constraint-fwd.hpp | 14 ++++----- .../dynamics/multibody-constraint-fwd.txx | 13 --------- .../modelling/dynamics/multibody-free-fwd.hpp | 29 +++++++++---------- .../modelling/dynamics/multibody-free-fwd.txx | 14 --------- .../modelling/dynamics/ode-abstract.hpp | 17 +++++------ .../modelling/dynamics/ode-abstract.txx | 13 --------- 9 files changed, 36 insertions(+), 90 deletions(-) delete mode 100644 include/aligator/modelling/dynamics/linear-ode.txx delete mode 100644 include/aligator/modelling/dynamics/multibody-constraint-fwd.txx delete mode 100644 include/aligator/modelling/dynamics/multibody-free-fwd.txx delete mode 100644 include/aligator/modelling/dynamics/ode-abstract.txx diff --git a/CMakeLists.txt b/CMakeLists.txt index 62afb071a..d548c8783 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ # -# Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA +# Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA # cmake_minimum_required(VERSION 3.22) diff --git a/include/aligator/modelling/dynamics/linear-ode.hpp b/include/aligator/modelling/dynamics/linear-ode.hpp index 9986041c4..349960393 100644 --- a/include/aligator/modelling/dynamics/linear-ode.hpp +++ b/include/aligator/modelling/dynamics/linear-ode.hpp @@ -1,6 +1,7 @@ -/// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA +/// @copyright Copyright (C) 2022-2023 LAAS-CNRS, 2022-2026 INRIA #pragma once +#include "aligator/context.hpp" #include "aligator/modelling/dynamics/ode-abstract.hpp" #include "aligator/core/vector-space.hpp" @@ -63,11 +64,11 @@ template struct LinearODETpl : ODEAbstractTpl<_Scalar> { } }; +#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION +extern template struct LinearODETpl; +#endif + } // namespace dynamics } // namespace aligator #include "aligator/modelling/dynamics/linear-ode.hxx" - -#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/linear-ode.txx" -#endif diff --git a/include/aligator/modelling/dynamics/linear-ode.txx b/include/aligator/modelling/dynamics/linear-ode.txx deleted file mode 100644 index 5f39a4e40..000000000 --- a/include/aligator/modelling/dynamics/linear-ode.txx +++ /dev/null @@ -1,13 +0,0 @@ -/// @copyright Copyright (C) 2024 LAAS-CNRS, INRIA -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/dynamics/linear-ode.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct LinearODETpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp b/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp index 22bc4b2c8..f2f2c35e2 100644 --- a/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp +++ b/include/aligator/modelling/dynamics/multibody-constraint-fwd.hpp @@ -1,4 +1,4 @@ -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2025 INRIA +/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA #pragma once #include "aligator/modelling/dynamics/ode-abstract.hpp" @@ -78,15 +78,15 @@ struct MultibodyConstraintFwdDataTpl : ContinuousDynamicsDataTpl { MatrixXs dtau_du_; RigidConstraintDataVector constraint_datas_; pinocchio::ProximalSettingsTpl settings; - /// shared_ptr to the underlying pinocchio::DataTpl object. PinDataType pin_data_; - MultibodyConstraintFwdDataTpl( + explicit MultibodyConstraintFwdDataTpl( const MultibodyConstraintFwdDynamicsTpl &cont_dyn); }; -} // namespace dynamics -} // namespace aligator - #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/multibody-constraint-fwd.txx" +extern template struct MultibodyConstraintFwdDynamicsTpl; +extern template struct MultibodyConstraintFwdDataTpl; #endif + +} // namespace dynamics +} // namespace aligator diff --git a/include/aligator/modelling/dynamics/multibody-constraint-fwd.txx b/include/aligator/modelling/dynamics/multibody-constraint-fwd.txx deleted file mode 100644 index ccaba6537..000000000 --- a/include/aligator/modelling/dynamics/multibody-constraint-fwd.txx +++ /dev/null @@ -1,13 +0,0 @@ -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA -#pragma once - -#include "./multibody-constraint-fwd.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct MultibodyConstraintFwdDynamicsTpl; -extern template struct MultibodyConstraintFwdDataTpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/dynamics/multibody-free-fwd.hpp b/include/aligator/modelling/dynamics/multibody-free-fwd.hpp index 3519b1c9e..8064ec9ab 100644 --- a/include/aligator/modelling/dynamics/multibody-free-fwd.hpp +++ b/include/aligator/modelling/dynamics/multibody-free-fwd.hpp @@ -1,4 +1,4 @@ -/// @copyright Copyright (C) 2022 LAAS-CNRS, INRIA +/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA #pragma once #include "aligator/modelling/dynamics/ode-abstract.hpp" @@ -11,16 +11,14 @@ namespace aligator { namespace dynamics { template struct MultibodyFreeFwdDataTpl; -/** - * @brief Free-space multibody forward dynamics, using Pinocchio. - * - * @details This is described in state-space \f$\mathcal{X} = T\mathcal{Q}\f$ - * (the phase space \f$x = (q,v)\f$) using the differential equation \f[ \dot{x} - * = f(x, u) = \begin{bmatrix} v \\ a(q, v, \tau(u)) \end{bmatrix} \f] where - * \f$\tau(u) = Bu\f$, \f$B\f$ is a given actuation matrix, and - * \f$a(q,v,\tau)\f$ is the acceleration computed from the ABA algorithm. - * - */ +/// @brief Free-space multibody forward dynamics, using Pinocchio. +/// +/// @details This is described in state-space \f$\mathcal{X} = T\mathcal{Q}\f$ +/// (the phase space \f$x = (q,v)\f$) using the differential equation \f[ +/// \dot{x} = f(x, u) = \begin{bmatrix} v \\ a(q, v, \tau(u)) \end{bmatrix} \f] +/// where +/// \f$\tau(u) = Bu\f$, \f$B\f$ is a given actuation matrix, and +/// \f$a(q,v,\tau)\f$ is the acceleration computed from the ABA algorithm. template struct MultibodyFreeFwdDynamicsTpl : ODEAbstractTpl<_Scalar> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -81,10 +79,11 @@ struct MultibodyFreeFwdDataTpl : ContinuousDynamicsDataTpl { MultibodyFreeFwdDataTpl(const MultibodyFreeFwdDynamicsTpl *cont_dyn); }; -} // namespace dynamics -} // namespace aligator - #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/multibody-free-fwd.txx" +extern template struct MultibodyFreeFwdDynamicsTpl; +extern template struct MultibodyFreeFwdDataTpl; #endif + +} // namespace dynamics +} // namespace aligator #endif diff --git a/include/aligator/modelling/dynamics/multibody-free-fwd.txx b/include/aligator/modelling/dynamics/multibody-free-fwd.txx deleted file mode 100644 index 2cb8ed6c0..000000000 --- a/include/aligator/modelling/dynamics/multibody-free-fwd.txx +++ /dev/null @@ -1,14 +0,0 @@ -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/dynamics/multibody-free-fwd.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct MultibodyFreeFwdDynamicsTpl; -extern template struct MultibodyFreeFwdDataTpl; - -} // namespace dynamics -} // namespace aligator diff --git a/include/aligator/modelling/dynamics/ode-abstract.hpp b/include/aligator/modelling/dynamics/ode-abstract.hpp index 04a3e85a5..4a8f2e5f6 100644 --- a/include/aligator/modelling/dynamics/ode-abstract.hpp +++ b/include/aligator/modelling/dynamics/ode-abstract.hpp @@ -1,16 +1,15 @@ #pragma once /// @file ode-abstract.hpp /// @brief Defines a class representing ODEs. -/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, INRIA +/// @copyright Copyright (C) 2022-2024 LAAS-CNRS, 2022-2026 INRIA #include "aligator/modelling/dynamics/continuous-dynamics-abstract.hpp" namespace aligator { namespace dynamics { -/** @brief Base class for ODE dynamics \f$ \dot{x} = f(x, u) \f$. - * @details Formulated as a DAE (for ContinuousDynamicsAbstractTpl), this class - * models \f[ f(x, u) - \dot{x}. \f] - */ +/// @brief Base class for ODE dynamics \f$ \dot{x} = f(x, u) \f$. +/// @details Formulated as a DAE (for ContinuousDynamicsAbstractTpl), this +/// class models \f[ f(x, u) - \dot{x}. \f] template struct ODEAbstractTpl : ContinuousDynamicsAbstractTpl<_Scalar> { using Scalar = _Scalar; @@ -41,9 +40,9 @@ struct ODEAbstractTpl : ContinuousDynamicsAbstractTpl<_Scalar> { const ConstVectorRef &xdot, Data &data) const override; }; -} // namespace dynamics -} // namespace aligator - #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/dynamics/ode-abstract.txx" +extern template struct ODEAbstractTpl; #endif + +} // namespace dynamics +} // namespace aligator diff --git a/include/aligator/modelling/dynamics/ode-abstract.txx b/include/aligator/modelling/dynamics/ode-abstract.txx deleted file mode 100644 index 79776d9a1..000000000 --- a/include/aligator/modelling/dynamics/ode-abstract.txx +++ /dev/null @@ -1,13 +0,0 @@ -/// @copyright Copyright (C) 2024 LAAS-CNRS, INRIA -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/dynamics/ode-abstract.hpp" - -namespace aligator { -namespace dynamics { - -extern template struct ODEAbstractTpl; - -} // namespace dynamics -} // namespace aligator From 28a2cfdbe14628c954ab9a54eb3fd203713dca06 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 10 Feb 2026 11:28:25 +0100 Subject: [PATCH 4/9] Change includes of deprecated header `` (Removed in pin4) --- include/aligator/modelling/multibody/contact-force.hpp | 7 +++---- tests/continuous.cpp | 2 +- tests/forces.cpp | 3 +-- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/include/aligator/modelling/multibody/contact-force.hpp b/include/aligator/modelling/multibody/contact-force.hpp index 5552939c8..505eecbca 100644 --- a/include/aligator/modelling/multibody/contact-force.hpp +++ b/include/aligator/modelling/multibody/contact-force.hpp @@ -1,12 +1,11 @@ /// @file -/// @copyright Copyright (C) 2024 LAAS-CNRS, 2024-2025 INRIA +/// @copyright Copyright (C) 2024 LAAS-CNRS, 2024-2026 INRIA #pragma once -#include "./fwd.hpp" +#include "fwd.hpp" #include "aligator/core/function-abstract.hpp" - -#include #include "aligator/modelling/spaces/multibody.hpp" + #include namespace aligator { diff --git a/tests/continuous.cpp b/tests/continuous.cpp index 45d12771b..8e03d874b 100644 --- a/tests/continuous.cpp +++ b/tests/continuous.cpp @@ -2,7 +2,7 @@ #ifdef ALIGATOR_WITH_PINOCCHIO #include "aligator/modelling/dynamics/multibody-free-fwd.hpp" -#include +#include #endif #ifdef ALIGATOR_WITH_PINOCCHIO diff --git a/tests/forces.cpp b/tests/forces.cpp index 5dfdbd15f..6f7fc7329 100644 --- a/tests/forces.cpp +++ b/tests/forces.cpp @@ -1,6 +1,5 @@ #include -#include -#include +#include #include #include #include "aligator/modelling/spaces/multibody.hpp" From 9ba1e366532c1dfe82e212b2574285ec3b55f6b8 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 10 Feb 2026 11:52:10 +0100 Subject: [PATCH 5/9] Fix build bugs due to corrector Fix tests/forces.cpp Add copyright heading, use `emplace_back()` --- tests/CMakeLists.txt | 1 - tests/forces.cpp | 46 +++++++++++++++-------------------- tests/mpc-cycle.cpp | 6 ++--- tests/solver-storage.cpp | 31 ----------------------- tests/test_util/pinocchio.hpp | 26 ++++++++++++++++++++ 5 files changed, 49 insertions(+), 61 deletions(-) delete mode 100644 tests/solver-storage.cpp create mode 100644 tests/test_util/pinocchio.hpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 338e728f8..24f748235 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -54,7 +54,6 @@ set( lqr problem manifolds - solver-storage utils ) diff --git a/tests/forces.cpp b/tests/forces.cpp index 6f7fc7329..28acf0371 100644 --- a/tests/forces.cpp +++ b/tests/forces.cpp @@ -1,8 +1,8 @@ -#include +/// @file +/// @copyright Copyright (C) 2024-2026 INRIA #include #include #include -#include "aligator/modelling/spaces/multibody.hpp" #include @@ -10,6 +10,8 @@ #include #include +#include "test_util/pinocchio.hpp" + TEST_CASE("contact_forces_6d", "[forces]") { using namespace Eigen; using namespace pinocchio; @@ -44,20 +46,18 @@ TEST_CASE("contact_forces_6d", "[forces]") { RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.0); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.0); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); constraint_models.push_back(ci_RF); - constraint_data.push_back(RigidConstraintData(ci_RF)); + constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); @@ -158,20 +158,18 @@ TEST_CASE("contact_forces_3d", "[forces]") { RigidConstraintModel ci_LF(CONTACT_3D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); constraint_models.push_back(ci_RF); - constraint_data.push_back(RigidConstraintData(ci_RF)); + constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); @@ -272,20 +270,18 @@ TEST_CASE("wrench_cone", "[forces]") { RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); // constraint_models.push_back(ci_RF); - // constraint_data.push_back(RigidConstraintData(ci_RF)); + // constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); @@ -384,20 +380,18 @@ TEST_CASE("friction_cone", "[forces]") { RigidConstraintModel ci_LF(CONTACT_3D, model, LF_id, LOCAL); ci_LF.joint1_placement.setRandom(); - ci_LF.corrector.Kp.array() = 10; - ci_LF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_LF, 10.); ci_LF.name = "LF_foot"; RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); ci_RF.joint1_placement.setRandom(); - ci_RF.corrector.Kp.array() = 10; - ci_RF.corrector.Kd.array() = 10; + set_baumgarte_gains(ci_RF, 10.); ci_RF.name = "RF_foot"; constraint_models.push_back(ci_LF); - constraint_data.push_back(RigidConstraintData(ci_LF)); + constraint_data.emplace_back(ci_LF); // constraint_models.push_back(ci_RF); - // constraint_data.push_back(RigidConstraintData(ci_RF)); + // constraint_data.emplace_back(ci_RF); const double mu0 = 0.; ProximalSettings prox_settings(1e-12, mu0, 1); diff --git a/tests/mpc-cycle.cpp b/tests/mpc-cycle.cpp index 3e52bbe3c..da9362dff 100644 --- a/tests/mpc-cycle.cpp +++ b/tests/mpc-cycle.cpp @@ -12,9 +12,10 @@ #include #include -#include #include +#include "test_util/pinocchio.hpp" + using namespace aligator; using context::SolverProxDDP; @@ -99,8 +100,7 @@ auto makeConstraint(pinocchio::Model const &model) { auto constraint = pinocchio::RigidConstraintModel( pinocchio::ContactType::CONTACT_6D, model, frame.parentJoint, frame.placement, pinocchio::LOCAL_WORLD_ALIGNED); - constraint.corrector.Kp << 0, 0, 100, 0, 0, 0; - constraint.corrector.Kd << 50, 50, 50, 50, 50, 50; + set_baumgarte_gains(constraint, 100, 50); constraint.name = "contact"; return constraint; diff --git a/tests/solver-storage.cpp b/tests/solver-storage.cpp deleted file mode 100644 index 858ef7624..000000000 --- a/tests/solver-storage.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "aligator/solvers/value-function.hpp" - -#include - -#include - -TEST_CASE("prox_storage", "[solver_workspace]") { - using VParams = aligator::ValueFunctionTpl; - using QParams = aligator::QFunctionTpl; - const int NX = 3; - const int NU = 2; - VParams vstore(NX); - QParams qstore(NX, NU, NX); - - REQUIRE(vstore.Vx_.cols() == 1); - REQUIRE(vstore.Vx_.rows() == NX); - REQUIRE(vstore.Vxx_.rows() == NX); - REQUIRE(vstore.Vxx_.cols() == NX); - fmt::print("{} < Vx\n", vstore.Vx_); - fmt::print("{} < Vxx\n", vstore.Vxx_); - - fmt::print("{} < Qx\n", qstore.Qx); - fmt::print("{} < Qu\n", qstore.Qu); - fmt::print("{} < Qxx\n", qstore.Qxx); - fmt::print("{} < Qxu\n", qstore.Qxu); - fmt::print("{} < Qxy\n", qstore.Qxy); - fmt::print("{} < Quu\n", qstore.Quu); - fmt::print("{} < Qyy\n", qstore.Qyy); -} - -TEST_CASE("fddp_storage", "[solver_workspace]") {} diff --git a/tests/test_util/pinocchio.hpp b/tests/test_util/pinocchio.hpp new file mode 100644 index 000000000..4c16f6e93 --- /dev/null +++ b/tests/test_util/pinocchio.hpp @@ -0,0 +1,26 @@ + +#include +#include + +inline auto & +get_baumgarte_corrector_params(pinocchio::RigidConstraintModel &rcm) { +#ifdef ALIGATOR_PINOCCHIO_V4 + return rcm.baumgarte_corrector_parameters(); +#else + return rcm.corrector; +#endif +} + +inline void set_baumgarte_gains(pinocchio::RigidConstraintModel &rcm, + const double Kp, + std::optional Kd_ = std::nullopt) { + double Kd = Kd_.value_or(Kp); + auto &corr = get_baumgarte_corrector_params(rcm); +#ifdef ALIGATOR_PINOCCHIO_V4 + corr.Kp = Kp; + corr.Kd = Kd; +#else + corr.Kp.array() = Kp; + corr.Kd.array() = Kd; +#endif +} From f3076ecc800fe02f05719f40999a7c6b7b7debf6 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 10 Feb 2026 11:56:52 +0100 Subject: [PATCH 6/9] Update CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 195c7d724..23ac0b7d4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - Update finite difference helper to support explicit dynamics, replacing the obsolete implicit-only implementation. (https://github.com/Simple-Robotics/aligator/pull/392) - Fix `SolverProxDDP::tryLinearStep()` temporary control update buffer allocation to use `nu_max` instead of `ndx_max`. (https://github.com/Simple-Robotics/aligator/pull/397) +- Tentative support building with the upcoming Pinocchio 4 (https://github.com/Simple-Robotics/aligator/pull/390) ## [0.18.0] - 2026-01-27 From 16c64a805f3fa4de75fd70a74cab1e7ad69c1f8e Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 24 Feb 2026 11:23:37 +0100 Subject: [PATCH 7/9] fwd.hpp : finally remove `StdVectorEigenAligned` --- include/aligator/fwd.hpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/include/aligator/fwd.hpp b/include/aligator/fwd.hpp index 7142fb275..90901ce84 100644 --- a/include/aligator/fwd.hpp +++ b/include/aligator/fwd.hpp @@ -164,11 +164,4 @@ template struct ResultsTpl; // fwd FilterTpl template struct FilterTpl; -/// @brief Typedef for vector with Eigen::aligned_allocator allocator type. -/// @deprecated This is deprecated. Use the PINOCCHIO_ALIGNED_STD_VECTOR macro -/// or underlying typedef when relevant, or write the full std::vector template. -template -using StdVectorEigenAligned ALIGATOR_DEPRECATED = - std::vector>; - } // namespace aligator From 9b5c4e6cc31b58777e8b5691f44f0289f6a1cc28 Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 24 Feb 2026 11:23:51 +0100 Subject: [PATCH 8/9] constrained-rnea: pin4 api postfix --- include/aligator/modelling/multibody/constrained-rnea.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aligator/modelling/multibody/constrained-rnea.hpp b/include/aligator/modelling/multibody/constrained-rnea.hpp index 6e2825f48..60c3dbeac 100644 --- a/include/aligator/modelling/multibody/constrained-rnea.hpp +++ b/include/aligator/modelling/multibody/constrained-rnea.hpp @@ -28,7 +28,7 @@ int computeRigidConstraintsTotalSize( for (size_t k = 0; k < constraint_models.size(); ++k) { const int constraint_size = #ifdef ALIGATOR_PINOCCHIO_V4 - constraint_models[k].residualSize(constraint_datas[k]); + constraint_models[k].residualSize(); #else static_cast(constraint_models[k].size()); #endif From 65f7054f3717b7f0abae2e4eda4cf0a6da562ecf Mon Sep 17 00:00:00 2001 From: ManifoldFR Date: Tue, 24 Feb 2026 11:24:47 +0100 Subject: [PATCH 9/9] contact-map.hpp : do not use Eigen::aligned_allocator anymore --- include/aligator/modelling/contact-map.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/aligator/modelling/contact-map.hpp b/include/aligator/modelling/contact-map.hpp index 23c8852a7..907854d31 100644 --- a/include/aligator/modelling/contact-map.hpp +++ b/include/aligator/modelling/contact-map.hpp @@ -8,7 +8,7 @@ namespace aligator { template struct ContactMapTpl { using Scalar = _Scalar; ALIGATOR_DYNAMIC_TYPEDEFS(Scalar); - using PoseVec = std::vector>; + using PoseVec = std::vector; ContactMapTpl(const std::vector &contact_names, const std::vector &contact_states,