Add a multibody friction cone cost, rename FrictionConeResidual to CentroidalFrictionConeResidual

CHANGELOG.md

@@ -7,10 +7,15 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ## [Unreleased]
 
+### Added
+
+- Add a multibody friction cone cost ([#234](https://github.com/Simple-Robotics/aligator/pull/234))
+
 ### Changed
 
 - Change uses of `ConstraintSetBase` template class to `ConstraintSetTpl` (following changes in proxsuite-nlp 0.9.0) ([#223](https://github.com/Simple-Robotics/aligator/pull/233))
 - [gar] Throw an exception if trying to instantiate `ParallelRiccatiSolver` with num_threads smaller than 2.
+- [API BREAKING] Rename friction cone for centroidal into CentroidalFrictionCone ([#234](https://github.com/Simple-Robotics/aligator/pull/234))
 
 ## [0.9.0] - 2024-10-11
 

bindings/python/src/modelling/expose-centroidal.cpp

@@ -7,7 +7,7 @@
 #include "aligator/modelling/centroidal/linear-momentum.hpp"
 #include "aligator/modelling/centroidal/angular-momentum.hpp"
 #include "aligator/modelling/centroidal/centroidal-acceleration.hpp"
-#include "aligator/modelling/centroidal/friction-cone.hpp"
+#include "aligator/modelling/centroidal/centroidal-friction-cone.hpp"
 #include "aligator/modelling/centroidal/centroidal-wrench-cone.hpp"
 #include "aligator/modelling/centroidal/angular-acceleration.hpp"
 #include "aligator/modelling/centroidal/centroidal-wrapper.hpp"
@@ -57,8 +57,9 @@ void exposeCentroidalFunctions() {
       CentroidalAccelerationResidualTpl<Scalar>;
   using CentroidalAccelerationData = CentroidalAccelerationDataTpl<Scalar>;
 
-  using FrictionConeResidual = FrictionConeResidualTpl<Scalar>;
-  using FrictionConeData = FrictionConeDataTpl<Scalar>;
+  using CentroidalFrictionConeResidual =
+      CentroidalFrictionConeResidualTpl<Scalar>;
+  using CentroidalFrictionConeData = CentroidalFrictionConeDataTpl<Scalar>;
 
   using CentroidalWrenchConeResidual = CentroidalWrenchConeResidualTpl<Scalar>;
   using CentroidalWrenchConeData = CentroidalWrenchConeDataTpl<Scalar>;
@@ -137,17 +138,18 @@ void exposeCentroidalFunctions() {
       "CentroidalAccelerationData", "Data Structure for CentroidalAcceleration",
       bp::no_init);
 
-  bp::class_<FrictionConeResidual, bp::bases<StageFunction>>(
-      "FrictionConeResidual",
+  bp::class_<CentroidalFrictionConeResidual, bp::bases<StageFunction>>(
+      "CentroidalFrictionConeResidual",
       "A residual function :math:`r(x) = [fz, mu2 * fz2 - (fx2 + fy2)]` ",
       bp::init<const int, const int, const int, const double, const double>(
           ("self"_a, "ndx", "nu", "k", "mu", "epsilon")))
       .def(func_visitor);
 
-  bp::register_ptr_to_python<shared_ptr<FrictionConeData>>();
+  bp::register_ptr_to_python<shared_ptr<CentroidalFrictionConeData>>();
 
-  bp::class_<FrictionConeData, bp::bases<StageFunctionData>>(
-      "FrictionConeData", "Data Structure for FrictionCone", bp::no_init);
+  bp::class_<CentroidalFrictionConeData, bp::bases<StageFunctionData>>(
+      "CentroidalFrictionConeData", "Data Structure for CentroidalFrictionCone",
+      bp::no_init);
 
   bp::class_<CentroidalWrenchConeResidual, bp::bases<StageFunction>>(
       "CentroidalWrenchConeResidual",

bindings/python/src/modelling/expose-force-cost.cpp

@@ -8,6 +8,7 @@
 #ifdef ALIGATOR_PINOCCHIO_V3
 #include "aligator/modelling/multibody/contact-force.hpp"
 #include "aligator/modelling/multibody/multibody-wrench-cone.hpp"
+#include "aligator/modelling/multibody/multibody-friction-cone.hpp"
 #include "aligator/python/polymorphic-convertible.hpp"
 
 namespace aligator {
@@ -38,6 +39,10 @@ void exposeContactForce() {
   using MultibodyWrenchConeResidual = MultibodyWrenchConeResidualTpl<Scalar>;
   using MultibodyWrenchConeData = MultibodyWrenchConeDataTpl<Scalar>;
 
+  using MultibodyFrictionConeResidual =
+      MultibodyFrictionConeResidualTpl<Scalar>;
+  using MultibodyFrictionConeData = MultibodyFrictionConeDataTpl<Scalar>;
+
   bp::class_<ContactForceResidual, bp::bases<StageFunction>>(
       "ContactForceResidual",
       "A residual function :math:`r(x) = v_{j,xy} e^{-s z_j}` where :math:`j` "
@@ -84,6 +89,26 @@ void exposeContactForce() {
       .def_readwrite("pin_data", &MultibodyWrenchConeData::pin_data_)
       .def_readwrite("constraint_datas",
                      &MultibodyWrenchConeData::constraint_datas_);
+
+  bp::class_<MultibodyFrictionConeResidual, bp::bases<StageFunction>>(
+      "MultibodyFrictionConeResidual", "A residual function :math:`r(x) = Af` ",
+      bp::no_init)
+      .def(bp::init<int, PinModel, const context::MatrixXs &,
+                    const RigidConstraintModelVector &,
+                    const pinocchio::ProximalSettingsTpl<Scalar> &,
+                    const std::string &, const double>(
+          bp::args("self", "ndx", "model", "actuation_matrix",
+                   "constraint_models", "prox_settings", "contact_name", "mu")))
+      .def(func_visitor)
+      .def_readwrite("constraint_models",
+                     &MultibodyFrictionConeResidual::constraint_models_);
+
+  bp::class_<MultibodyFrictionConeData, bp::bases<StageFunctionData>>(
+      "MultibodyFrictionConeData", bp::no_init)
+      .def_readwrite("tau", &MultibodyFrictionConeData::tau_)
+      .def_readwrite("pin_data", &MultibodyFrictionConeData::pin_data_)
+      .def_readwrite("constraint_datas",
+                     &MultibodyFrictionConeData::constraint_datas_);
 }
 
 } // namespace python

include/aligator/modelling/centroidal/friction-cone.hpp → include/aligator/modelling/centroidal/centroidal-friction-cone.hpp

@@ -16,20 +16,20 @@ namespace aligator {
  * \f$ small threshold and \f$ \mu \f$ friction coefficient.
  */
 
-template <typename Scalar> struct FrictionConeDataTpl;
+template <typename Scalar> struct CentroidalFrictionConeDataTpl;
 
 template <typename _Scalar>
-struct FrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
+struct CentroidalFrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
 
 public:
   using Scalar = _Scalar;
   ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
   using Base = StageFunctionTpl<Scalar>;
   using BaseData = typename Base::Data;
-  using Data = FrictionConeDataTpl<Scalar>;
+  using Data = CentroidalFrictionConeDataTpl<Scalar>;
 
-  FrictionConeResidualTpl(const int ndx, const int nu, const int k,
-                          const double mu, const double epsilon)
+  CentroidalFrictionConeResidualTpl(const int ndx, const int nu, const int k,
+                                    const double mu, const double epsilon)
       : Base(ndx, nu, 2), k_(k), mu2_(mu * mu), epsilon_(epsilon) {}
 
   void evaluate(const ConstVectorRef &, const ConstVectorRef &u,
@@ -49,18 +49,19 @@ struct FrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
 };
 
 template <typename Scalar>
-struct FrictionConeDataTpl : StageFunctionDataTpl<Scalar> {
+struct CentroidalFrictionConeDataTpl : StageFunctionDataTpl<Scalar> {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   using Base = StageFunctionDataTpl<Scalar>;
   using Matrix23s = Eigen::Matrix<Scalar, 2, 3>;
 
   Matrix23s Jtemp_;
 
-  FrictionConeDataTpl(const FrictionConeResidualTpl<Scalar> *model);
+  CentroidalFrictionConeDataTpl(
+      const CentroidalFrictionConeResidualTpl<Scalar> *model);
 };
 
 } // namespace aligator
 
 #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
-#include "aligator/modelling/centroidal/friction-cone.txx"
+#include "aligator/modelling/centroidal/centroidal-friction-cone.txx"
 #endif

include/aligator/modelling/centroidal/centroidal-friction-cone.hxx

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "aligator/modelling/centroidal/centroidal-friction-cone.hpp"
+
+namespace aligator {
+
+template <typename Scalar>
+void CentroidalFrictionConeResidualTpl<Scalar>::evaluate(
+    const ConstVectorRef &, const ConstVectorRef &u, BaseData &data) const {
+  Data &d = static_cast<Data &>(data);
+
+  d.value_[0] = -u[k_ * 3 + 2] + epsilon_;
+  d.value_[1] = -mu2_ * std::pow(u[k_ * 3 + 2], 2) + std::pow(u[k_ * 3], 2) +
+                std::pow(u[k_ * 3 + 1], 2);
+}
+
+template <typename Scalar>
+void CentroidalFrictionConeResidualTpl<Scalar>::computeJacobians(
+    const ConstVectorRef &, const ConstVectorRef &u, BaseData &data) const {
+  Data &d = static_cast<Data &>(data);
+
+  d.Jtemp_ << 0, 0, -1, 2 * u[k_ * 3], 2 * u[k_ * 3 + 1],
+      -2 * mu2_ * u[k_ * 3 + 2];
+  d.Ju_.template block<2, 3>(0, k_ * 3) = d.Jtemp_;
+}
+
+template <typename Scalar>
+CentroidalFrictionConeDataTpl<Scalar>::CentroidalFrictionConeDataTpl(
+    const CentroidalFrictionConeResidualTpl<Scalar> *model)
+    : Base(*model) {
+  Jtemp_.setZero();
+}
+
+} // namespace aligator

include/aligator/modelling/centroidal/centroidal-friction-cone.txx

@@ -0,0 +1,11 @@
+#pragma once
+
+#include "aligator/context.hpp"
+#include "aligator/modelling/centroidal/centroidal-friction-cone.hpp"
+
+namespace aligator {
+
+extern template struct CentroidalFrictionConeResidualTpl<context::Scalar>;
+extern template struct CentroidalFrictionConeDataTpl<context::Scalar>;
+
+} // namespace aligator

include/aligator/modelling/multibody/multibody-friction-cone.hpp

@@ -0,0 +1,112 @@
+#pragma once
+
+#include "./fwd.hpp"
+#include "aligator/core/function-abstract.hpp"
+
+#include <proxsuite-nlp/modelling/spaces/multibody.hpp>
+
+#ifdef ALIGATOR_PINOCCHIO_V3
+#include <pinocchio/algorithm/proximal.hpp>
+
+namespace aligator {
+
+template <typename Scalar> struct MultibodyFrictionConeDataTpl;
+
+/**
+ * @brief This residual returns the derivative of centroidal momentum
+ * for a kinodynamics model.
+ */
+
+template <typename _Scalar>
+struct MultibodyFrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
+public:
+  using Scalar = _Scalar;
+  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
+  using Base = StageFunctionTpl<Scalar>;
+  using BaseData = typename Base::Data;
+  using Model = pinocchio::ModelTpl<Scalar>;
+  using SE3 = pinocchio::SE3Tpl<Scalar>;
+  using Data = MultibodyFrictionConeDataTpl<Scalar>;
+  using RigidConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(
+      pinocchio::RigidConstraintModel);
+  using RigidConstraintDataVector =
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
+  using ProxSettings = pinocchio::ProximalSettingsTpl<Scalar>;
+
+  Model pin_model_;
+  MatrixXs actuation_matrix_;
+  RigidConstraintModelVector constraint_models_;
+  ProxSettings prox_settings_;
+  double mu_;
+  int contact_id_;
+
+  Eigen::Matrix<Scalar, 5, 3> Acone_;
+
+  MultibodyFrictionConeResidualTpl(
+      const int ndx, const Model &model, const MatrixXs &actuation,
+      const RigidConstraintModelVector &constraint_models,
+      const ProxSettings &prox_settings, const std::string &contact_name,
+      const double mu)
+      : Base(ndx, (int)actuation.cols(), 5), pin_model_(model),
+        actuation_matrix_(actuation), constraint_models_(constraint_models),
+        prox_settings_(prox_settings), mu_(mu) {
+    if (model.nv != actuation.rows()) {
+      ALIGATOR_DOMAIN_ERROR(
+          fmt::format("actuation matrix should have number of rows = pinocchio "
+                      "model nv ({} and {}).",
+                      actuation.rows(), model.nv));
+    }
+    contact_id_ = -1;
+    for (std::size_t i = 0; i < constraint_models.size(); i++) {
+      if (constraint_models[i].name == contact_name) {
+        contact_id_ = (int)i;
+      }
+    }
+    if (contact_id_ == -1) {
+      ALIGATOR_RUNTIME_ERROR(
+          "Contact name is not included in constraint models");
+    }
+    Acone_ << 0, 0, -1, -1, 0, -mu_, 1, 0, -mu_, 0, -1, -mu_, 0, 1, -mu_;
+  }
+
+  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
+                BaseData &data) const;
+
+  void computeJacobians(const ConstVectorRef &, const ConstVectorRef &,
+                        BaseData &data) const;
+
+  shared_ptr<BaseData> createData() const {
+    return std::make_shared<Data>(this);
+  }
+};
+
+template <typename Scalar>
+struct MultibodyFrictionConeDataTpl : StageFunctionDataTpl<Scalar> {
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using Base = StageFunctionDataTpl<Scalar>;
+  using PinData = pinocchio::DataTpl<Scalar>;
+  using VectorXs = typename math_types<Scalar>::VectorXs;
+  using MatrixXs = typename math_types<Scalar>::MatrixXs;
+  using Matrix6Xs = typename math_types<Scalar>::Matrix6Xs;
+  using RigidConstraintDataVector =
+      PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
+
+  /// Pinocchio data object.
+  PinData pin_data_;
+  VectorXs tau_;
+  MatrixXs temp_;
+
+  RigidConstraintDataVector constraint_datas_;
+  pinocchio::ProximalSettingsTpl<Scalar> settings;
+
+  MultibodyFrictionConeDataTpl(
+      const MultibodyFrictionConeResidualTpl<Scalar> *model);
+};
+
+} // namespace aligator
+
+#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
+#include "aligator/modelling/multibody/multibody-friction-cone.txx"
+#endif
+
+#endif // ALIGATOR_PINOCCHIO_V3