From 72f7092580f6ce3e7f4479366f76af2377f7f600 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Thu, 17 Oct 2024 16:59:29 +0200 Subject: [PATCH 1/4] Add multibody friction cone and rename centroidal friction cone --- .../src/modelling/expose-centroidal.cpp | 18 +-- ...-cone.hpp => centroidal-friction-cone.hpp} | 17 +-- .../centroidal/centroidal-friction-cone.hxx | 34 ++++++ .../centroidal/centroidal-friction-cone.txx | 11 ++ .../modelling/centroidal/friction-cone.hxx | 36 ------ .../modelling/centroidal/friction-cone.txx | 11 -- .../multibody/multibody-friction-cone.hpp | 112 ++++++++++++++++++ .../multibody/multibody-friction-cone.hxx | 67 +++++++++++ .../multibody/multibody-friction-cone.txx | 11 ++ .../centroidal/centroidal-friction-cone.cpp | 8 ++ src/modelling/centroidal/friction-cone.cpp | 8 -- tests/python/test_centroidal.py | 2 +- 12 files changed, 263 insertions(+), 72 deletions(-) rename include/aligator/modelling/centroidal/{friction-cone.hpp => centroidal-friction-cone.hpp} (72%) create mode 100644 include/aligator/modelling/centroidal/centroidal-friction-cone.hxx create mode 100644 include/aligator/modelling/centroidal/centroidal-friction-cone.txx delete mode 100644 include/aligator/modelling/centroidal/friction-cone.hxx delete mode 100644 include/aligator/modelling/centroidal/friction-cone.txx create mode 100644 include/aligator/modelling/multibody/multibody-friction-cone.hpp create mode 100644 include/aligator/modelling/multibody/multibody-friction-cone.hxx create mode 100644 include/aligator/modelling/multibody/multibody-friction-cone.txx create mode 100644 src/modelling/centroidal/centroidal-friction-cone.cpp delete mode 100644 src/modelling/centroidal/friction-cone.cpp diff --git a/bindings/python/src/modelling/expose-centroidal.cpp b/bindings/python/src/modelling/expose-centroidal.cpp index 611bc2737..74c75dd78 100644 --- a/bindings/python/src/modelling/expose-centroidal.cpp +++ b/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; using CentroidalAccelerationData = CentroidalAccelerationDataTpl; - using FrictionConeResidual = FrictionConeResidualTpl; - using FrictionConeData = FrictionConeDataTpl; + using CentroidalFrictionConeResidual = + CentroidalFrictionConeResidualTpl; + using CentroidalFrictionConeData = CentroidalFrictionConeDataTpl; using CentroidalWrenchConeResidual = CentroidalWrenchConeResidualTpl; using CentroidalWrenchConeData = CentroidalWrenchConeDataTpl; @@ -137,17 +138,18 @@ void exposeCentroidalFunctions() { "CentroidalAccelerationData", "Data Structure for CentroidalAcceleration", bp::no_init); - bp::class_>( - "FrictionConeResidual", + bp::class_>( + "CentroidalFrictionConeResidual", "A residual function :math:`r(x) = [fz, mu2 * fz2 - (fx2 + fy2)]` ", bp::init( ("self"_a, "ndx", "nu", "k", "mu", "epsilon"))) .def(func_visitor); - bp::register_ptr_to_python>(); + bp::register_ptr_to_python>(); - bp::class_>( - "FrictionConeData", "Data Structure for FrictionCone", bp::no_init); + bp::class_>( + "CentroidalFrictionConeData", "Data Structure for CentroidalFrictionCone", + bp::no_init); bp::class_>( "CentroidalWrenchConeResidual", diff --git a/include/aligator/modelling/centroidal/friction-cone.hpp b/include/aligator/modelling/centroidal/centroidal-friction-cone.hpp similarity index 72% rename from include/aligator/modelling/centroidal/friction-cone.hpp rename to include/aligator/modelling/centroidal/centroidal-friction-cone.hpp index 5d37e7fca..043ff9f48 100644 --- a/include/aligator/modelling/centroidal/friction-cone.hpp +++ b/include/aligator/modelling/centroidal/centroidal-friction-cone.hpp @@ -16,20 +16,20 @@ namespace aligator { * \f$ small threshold and \f$ \mu \f$ friction coefficient. */ -template struct FrictionConeDataTpl; +template struct CentroidalFrictionConeDataTpl; template -struct FrictionConeResidualTpl : StageFunctionTpl<_Scalar> { +struct CentroidalFrictionConeResidualTpl : StageFunctionTpl<_Scalar> { public: using Scalar = _Scalar; ALIGATOR_DYNAMIC_TYPEDEFS(Scalar); using Base = StageFunctionTpl; using BaseData = typename Base::Data; - using Data = FrictionConeDataTpl; + using Data = CentroidalFrictionConeDataTpl; - 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 -struct FrictionConeDataTpl : StageFunctionDataTpl { +struct CentroidalFrictionConeDataTpl : StageFunctionDataTpl { EIGEN_MAKE_ALIGNED_OPERATOR_NEW using Base = StageFunctionDataTpl; using Matrix23s = Eigen::Matrix; Matrix23s Jtemp_; - FrictionConeDataTpl(const FrictionConeResidualTpl *model); + CentroidalFrictionConeDataTpl( + const CentroidalFrictionConeResidualTpl *model); }; } // namespace aligator #ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION -#include "aligator/modelling/centroidal/friction-cone.txx" +#include "aligator/modelling/centroidal/centroidal-friction-cone.txx" #endif diff --git a/include/aligator/modelling/centroidal/centroidal-friction-cone.hxx b/include/aligator/modelling/centroidal/centroidal-friction-cone.hxx new file mode 100644 index 000000000..f8aacd34b --- /dev/null +++ b/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 +void CentroidalFrictionConeResidualTpl::evaluate( + const ConstVectorRef &, const ConstVectorRef &u, BaseData &data) const { + Data &d = static_cast(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 +void CentroidalFrictionConeResidualTpl::computeJacobians( + const ConstVectorRef &, const ConstVectorRef &u, BaseData &data) const { + Data &d = static_cast(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 +CentroidalFrictionConeDataTpl::CentroidalFrictionConeDataTpl( + const CentroidalFrictionConeResidualTpl *model) + : Base(*model) { + Jtemp_.setZero(); +} + +} // namespace aligator diff --git a/include/aligator/modelling/centroidal/centroidal-friction-cone.txx b/include/aligator/modelling/centroidal/centroidal-friction-cone.txx new file mode 100644 index 000000000..f207539e8 --- /dev/null +++ b/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; +extern template struct CentroidalFrictionConeDataTpl; + +} // namespace aligator diff --git a/include/aligator/modelling/centroidal/friction-cone.hxx b/include/aligator/modelling/centroidal/friction-cone.hxx deleted file mode 100644 index 17a7689cc..000000000 --- a/include/aligator/modelling/centroidal/friction-cone.hxx +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "aligator/modelling/centroidal/friction-cone.hpp" - -namespace aligator { - -template -void FrictionConeResidualTpl::evaluate(const ConstVectorRef &, - const ConstVectorRef &u, - BaseData &data) const { - Data &d = static_cast(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 -void FrictionConeResidualTpl::computeJacobians(const ConstVectorRef &, - const ConstVectorRef &u, - BaseData &data) const { - Data &d = static_cast(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 -FrictionConeDataTpl::FrictionConeDataTpl( - const FrictionConeResidualTpl *model) - : Base(*model) { - Jtemp_.setZero(); -} - -} // namespace aligator diff --git a/include/aligator/modelling/centroidal/friction-cone.txx b/include/aligator/modelling/centroidal/friction-cone.txx deleted file mode 100644 index ca312e8e3..000000000 --- a/include/aligator/modelling/centroidal/friction-cone.txx +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include "aligator/context.hpp" -#include "aligator/modelling/centroidal/friction-cone.hpp" - -namespace aligator { - -extern template struct FrictionConeResidualTpl; -extern template struct FrictionConeDataTpl; - -} // namespace aligator diff --git a/include/aligator/modelling/multibody/multibody-friction-cone.hpp b/include/aligator/modelling/multibody/multibody-friction-cone.hpp new file mode 100644 index 000000000..89deb7c88 --- /dev/null +++ b/include/aligator/modelling/multibody/multibody-friction-cone.hpp @@ -0,0 +1,112 @@ +#pragma once + +#include "./fwd.hpp" +#include "aligator/core/function-abstract.hpp" + +#include + +#ifdef ALIGATOR_PINOCCHIO_V3 +#include + +namespace aligator { + +template struct MultibodyFrictionConeDataTpl; + +/** + * @brief This residual returns the derivative of centroidal momentum + * for a kinodynamics model. + */ + +template +struct MultibodyFrictionConeResidualTpl : StageFunctionTpl<_Scalar> { +public: + using Scalar = _Scalar; + ALIGATOR_DYNAMIC_TYPEDEFS(Scalar); + using Base = StageFunctionTpl; + using BaseData = typename Base::Data; + using Model = pinocchio::ModelTpl; + using SE3 = pinocchio::SE3Tpl; + using Data = MultibodyFrictionConeDataTpl; + using RigidConstraintModelVector = PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR( + pinocchio::RigidConstraintModel); + using RigidConstraintDataVector = + PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData); + using ProxSettings = pinocchio::ProximalSettingsTpl; + + Model pin_model_; + MatrixXs actuation_matrix_; + RigidConstraintModelVector constraint_models_; + ProxSettings prox_settings_; + double mu_; + int contact_id_; + + Eigen::Matrix 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 createData() const { + return std::make_shared(this); + } +}; + +template +struct MultibodyFrictionConeDataTpl : StageFunctionDataTpl { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using Base = StageFunctionDataTpl; + using PinData = pinocchio::DataTpl; + using VectorXs = typename math_types::VectorXs; + using MatrixXs = typename math_types::MatrixXs; + using Matrix6Xs = typename math_types::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 settings; + + MultibodyFrictionConeDataTpl( + const MultibodyFrictionConeResidualTpl *model); +}; + +} // namespace aligator + +#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION +#include "aligator/modelling/multibody/multibody-friction-cone.txx" +#endif + +#endif // ALIGATOR_PINOCCHIO_V3 diff --git a/include/aligator/modelling/multibody/multibody-friction-cone.hxx b/include/aligator/modelling/multibody/multibody-friction-cone.hxx new file mode 100644 index 000000000..bcadeab7f --- /dev/null +++ b/include/aligator/modelling/multibody/multibody-friction-cone.hxx @@ -0,0 +1,67 @@ +#pragma once + +#include "aligator/modelling/multibody/multibody-friction-cone.hpp" + +#include +#include + +namespace aligator { + +template +void MultibodyFrictionConeResidualTpl::evaluate(const ConstVectorRef &x, + const ConstVectorRef &u, + BaseData &data) const { + Data &d = static_cast(data); + + const auto q = x.head(pin_model_.nq); + const auto v = x.tail(pin_model_.nv); + + d.tau_.noalias() = actuation_matrix_ * u; + pinocchio::constraintDynamics(pin_model_, d.pin_data_, q, v, d.tau_, + constraint_models_, d.constraint_datas_, + d.settings); + + // Unilateral contact + d.value_.noalias() = + Acone_ * d.pin_data_.lambda_c.segment(contact_id_ * 3, 3); +} + +template +void MultibodyFrictionConeResidualTpl::computeJacobians( + const ConstVectorRef &, const ConstVectorRef &, BaseData &data) const { + Data &d = static_cast(data); + + pinocchio::computeConstraintDynamicsDerivatives( + 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_; +} + +template +MultibodyFrictionConeDataTpl::MultibodyFrictionConeDataTpl( + const MultibodyFrictionConeResidualTpl *model) + : Base(model->ndx1, model->nu, 5), pin_data_(model->pin_model_), + tau_(model->pin_model_.nv), temp_(3, model->nu) { + tau_.setZero(); + temp_.setZero(); + + pinocchio::initConstraintDynamics(model->pin_model_, pin_data_, + model->constraint_models_); + for (auto cm = std::begin(model->constraint_models_); + cm != std::end(model->constraint_models_); ++cm) { + constraint_datas_.push_back( + pinocchio::RigidConstraintDataTpl(*cm)); + } +} + +} // namespace aligator diff --git a/include/aligator/modelling/multibody/multibody-friction-cone.txx b/include/aligator/modelling/multibody/multibody-friction-cone.txx new file mode 100644 index 000000000..34f4f124f --- /dev/null +++ b/include/aligator/modelling/multibody/multibody-friction-cone.txx @@ -0,0 +1,11 @@ +#pragma once + +#include "aligator/context.hpp" +#include "aligator/modelling/multibody/multibody-friction-cone.hpp" + +namespace aligator { + +extern template struct MultibodyFrictionConeResidualTpl; +extern template struct MultibodyFrictionConeDataTpl; + +} // namespace aligator diff --git a/src/modelling/centroidal/centroidal-friction-cone.cpp b/src/modelling/centroidal/centroidal-friction-cone.cpp new file mode 100644 index 000000000..d6ec65fde --- /dev/null +++ b/src/modelling/centroidal/centroidal-friction-cone.cpp @@ -0,0 +1,8 @@ +#include "aligator/modelling/centroidal/centroidal-friction-cone.hxx" + +namespace aligator { + +template struct CentroidalFrictionConeResidualTpl; +template struct CentroidalFrictionConeDataTpl; + +} // namespace aligator diff --git a/src/modelling/centroidal/friction-cone.cpp b/src/modelling/centroidal/friction-cone.cpp deleted file mode 100644 index 6b6a39c7d..000000000 --- a/src/modelling/centroidal/friction-cone.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "aligator/modelling/centroidal/friction-cone.hxx" - -namespace aligator { - -template struct FrictionConeResidualTpl; -template struct FrictionConeDataTpl; - -} // namespace aligator diff --git a/tests/python/test_centroidal.py b/tests/python/test_centroidal.py index 3ba2fbe31..3de54c4bc 100644 --- a/tests/python/test_centroidal.py +++ b/tests/python/test_centroidal.py @@ -243,7 +243,7 @@ def test_friction_cone(): mu = 0.5 epsilon = 1e-3 - fun = aligator.FrictionConeResidual(ndx, nu, k, mu, epsilon) + fun = aligator.CentroidalFrictionConeResidual(ndx, nu, k, mu, epsilon) fdata = fun.createData() fun.evaluate(x0, u0, fdata) From 93ecc35e6273d573d30a93e363c7fc56afe84aa2 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 18 Oct 2024 14:21:42 +0200 Subject: [PATCH 2/4] Add friction cone test --- .../src/modelling/expose-force-cost.cpp | 25 ++++ .../multibody/multibody-friction-cone.cpp | 11 ++ tests/forces.cpp | 111 ++++++++++++++++++ 3 files changed, 147 insertions(+) create mode 100644 src/modelling/multibody/multibody-friction-cone.cpp diff --git a/bindings/python/src/modelling/expose-force-cost.cpp b/bindings/python/src/modelling/expose-force-cost.cpp index 9047b42a7..16066476b 100644 --- a/bindings/python/src/modelling/expose-force-cost.cpp +++ b/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; using MultibodyWrenchConeData = MultibodyWrenchConeDataTpl; + using MultibodyFrictionConeResidual = + MultibodyFrictionConeResidualTpl; + using MultibodyFrictionConeData = MultibodyFrictionConeDataTpl; + bp::class_>( "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", "A residual function :math:`r(x) = Af` ", + bp::no_init) + .def(bp::init &, + 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::no_init) + .def_readwrite("tau", &MultibodyFrictionConeData::tau_) + .def_readwrite("pin_data", &MultibodyFrictionConeData::pin_data_) + .def_readwrite("constraint_datas", + &MultibodyFrictionConeData::constraint_datas_); } } // namespace python diff --git a/src/modelling/multibody/multibody-friction-cone.cpp b/src/modelling/multibody/multibody-friction-cone.cpp new file mode 100644 index 000000000..81693c775 --- /dev/null +++ b/src/modelling/multibody/multibody-friction-cone.cpp @@ -0,0 +1,11 @@ +#include "aligator/modelling/multibody/multibody-friction-cone.hxx" +#ifdef ALIGATOR_PINOCCHIO_V3 + +namespace aligator { + +template struct MultibodyFrictionConeResidualTpl; +template struct MultibodyFrictionConeDataTpl; + +} // namespace aligator + +#endif diff --git a/tests/forces.cpp b/tests/forces.cpp index 5014eb1f9..575b7f443 100644 --- a/tests/forces.cpp +++ b/tests/forces.cpp @@ -9,6 +9,7 @@ #ifdef ALIGATOR_PINOCCHIO_V3 #include #include +#include BOOST_AUTO_TEST_SUITE(forces) @@ -231,6 +232,116 @@ BOOST_AUTO_TEST_CASE(wrench_cone) { BOOST_CHECK(cone_partial_du_fd.isApprox(cone_partial_du, sqrt(alpha))); } +BOOST_AUTO_TEST_CASE(friction_cone) { + using namespace Eigen; + using namespace pinocchio; + using namespace aligator; + + using MultibodyFrictionConeData = + aligator::MultibodyFrictionConeDataTpl; + using MultibodyFrictionConeResidual = + aligator::MultibodyFrictionConeResidualTpl; + using StageFunctionData = aligator::StageFunctionDataTpl; + using Manifold = proxsuite::nlp::MultibodyPhaseSpace; + + 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(); + + double mu = 0.1; + MultibodyFrictionConeResidual fun = MultibodyFrictionConeResidual( + space.ndx(), model, act_matrix, constraint_models, prox_settings, + "LF_foot", mu); + shared_ptr sfdata = fun.createData(); + shared_ptr fdata = + std::static_pointer_cast(sfdata); + + forwardKinematics(model, fdata->pin_data_, q); + updateFramePlacements(model, data); + 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); + cone_partial_dx = fdata->Jx_; + cone_partial_du = fdata->Ju_; + + // Data_fd + MatrixXd cone_partial_dx_fd(5, model.nv * 2); + cone_partial_dx_fd.setZero(); + MatrixXd cone_partial_du_fd(5, model.nv - 6); + cone_partial_du_fd.setZero(); + + const VectorXd cone0 = 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); + cone_partial_dx_fd.col(k) = (fdata->value_ - cone0) / 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); + cone_partial_du_fd.col(k) = (fdata->value_ - cone0) / alpha; + u_eps[k] = 0.; + } + + BOOST_CHECK(cone_partial_dx_fd.isApprox(cone_partial_dx, sqrt(alpha))); + BOOST_CHECK(cone_partial_du_fd.isApprox(cone_partial_du, sqrt(alpha))); +} + BOOST_AUTO_TEST_SUITE_END() #endif From 25925e4170d964a7b28fcd455b7cf1e072490c13 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 18 Oct 2024 14:29:13 +0200 Subject: [PATCH 3/4] Changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index ac91487cd..f06e6b786 100644 --- a/CHANGELOG.md +++ b/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 + ### 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. +- Rename friction cone for centroidal into CentroidalFrictionCone ## [0.9.0] - 2024-10-11 From 739bc239d5f4f39e765e517f121c76acf6b7634a Mon Sep 17 00:00:00 2001 From: Wilson Date: Fri, 18 Oct 2024 16:03:28 +0200 Subject: [PATCH 4/4] Apply suggestions from code review --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f06e6b786..b1bc44a4e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,13 +9,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added -- Add a multibody friction cone cost +- 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. -- Rename friction cone for centroidal into CentroidalFrictionCone +- [API BREAKING] Rename friction cone for centroidal into CentroidalFrictionCone ([#234](https://github.com/Simple-Robotics/aligator/pull/234)) ## [0.9.0] - 2024-10-11