Fix building with upcoming Pinocchio 4

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

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)

bindings/python/src/modelling/multibody/expose-pinocchio-functions.cpp

@@ -197,16 +197,14 @@ 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);
 
-  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() {

examples/talos-walk-utils.cpp

@@ -9,7 +9,7 @@
 #include <pinocchio/algorithm/proximal.hpp>
 
 void makeTalosReduced(Model &model_complete, Model &model,
-                      Eigen::VectorXd &q0) {
+                      Eigen::Ref<Eigen::VectorXd> 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<Eigen::Vector3d> 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<pin::RigidConstraintModel> &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<pin::RigidConstraintModel> 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<Support> double_phase;
@@ -195,14 +203,14 @@ TrajOptProblem defineLocomotionProblem(const std::size_t T_ss,
     std::shared_ptr<FramePlacementResidual> 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<FramePlacementResidual>(
           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<FramePlacementResidual>(
           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);

examples/talos-walk-utils.hpp

@@ -38,7 +38,8 @@ using TrajOptProblem = aligator::TrajOptProblemTpl<double>;
 
 enum Support { LEFT, RIGHT, DOUBLE };
 
-void makeTalosReduced(Model &model_complete, Model &model, Eigen::VectorXd &q0);
+void makeTalosReduced(Model &model_complete, Model &model,
+                      Eigen::Ref<Eigen::VectorXd> q0);
 
 TrajOptProblem defineLocomotionProblem(const std::size_t T_ss,
                                        const std::size_t T_ds);

include/aligator/fwd.hpp

@@ -164,11 +164,4 @@ template <typename Scalar> struct ResultsTpl;
 // fwd FilterTpl
 template <typename Scalar> 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 <typename T>
-using StdVectorEigenAligned ALIGATOR_DEPRECATED =
-    std::vector<T, Eigen::aligned_allocator<T>>;
-
 } // namespace aligator

include/aligator/modelling/contact-map.hpp

@@ -8,7 +8,7 @@ namespace aligator {
 template <typename _Scalar> struct ContactMapTpl {
   using Scalar = _Scalar;
   ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
-  using PoseVec = std::vector<Vector3s, Eigen::aligned_allocator<Vector3s>>;
+  using PoseVec = std::vector<Vector3s>;
 
   ContactMapTpl(const std::vector<std::string> &contact_names,
                 const std::vector<bool> &contact_states,

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 <typename _Scalar> struct LinearODETpl : ODEAbstractTpl<_Scalar> {
   }
 };
 
+#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
+extern template struct LinearODETpl<context::Scalar>;
+#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

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<Scalar> {
   MatrixXs dtau_du_;
   RigidConstraintDataVector constraint_datas_;
   pinocchio::ProximalSettingsTpl<Scalar> settings;
-  /// shared_ptr to the underlying pinocchio::DataTpl object.
   PinDataType pin_data_;
-  MultibodyConstraintFwdDataTpl(
+  explicit MultibodyConstraintFwdDataTpl(
       const MultibodyConstraintFwdDynamicsTpl<Scalar> &cont_dyn);
 };
 
-} // namespace dynamics
-} // namespace aligator
-
 #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
-#include "aligator/modelling/dynamics/multibody-constraint-fwd.txx"
+extern template struct MultibodyConstraintFwdDynamicsTpl<context::Scalar>;
+extern template struct MultibodyConstraintFwdDataTpl<context::Scalar>;
 #endif
+
+} // namespace dynamics
+} // namespace aligator

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 <typename Scalar> 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 <typename _Scalar>
 struct MultibodyFreeFwdDynamicsTpl : ODEAbstractTpl<_Scalar> {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -81,10 +79,11 @@ struct MultibodyFreeFwdDataTpl : ContinuousDynamicsDataTpl<Scalar> {
   MultibodyFreeFwdDataTpl(const MultibodyFreeFwdDynamicsTpl<Scalar> *cont_dyn);
 };
 
-} // namespace dynamics
-} // namespace aligator
-
 #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
-#include "aligator/modelling/dynamics/multibody-free-fwd.txx"
+extern template struct MultibodyFreeFwdDynamicsTpl<context::Scalar>;
+extern template struct MultibodyFreeFwdDataTpl<context::Scalar>;
 #endif
+
+} // namespace dynamics
+} // namespace aligator
 #endif

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 <typename _Scalar>
 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<context::Scalar>;
 #endif
+
+} // namespace dynamics
+} // namespace aligator