Change linear friction cone to ice cream cone

CHANGELOG.md

@@ -22,6 +22,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - 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))
+- Change the linear multibody friction cone to the true "ice cream" cone ([#238](https://github.com/Simple-Robotics/aligator/pull/238))
 
 ## [0.9.0] - 2024-10-11
 

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

@@ -14,6 +14,7 @@
 namespace aligator {
 namespace python {
 
+using Vector3or6 = Eigen::Matrix<double, -1, 1, Eigen::ColMajor, 6, 1>;
 using context::ConstVectorRef;
 using context::MultibodyPhaseSpace;
 using context::PinModel;
@@ -51,7 +52,7 @@ void exposeContactForce() {
       .def(bp::init<int, PinModel, const context::MatrixXs &,
                     const RigidConstraintModelVector &,
                     const pinocchio::ProximalSettingsTpl<Scalar> &,
-                    const context::Vector6s &, const std::string &>(bp::args(
+                    const Eigen::VectorXd &, const std::string &>(bp::args(
           "self", "ndx", "model", "actuation_matrix", "constraint_models",
           "prox_settings", "fref", "contact_name")))
       .def(func_visitor)

include/aligator/modelling/multibody/contact-force.hpp

@@ -3,6 +3,7 @@
 #include "./fwd.hpp"
 #include "aligator/core/function-abstract.hpp"
 
+#include <Eigen/src/Core/util/Constants.h>
 #include <proxsuite-nlp/modelling/spaces/multibody.hpp>
 
 #ifdef ALIGATOR_PINOCCHIO_V3
@@ -32,20 +33,21 @@ struct ContactForceResidualTpl : StageFunctionTpl<_Scalar> {
   using RigidConstraintDataVector =
       PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData);
   using ProxSettings = pinocchio::ProximalSettingsTpl<Scalar>;
+  using Vector3or6 = Eigen::Matrix<Scalar, -1, 1, Eigen::ColMajor, 6, 1>;
 
   Model pin_model_;
   MatrixXs actuation_matrix_;
   RigidConstraintModelVector constraint_models_;
   ProxSettings prox_settings_;
-  Eigen::VectorXd fref_;
+  Vector3or6 fref_;
   long force_size_;
   int contact_id_;
 
   ContactForceResidualTpl(const int ndx, const Model &model,
                           const MatrixXs &actuation,
                           const RigidConstraintModelVector &constraint_models,
                           const ProxSettings &prox_settings,
-                          const Eigen::VectorXd &fref,
+                          const Vector3or6 &fref,
                           const std::string &contact_name)
       : Base(ndx, (int)actuation.cols(), fref.size()), pin_model_(model),
         actuation_matrix_(actuation), constraint_models_(constraint_models),
@@ -68,10 +70,8 @@ struct ContactForceResidualTpl : StageFunctionTpl<_Scalar> {
     }
   }
 
-  const Eigen::VectorXd &getReference() const { return fref_; }
-  void setReference(const Eigen::Ref<const Eigen::VectorXd> &fnew) {
-    fref_ = fnew;
-  }
+  const Vector3or6 &getReference() const { return fref_; }
+  void setReference(const Eigen::Ref<const Vector3or6> &fnew) { fref_ = fnew; }
 
   void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                 BaseData &data) const;

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

@@ -40,14 +40,12 @@ struct MultibodyFrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
   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),
+      : Base(ndx, (int)actuation.cols(), 2), pin_model_(model),
         actuation_matrix_(actuation), constraint_models_(constraint_models),
         prox_settings_(prox_settings), mu_(mu) {
     if (model.nv != actuation.rows()) {
@@ -66,7 +64,6 @@ struct MultibodyFrictionConeResidualTpl : StageFunctionTpl<_Scalar> {
       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,
@@ -87,14 +84,14 @@ struct MultibodyFrictionConeDataTpl : 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_;
+  Eigen::Matrix<Scalar, 1, 3> dcone_df_;
 
   RigidConstraintDataVector constraint_datas_;
   pinocchio::ProximalSettingsTpl<Scalar> settings;

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

@@ -22,8 +22,10 @@ void MultibodyFrictionConeResidualTpl<Scalar>::evaluate(const ConstVectorRef &x,
                                 d.settings);
 
   // Unilateral contact
-  d.value_.noalias() =
-      Acone_ * d.pin_data_.lambda_c.segment(contact_id_ * 3, 3);
+  d.value_[0] = -d.pin_data_.lambda_c[contact_id_ * 3 + 2];
+  d.value_[1] = -mu_ * d.pin_data_.lambda_c[contact_id_ * 3 + 2] +
+                sqrt(pow(d.pin_data_.lambda_c[contact_id_ * 3], 2) +
+                     pow(d.pin_data_.lambda_c[contact_id_ * 3 + 1], 2));
 }
 
 template <typename Scalar>
@@ -35,25 +37,42 @@ void MultibodyFrictionConeResidualTpl<Scalar>::computeJacobians(
       pin_model_, d.pin_data_, constraint_models_, d.constraint_datas_,
       d.settings);
 
-  d.Jx_.leftCols(pin_model_.nv).noalias() =
-      Acone_ *
-      d.pin_data_.dlambda_dq.block(contact_id_ * 3, 0, 3, pin_model_.nv);
-  d.Jx_.rightCols(pin_model_.nv).noalias() =
-      Acone_ *
-      d.pin_data_.dlambda_dv.block(contact_id_ * 3, 0, 3, pin_model_.nv);
   d.temp_.noalias() =
       d.pin_data_.dlambda_dtau.block(contact_id_ * 3, 0, 3, pin_model_.nv) *
       actuation_matrix_;
-  d.Ju_.noalias() = Acone_ * d.temp_;
+  d.dcone_df_ << d.pin_data_.lambda_c[contact_id_ * 3] /
+                     sqrt(pow(d.pin_data_.lambda_c[contact_id_ * 3], 2) +
+                          pow(d.pin_data_.lambda_c[contact_id_ * 3 + 1], 2)),
+      d.pin_data_.lambda_c[contact_id_ * 3 + 1] /
+          sqrt(pow(d.pin_data_.lambda_c[contact_id_ * 3], 2) +
+               pow(d.pin_data_.lambda_c[contact_id_ * 3 + 1], 2)),
+      -mu_;
+
+  d.Jx_.block(0, 0, 1, pin_model_.nv).noalias() =
+      -d.pin_data_.dlambda_dq.block(contact_id_ * 3 + 2, 0, 1, pin_model_.nv);
+  d.Jx_.block(0, pin_model_.nv, 1, pin_model_.nv).noalias() =
+      -d.pin_data_.dlambda_dv.block(contact_id_ * 3 + 2, 0, 1, pin_model_.nv);
+  d.Ju_.block(0, 0, 1, actuation_matrix_.cols()).noalias() =
+      -d.temp_.block(2, 0, 1, actuation_matrix_.cols());
+
+  d.Jx_.block(1, 0, 1, pin_model_.nv).noalias() =
+      d.dcone_df_ *
+      d.pin_data_.dlambda_dq.block(contact_id_ * 3, 0, 3, pin_model_.nv);
+  d.Jx_.block(1, pin_model_.nv, 1, pin_model_.nv).noalias() =
+      d.dcone_df_ *
+      d.pin_data_.dlambda_dv.block(contact_id_ * 3, 0, 3, pin_model_.nv);
+  d.Ju_.block(1, 0, 1, actuation_matrix_.cols()).noalias() =
+      d.dcone_df_ * d.temp_;
 }
 
 template <typename Scalar>
 MultibodyFrictionConeDataTpl<Scalar>::MultibodyFrictionConeDataTpl(
     const MultibodyFrictionConeResidualTpl<Scalar> *model)
-    : Base(model->ndx1, model->nu, 5), pin_data_(model->pin_model_),
+    : Base(model->ndx1, model->nu, 2), pin_data_(model->pin_model_),
       tau_(model->pin_model_.nv), temp_(3, model->nu) {
   tau_.setZero();
   temp_.setZero();
+  dcone_df_.setZero();
 
   pinocchio::initConstraintDynamics(model->pin_model_, pin_data_,
                                     model->constraint_models_);

tests/forces.cpp

@@ -1,3 +1,5 @@
+#include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
 #include <pinocchio/parsers/sample-models.hpp>
 #include "aligator/core/function-abstract.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
@@ -13,7 +15,7 @@
 
 BOOST_AUTO_TEST_SUITE(forces)
 
-BOOST_AUTO_TEST_CASE(contact_forces) {
+BOOST_AUTO_TEST_CASE(contact_forces_6d) {
   using namespace Eigen;
   using namespace pinocchio;
   using namespace aligator;
@@ -22,6 +24,7 @@ BOOST_AUTO_TEST_CASE(contact_forces) {
   using ContactForceResidual = aligator::ContactForceResidualTpl<double>;
   using StageFunctionData = aligator::StageFunctionDataTpl<double>;
   using Manifold = proxsuite::nlp::MultibodyPhaseSpace<double>;
+  using Vector3or6 = Eigen::Matrix<double, -1, 1, Eigen::ColMajor, 6, 1>;
 
   Model model;
   buildModels::humanoidRandom(model, true);
@@ -68,11 +71,16 @@ BOOST_AUTO_TEST_CASE(contact_forces) {
   MatrixXd act_matrix(model.nv, model.nv - 6);
   act_matrix.setZero();
   act_matrix.bottomRows(model.nv - 6).setIdentity();
-  VectorXd fref(6);
+
+  // Test force 6d
+  Vector3or6 fref{6};
   fref.setZero();
+  fref[2] = 100;
   ContactForceResidual fun =
       ContactForceResidual(space.ndx(), model, act_matrix, constraint_models,
                            prox_settings, fref, "RF_foot");
+
+  BOOST_CHECK(fun.getReference().isApprox(fref));
   shared_ptr<StageFunctionData> sfdata = fun.createData();
   shared_ptr<ContactForceData> fdata =
       std::static_pointer_cast<ContactForceData>(sfdata);
@@ -121,6 +129,120 @@ BOOST_AUTO_TEST_CASE(contact_forces) {
   BOOST_CHECK(lambda_partial_du_fd.isApprox(lambda_partial_du, sqrt(alpha)));
 }
 
+BOOST_AUTO_TEST_CASE(contact_forces_3d) {
+  using namespace Eigen;
+  using namespace pinocchio;
+  using namespace aligator;
+
+  using ContactForceData = aligator::ContactForceDataTpl<double>;
+  using ContactForceResidual = aligator::ContactForceResidualTpl<double>;
+  using StageFunctionData = aligator::StageFunctionDataTpl<double>;
+  using Manifold = proxsuite::nlp::MultibodyPhaseSpace<double>;
+  using Vector3or6 = Eigen::Matrix<double, -1, 1, Eigen::ColMajor, 6, 1>;
+
+  Model model;
+  buildModels::humanoidRandom(model, true);
+  Data data(model), data_fd(model);
+
+  VectorXd q = randomConfiguration(model);
+  VectorXd v = VectorXd::Random(model.nv);
+  VectorXd x0(model.nv * 2 + 1);
+  x0 << q, v;
+  VectorXd u0 = VectorXd::Random(model.nv - 6);
+
+  const std::string LF = "lleg6_joint";
+  const Model::JointIndex LF_id = model.getJointId(LF);
+  const std::string RF = "rleg6_joint";
+  const Model::JointIndex RF_id = model.getJointId(RF);
+
+  // Contact models and data
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)
+  constraint_models;
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
+  constraint_data;
+
+  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;
+  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;
+  ci_RF.name = "RF_foot";
+
+  constraint_models.push_back(ci_LF);
+  constraint_data.push_back(RigidConstraintData(ci_LF));
+  constraint_models.push_back(ci_RF);
+  constraint_data.push_back(RigidConstraintData(ci_RF));
+
+  const double mu0 = 0.;
+  ProximalSettings prox_settings(1e-12, mu0, 1);
+
+  Manifold space{model};
+  MatrixXd act_matrix(model.nv, model.nv - 6);
+  act_matrix.setZero();
+  act_matrix.bottomRows(model.nv - 6).setIdentity();
+
+  // Test force 3d
+  Vector3or6 fref{3};
+  fref.setZero();
+  fref[2] = 100;
+  ContactForceResidual fun =
+      ContactForceResidual(space.ndx(), model, act_matrix, constraint_models,
+                           prox_settings, fref, "RF_foot");
+  shared_ptr<StageFunctionData> sfdata = fun.createData();
+  shared_ptr<ContactForceData> fdata =
+      std::static_pointer_cast<ContactForceData>(sfdata);
+
+  BOOST_CHECK(fun.getReference().isApprox(fref));
+
+  forwardKinematics(model, fdata->pin_data_, q);
+  updateFramePlacements(model, data);
+  fun.evaluate(x0, u0, *fdata);
+  fun.computeJacobians(x0, u0, *fdata);
+
+  MatrixXd lambda_partial_dx(3, model.nv * 2);
+  MatrixXd lambda_partial_du(3, model.nv - 6);
+  lambda_partial_dx = fdata->Jx_;
+  lambda_partial_du = fdata->Ju_;
+
+  // Data_fd
+  MatrixXd lambda_partial_dx_fd(3, model.nv * 2);
+  lambda_partial_dx_fd.setZero();
+  MatrixXd lambda_partial_du_fd(3, model.nv - 6);
+  lambda_partial_du_fd.setZero();
+
+  const VectorXd lambda0 = fdata->value_;
+  VectorXd v_eps(VectorXd::Zero(model.nv * 2));
+  VectorXd u_eps(VectorXd::Zero(model.nv - 6));
+  VectorXd x_plus(model.nq + model.nv);
+  VectorXd u_plus(model.nv - 6);
+
+  const double alpha = 1e-8;
+
+  for (int k = 0; k < model.nv * 2; ++k) {
+    v_eps[k] += alpha;
+    x_plus = space.integrate(x0, v_eps);
+    fun.evaluate(x_plus, u0, *fdata);
+    lambda_partial_dx_fd.col(k) = (fdata->value_ - lambda0) / alpha;
+    v_eps[k] = 0.;
+  }
+
+  for (int k = 0; k < model.nv - 6; ++k) {
+    u_eps[k] += alpha;
+    u_plus = u0 + u_eps;
+    fun.evaluate(x0, u_plus, *fdata);
+    lambda_partial_du_fd.col(k) = (fdata->value_ - lambda0) / alpha;
+    u_eps[k] = 0.;
+  }
+  // std::cout << sqrt(alpha) << std::endl;
+  BOOST_CHECK(lambda_partial_dx_fd.isApprox(lambda_partial_dx, sqrt(alpha)));
+  BOOST_CHECK(lambda_partial_du_fd.isApprox(lambda_partial_du, sqrt(alpha)));
+}
+
 BOOST_AUTO_TEST_CASE(wrench_cone) {
   using namespace Eigen;
   using namespace pinocchio;
@@ -303,15 +425,15 @@ BOOST_AUTO_TEST_CASE(friction_cone) {
   fun.evaluate(x0, u0, *fdata);
   fun.computeJacobians(x0, u0, *fdata);
 
-  MatrixXd cone_partial_dx(5, model.nv * 2);
-  MatrixXd cone_partial_du(5, model.nv - 6);
+  MatrixXd cone_partial_dx(2, model.nv * 2);
+  MatrixXd cone_partial_du(2, model.nv - 6);
   cone_partial_dx = fdata->Jx_;
   cone_partial_du = fdata->Ju_;
 
   // Data_fd
-  MatrixXd cone_partial_dx_fd(5, model.nv * 2);
+  MatrixXd cone_partial_dx_fd(2, model.nv * 2);
   cone_partial_dx_fd.setZero();
-  MatrixXd cone_partial_du_fd(5, model.nv - 6);
+  MatrixXd cone_partial_du_fd(2, model.nv - 6);
   cone_partial_du_fd.setZero();
 
   const VectorXd cone0 = fdata->value_;