Merge branch 'devel' into pre-commit-ci-update-config

Author: ManifoldFR

CHANGELOG.md

@@ -11,12 +11,18 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 - Only link against needed pinocchio libraries ([#260](https://github.com/Simple-Robotics/aligator/pull/260))
 - Use Pinocchio instantiated functions ([#261](https://github.com/Simple-Robotics/aligator/pull/261))
+- Link to pinocchio collision
 
 ### Fixed
 
 - `LinesearchVariant::init()` should not be called unless the step accpetance strategy is a linesearch
 - Fixed compilation issues with C++20 (resolving [#246](https://github.com/Simple-Robotics/aligator/issues/246) and [#254](https://github.com/Simple-Robotics/aligator/discussions/254))
 
+### Added
+
+- Add a collision distance residual for collision pair
+- Add a relaxed log-barrier cost function
+
 ## [0.10.0] - 2024-12-09
 
 ### Added

CMakeLists.txt

@@ -398,7 +398,10 @@ function(create_library)
   )
 
   if(BUILD_WITH_PINOCCHIO_SUPPORT)
-    target_link_libraries(${PROJECT_NAME} PUBLIC pinocchio::pinocchio_default)
+    target_link_libraries(
+      ${PROJECT_NAME}
+      PUBLIC pinocchio::pinocchio_default pinocchio::pinocchio_collision
+    )
   endif()
 
   if(BUILD_WITH_OPENMP_SUPPORT)

bindings/python/src/modelling/expose-composite-costs.cpp

@@ -3,6 +3,7 @@
 
 #include "aligator/modelling/costs/quad-state-cost.hpp"
 #include "aligator/modelling/costs/log-residual-cost.hpp"
+#include "aligator/modelling/costs/relaxed-log-barrier.hpp"
 #include "aligator/python/polymorphic-convertible.hpp"
 
 namespace aligator {
@@ -25,6 +26,7 @@ void exposeComposites() {
   using QuadStateCost = QuadraticStateCostTpl<Scalar>;
   using QuadControlCost = QuadraticControlCostTpl<Scalar>;
   using LogResCost = LogResidualCostTpl<Scalar>;
+  using RelaxedLogCost = RelaxedLogBarrierCostTpl<Scalar>;
 
   PolymorphicMultiBaseVisitor<CostAbstract> visitor;
   bp::class_<QuadResCost, bp::bases<CostAbstract>>(
@@ -47,6 +49,18 @@ void exposeComposites() {
       .def(CopyableVisitor<LogResCost>())
       .def(visitor);
 
+  bp::class_<RelaxedLogCost, bp::bases<CostAbstract>>(
+      "RelaxedLogBarrierCost", "Relaxed log-barrier composite cost.",
+      bp::init<PolyManifold, PolyFunction, const ConstVectorRef &,
+               const Scalar>(
+          bp::args("self", "space", "function", "weights", "threshold")))
+      .def(bp::init<PolyManifold, PolyFunction, const Scalar, const Scalar>(
+          bp::args("self", "space", "function", "weights", "threshold")))
+      .def_readwrite("residual", &RelaxedLogCost::residual_)
+      .def_readwrite("weights", &RelaxedLogCost::barrier_weights_)
+      .def(CopyableVisitor<RelaxedLogCost>())
+      .def(visitor);
+
   bp::class_<CompositeData, bp::bases<CostData>>(
       "CompositeCostData",
       bp::init<int, int, shared_ptr<context::StageFunctionData>>(

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

@@ -7,6 +7,7 @@
 #include "aligator/modelling/multibody/frame-placement.hpp"
 #include "aligator/modelling/multibody/frame-velocity.hpp"
 #include "aligator/modelling/multibody/frame-translation.hpp"
+#include "aligator/modelling/multibody/frame-collision.hpp"
 #include "aligator/python/polymorphic-convertible.hpp"
 #ifdef ALIGATOR_PINOCCHIO_V3
 #include "aligator/modelling/multibody/constrained-rnea.hpp"
@@ -52,6 +53,11 @@ void exposeFrameFunctions() {
   using FrameTranslation = FrameTranslationResidualTpl<Scalar>;
   using FrameTranslationData = FrameTranslationDataTpl<Scalar>;
 
+  using FrameCollision = FrameCollisionResidualTpl<Scalar>;
+  using FrameCollisionData = FrameCollisionDataTpl<Scalar>;
+
+  using pinocchio::GeometryModel;
+
   bp::register_ptr_to_python<shared_ptr<PinData>>();
 
   PolymorphicMultiBaseVisitor<UnaryFunction, StageFunction> unary_visitor;
@@ -119,6 +125,24 @@ void exposeFrameFunctions() {
       .def_readonly("fJf", &FrameTranslationData::fJf_)
       .def_readonly("pin_data", &FrameTranslationData::pin_data_,
                     "Pinocchio data struct.");
+
+  bp::class_<FrameCollision, bp::bases<UnaryFunction>>(
+      "FrameCollisionResidual", "Frame collision residual function.",
+      bp::init<int, int, const PinModel &, const GeometryModel &,
+               pinocchio::PairIndex>(bp::args("self", "ndx", "nu", "model",
+                                              "geom_model", "frame_pair_id")))
+      .def(FrameAPIVisitor<FrameCollision>())
+      .def(unary_visitor);
+
+  bp::register_ptr_to_python<shared_ptr<FrameCollisionData>>();
+
+  bp::class_<FrameCollisionData, bp::bases<context::StageFunctionData>>(
+      "FrameCollisionData", "Data struct for FrameCollisionResidual.",
+      bp::no_init)
+      .def_readonly("pin_data", &FrameCollisionData::pin_data_,
+                    "Pinocchio data struct.")
+      .def_readonly("geom_data", &FrameCollisionData::geometry_,
+                    "Geometry data struct.");
 }
 
 #ifdef ALIGATOR_PINOCCHIO_V3

examples/ur5_reach.py

@@ -8,6 +8,7 @@
 
 from aligator import constraints, manifolds, dynamics  # noqa
 from pinocchio.visualize import MeshcatVisualizer
+import hppfcl
 
 from utils import ArgsBase, get_endpoint_traj
 
@@ -16,6 +17,7 @@ class Args(ArgsBase):
     plot: bool = True
     fddp: bool = False
     bounds: bool = False
+    collisions: bool = False
 
 
 args = Args().parse_args()
@@ -32,6 +34,30 @@ class Args(ArgsBase):
 vizer.initViewer(open=args.display, loadModel=True)
 vizer.setBackgroundColor()
 
+fr_name = "universe"
+fr_id = rmodel.getFrameId(fr_name)
+joint_id = rmodel.frames[fr_id].parentJoint
+if args.collisions:
+    obstacle_loc = pin.SE3.Identity()
+    obstacle_loc.translation[0] = 0.3
+    obstacle_loc.translation[1] = 0.5
+    obstacle_loc.translation[2] = 0.3
+    geom_object = pin.GeometryObject(
+        "capsule", fr_id, joint_id, hppfcl.Capsule(0.05, 0.4), obstacle_loc
+    )
+
+    fr_id2 = rmodel.getFrameId("wrist_3_joint")
+    joint_id2 = rmodel.frames[fr_id2].parentJoint
+    geom_object2 = pin.GeometryObject(
+        "endeffector", fr_id2, joint_id2, hppfcl.Sphere(0.1), pin.SE3.Identity()
+    )
+
+    geometry = pin.GeometryModel()
+    ig_frame = geometry.addGeometryObject(geom_object)
+    ig_frame2 = geometry.addGeometryObject(geom_object2)
+    geometry.addCollisionPair(pin.CollisionPair(ig_frame, ig_frame2))
+
+    vizer.addGeometryObject(geom_object, [1.0, 1.0, 0.5, 1.0])
 
 x0 = space.neutral()
 
@@ -61,6 +87,12 @@ class Args(ArgsBase):
 tool_name = "tool0"
 tool_id = rmodel.getFrameId(tool_name)
 target_pos = np.array([0.15, 0.65, 0.5])
+target_place = pin.SE3.Identity()
+target_place.translation = target_pos
+target_object = pin.GeometryObject(
+    "target", fr_id, joint_id, hppfcl.Sphere(0.05), target_place
+)
+vizer.addGeometryObject(target_object, [0.5, 0.5, 1.0, 1.0])
 print(target_pos)
 
 frame_fn = aligator.FrameTranslationResidual(ndx, nu, rmodel, target_pos, tool_id)
@@ -71,14 +103,18 @@ class Args(ArgsBase):
 )
 wt_x_term = wt_x.copy()
 wt_x_term[:] = 1e-4
-wt_frame_pos = 10.0 * np.eye(frame_fn.nr)
+wt_frame_pos = 100.0 * np.eye(frame_fn.nr)
 wt_frame_vel = 100.0 * np.ones(frame_vel_fn.nr)
 wt_frame_vel = np.diag(wt_frame_vel)
 
 term_cost = aligator.CostStack(space, nu)
-term_cost.addCost(aligator.QuadraticCost(wt_x_term, wt_u * 0))
-term_cost.addCost(aligator.QuadraticResidualCost(space, frame_fn, wt_frame_pos))
-term_cost.addCost(aligator.QuadraticResidualCost(space, frame_vel_fn, wt_frame_vel))
+term_cost.addCost("reg", aligator.QuadraticCost(wt_x_term, wt_u * 0))
+term_cost.addCost(
+    "frame", aligator.QuadraticResidualCost(space, frame_fn, wt_frame_pos)
+)
+term_cost.addCost(
+    "vel", aligator.QuadraticResidualCost(space, frame_vel_fn, wt_frame_vel)
+)
 
 u_max = rmodel.effortLimit
 u_min = -u_max
@@ -103,12 +139,16 @@ def computeQuasistatic(model: pin.Model, x0, a):
 
 
 stages = []
-
 for i in range(nsteps):
     rcost = aligator.CostStack(space, nu)
-    rcost.addCost(aligator.QuadraticCost(wt_x * dt, wt_u * dt))
+    rcost.addCost("reg", aligator.QuadraticCost(wt_x * dt, wt_u * dt))
 
     stm = aligator.StageModel(rcost, discrete_dynamics)
+    if args.collisions:
+        # Distance to obstacle constrained between 0.1 and 100 m
+        cstr_set = constraints.BoxConstraint(np.array([0.1]), np.array([100]))
+        frame_col = aligator.FrameCollisionResidual(ndx, nu, rmodel, geometry, 0)
+        stm.addConstraint(frame_col, cstr_set)
     if args.bounds:
         stm.addConstraint(*make_control_bounds())
     stages.append(stm)
@@ -119,7 +159,7 @@ def computeQuasistatic(model: pin.Model, x0, a):
 
 mu_init = 1e-7
 verbose = aligator.VerboseLevel.VERBOSE
-max_iters = 40
+max_iters = 500
 solver = aligator.SolverProxDDP(tol, mu_init, max_iters=max_iters, verbose=verbose)
 solver.rollout_type = aligator.ROLLOUT_NONLINEAR
 solver.sa_strategy = aligator.SA_LINESEARCH_NONMONOTONE

include/aligator/modelling/costs/relaxed-log-barrier.hpp

@@ -0,0 +1,62 @@
+/// @file
+/// @copyright Copyright (C) 2025 INRIA
+#pragma once
+
+#include "./composite-costs.hpp"
+
+namespace aligator {
+
+/// @brief  Log-barrier of an underlying cost function.
+template <typename Scalar>
+struct RelaxedLogBarrierCostTpl : CostAbstractTpl<Scalar> {
+  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
+  using CostDataAbstract = CostDataAbstractTpl<Scalar>;
+  using Data = CompositeCostDataTpl<Scalar>;
+  using StageFunction = StageFunctionTpl<Scalar>;
+  using Base = CostAbstractTpl<Scalar>;
+  using Manifold = ManifoldAbstractTpl<Scalar>;
+
+  VectorXs barrier_weights_;
+  xyz::polymorphic<StageFunction> residual_;
+  Scalar threshold_;
+
+  RelaxedLogBarrierCostTpl(xyz::polymorphic<Manifold> space,
+                           xyz::polymorphic<StageFunction> function,
+                           const ConstVectorRef &weight,
+                           const Scalar threshold);
+
+  RelaxedLogBarrierCostTpl(xyz::polymorphic<Manifold> space,
+                           xyz::polymorphic<StageFunction> function,
+                           const Scalar weight, const Scalar threshold);
+
+  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
+                CostDataAbstract &data) const;
+
+  void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u,
+                        CostDataAbstract &data) const;
+
+  void computeHessians(const ConstVectorRef &, const ConstVectorRef &,
+                       CostDataAbstract &data) const;
+
+  shared_ptr<CostDataAbstract> createData() const {
+    return std::make_shared<Data>(this->ndx(), this->nu,
+                                  residual_->createData());
+  }
+
+  /// @brief Get a pointer to the underlying type of the residual, by attempting
+  /// to cast.
+  template <typename Derived> Derived *getResidual() {
+    return dynamic_cast<Derived *>(&*residual_);
+  }
+
+  /// @copybrief getResidual().
+  template <typename Derived> const Derived *getResidual() const {
+    return dynamic_cast<const Derived *>(&*residual_);
+  }
+};
+
+extern template struct RelaxedLogBarrierCostTpl<context::Scalar>;
+
+} // namespace aligator
+
+#include "./relaxed-log-barrier.hxx"

include/aligator/modelling/costs/relaxed-log-barrier.hxx

@@ -0,0 +1,100 @@
+/// @file
+/// @copyright Copyright (C) 2025 INRIA
+#pragma once
+
+#include "./relaxed-log-barrier.hpp"
+
+namespace aligator {
+
+template <typename Scalar>
+RelaxedLogBarrierCostTpl<Scalar>::RelaxedLogBarrierCostTpl(
+    xyz::polymorphic<Manifold> space, xyz::polymorphic<StageFunction> function,
+    const ConstVectorRef &weight, const Scalar threshold)
+    : Base(space, function->nu), barrier_weights_(weight), residual_(function),
+      threshold_(threshold) {
+  if (weight.size() != function->nr) {
+    ALIGATOR_RUNTIME_ERROR(
+        "weight argument dimension ({:d}) != function codimension ({:d})",
+        weight.size(), function->nr);
+  }
+  bool negs = (weight.array() <= 0.0).any();
+  if (negs) {
+    ALIGATOR_RUNTIME_ERROR("weight coefficients must be > 0.");
+  }
+  if (threshold_ <= 0.) {
+    ALIGATOR_RUNTIME_ERROR("threshold must be > 0.");
+  }
+}
+
+template <typename Scalar>
+RelaxedLogBarrierCostTpl<Scalar>::RelaxedLogBarrierCostTpl(
+    xyz::polymorphic<Manifold> space, xyz::polymorphic<StageFunction> function,
+    const Scalar weight, const Scalar threshold)
+    : RelaxedLogBarrierCostTpl(space, function,
+                               VectorXs::Constant(function->nr, weight),
+                               threshold) {}
+
+template <typename Scalar>
+void RelaxedLogBarrierCostTpl<Scalar>::evaluate(const ConstVectorRef &x,
+                                                const ConstVectorRef &u,
+                                                CostDataAbstract &data) const {
+  Data &d = static_cast<Data &>(data);
+  residual_->evaluate(x, u, *d.residual_data);
+  d.value_ = 0.;
+  const int nrows = residual_->nr;
+  for (size_t i = 0; i < nrows; i++) {
+    if (d.residual_data->value_[i] < threshold_) {
+      Scalar sq = (d.residual_data->value_[i] - 2 * threshold_) / threshold_;
+      d.value_ += barrier_weights_(i) * (0.5 * (sq * sq - 1) - log(threshold_));
+    } else {
+      d.value_ -= barrier_weights_(i) * log(d.residual_data->value_[i]);
+    }
+  }
+}
+
+template <typename Scalar>
+void RelaxedLogBarrierCostTpl<Scalar>::computeGradients(
+    const ConstVectorRef &x, const ConstVectorRef &u,
+    CostDataAbstract &data) const {
+  Data &d = static_cast<Data &>(data);
+  StageFunctionDataTpl<Scalar> &res_data = *d.residual_data;
+  MatrixRef J = res_data.jac_buffer_.leftCols(data.grad_.size());
+  residual_->computeJacobians(x, u, res_data);
+  d.grad_.setZero();
+  VectorXs &v = res_data.value_;
+  const int nrows = residual_->nr;
+  for (int i = 0; i < nrows; i++) {
+    auto g_i = J.row(i);
+    if (v(i) < threshold_) {
+      d.grad_.noalias() += barrier_weights_(i) * g_i * (v(i) - 2 * threshold_) /
+                           (threshold_ * threshold_);
+    } else {
+      d.grad_.noalias() -= barrier_weights_(i) * g_i / v(i);
+    }
+  }
+}
+
+template <typename Scalar>
+void RelaxedLogBarrierCostTpl<Scalar>::computeHessians(
+    const ConstVectorRef &, const ConstVectorRef &,
+    CostDataAbstract &data) const {
+  Data &d = static_cast<Data &>(data);
+  StageFunctionDataTpl<Scalar> &res_data = *d.residual_data;
+  const Eigen::Index size = data.grad_.size();
+  d.hess_.setZero();
+  MatrixRef J = res_data.jac_buffer_.leftCols(size);
+  VectorXs &v = res_data.value_;
+  const int nrows = residual_->nr;
+  for (int i = 0; i < nrows; i++) {
+    auto g_i = J.row(i); // row vector
+    if (v(i) < threshold_) {
+      Scalar sq = (v(i) - 2 * threshold_) / (threshold_ * threshold_);
+      d.hess_.noalias() += barrier_weights_(i) * barrier_weights_(i) *
+                           (g_i.transpose() * g_i) * sq * sq;
+    } else {
+      d.hess_.noalias() += barrier_weights_(i) * barrier_weights_(i) *
+                           (g_i.transpose() * g_i) / (v(i) * v(i));
+    }
+  }
+}
+} // namespace aligator