Merge branch 'devel' into topic/gar-optimizations

CHANGELOG.md

@@ -27,9 +27,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Add `LqrProblemTpl::isApprox()`
 - 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))
+- CMake: Link to pinocchio::pinocchio_collision target
 
 ### Fixed
 
+- Fixed copy of TrajOptProblem ([#265](https://github.com/Simple-Robotics/aligator/pull/265))
 - `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))
 
@@ -39,6 +41,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Remove subdirectory `aligator/helpers` from include dir
 - Remove function `allocate_shared_eigen_aligned()`
 
+### 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

@@ -394,7 +394,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
+    )
     target_compile_definitions(${PROJECT_NAME} PUBLIC ALIGATOR_WITH_PINOCCHIO)
     if(PINOCCHIO_V3)
       target_compile_definitions(${PROJECT_NAME} PUBLIC ALIGATOR_PINOCCHIO_V3)

README.md

@@ -133,6 +133,7 @@ Please also consider citing the reference paper for the ProxDDP algorithm:
 * [Fabian Schramm](https://github.com/fabinsch) (Inria): core developer
 * [Ludovic De Matteïs](https://github.com/LudovicDeMatteis) (LAAS-CNRS/Inria): feature developer
 * [Ewen Dantec](https://edantec.github.io/) (Inria): feature developer
+* [Antoine Bussy](https://github.com/antoine-bussy) (Aldebaran)
 
 ## Acknowledgments
 

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/core/traj-opt-problem.hpp

@@ -129,7 +129,10 @@ template <typename _Scalar> struct TrajOptProblemTpl {
                     xyz::polymorphic<CostAbstract> term_cost);
   /// @}
 
-  bool initCondIsStateError() const { return init_state_error_ != nullptr; }
+  bool initCondIsStateError() const {
+    assert(init_cond_is_state_error_ == checkInitCondIsStateError());
+    return init_cond_is_state_error_;
+  }
 
   /// @brief Add a stage to the control problem.
   void addStage(const xyz::polymorphic<StageModel> &stage);
@@ -140,7 +143,7 @@ template <typename _Scalar> struct TrajOptProblemTpl {
       ALIGATOR_RUNTIME_ERROR(
           "Initial condition is not a StateErrorResidual.\n");
     }
-    return init_state_error_->target_;
+    return static_cast<StateErrorResidual const *>(&*init_constraint_)->target_;
   }
 
   /// @brief Set initial state constraint.
@@ -149,7 +152,7 @@ template <typename _Scalar> struct TrajOptProblemTpl {
       ALIGATOR_RUNTIME_ERROR(
           "Initial condition is not a StateErrorResidual.\n");
     }
-    init_state_error_->target_ = x0;
+    static_cast<StateErrorResidual *>(&*init_constraint_)->target_ = x0;
   }
 
   /// @brief Add a terminal constraint for the model.
@@ -189,9 +192,11 @@ template <typename _Scalar> struct TrajOptProblemTpl {
 
   bool checkIntegrity() const;
 
-private:
-  /// Pointer to underlying state error residual
-  StateErrorResidual *init_state_error_;
+protected:
+  // Check if the initial state is a StateErrorResidual.
+  // Since this is a costly operation (dynamic_cast), we cache the result.
+  bool checkInitCondIsStateError() const;
+  bool init_cond_is_state_error_ = false;
 };
 
 namespace internal {

include/aligator/core/traj-opt-problem.hxx

@@ -17,9 +17,9 @@ TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
     const std::vector<xyz::polymorphic<StageModel>> &stages,
     xyz::polymorphic<CostAbstract> term_cost)
     : init_constraint_(std::move(init_constraint)), stages_(stages),
-      term_cost_(std::move(term_cost)), unone_(term_cost_->nu) {
+      term_cost_(std::move(term_cost)), unone_(term_cost_->nu),
+      init_cond_is_state_error_(checkInitCondIsStateError()) {
   unone_.setZero();
-  init_state_error_ = dynamic_cast<StateErrorResidual *>(&*init_constraint_);
 }
 
 template <typename Scalar>
@@ -29,9 +29,7 @@ TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
     xyz::polymorphic<CostAbstract> term_cost)
     : TrajOptProblemTpl(
           StateErrorResidual(stages[0]->xspace_, stages[0]->nu(), x0), stages,
-          std::move(term_cost)) {
-  init_state_error_ = static_cast<StateErrorResidual *>(&*init_constraint_);
-}
+          std::move(term_cost)) {}
 
 template <typename Scalar>
 TrajOptProblemTpl<Scalar>::TrajOptProblemTpl(
@@ -131,6 +129,9 @@ template <typename Scalar>
 bool TrajOptProblemTpl<Scalar>::checkIntegrity() const {
   bool ok = true;
 
+  if (init_cond_is_state_error_ != checkInitCondIsStateError())
+    return false;
+
   if (numSteps() == 0)
     return true;
 
@@ -173,4 +174,11 @@ void TrajOptProblemTpl<Scalar>::replaceStageCircular(
   stages_.pop_back();
 }
 
+template <typename Scalar>
+bool TrajOptProblemTpl<Scalar>::checkInitCondIsStateError() const {
+  if (init_constraint_.valueless_after_move())
+    return false;
+  return dynamic_cast<StateErrorResidual const *>(&*init_constraint_);
+}
+
 } // namespace aligator

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"