Add collision constraint to ur5_reach example + fix minor mistake in relaxedLogBarrierCost

Author: edantec

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

@@ -52,10 +52,10 @@ void exposeComposites() {
   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",
-                                      "barrier_weights", "threshold")))
+               const Scalar>(
+          bp::args("self", "space", "function", "weights", "threshold")))
       .def(bp::init<PolyManifold, PolyFunction, const Scalar, const Scalar>(
-          bp::args("self", "space", "function", "scale", "threshold")))
+          bp::args("self", "space", "function", "weights", "threshold")))
       .def_readwrite("residual", &RelaxedLogCost::residual_)
       .def_readwrite("weights", &RelaxedLogCost::barrier_weights_)
       .def(CopyableVisitor<RelaxedLogCost>())

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
 
@@ -32,6 +33,30 @@ class Args(ArgsBase):
 vizer.initViewer(open=args.display, loadModel=True)
 vizer.setBackgroundColor()
 
+frname = "universe"
+fr_id = rmodel.getFrameId(frname)
+joint_id = rmodel.frames[fr_id].parentJoint
+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(
+    "sphere2", 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 +86,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 +102,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 +138,15 @@ 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)
+    # 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 +157,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

@@ -18,15 +18,16 @@ struct RelaxedLogBarrierCostTpl : CostAbstractTpl<Scalar> {
 
   VectorXs barrier_weights_;
   xyz::polymorphic<StageFunction> residual_;
-  double threshold_;
+  Scalar threshold_;
 
   RelaxedLogBarrierCostTpl(xyz::polymorphic<Manifold> space,
                            xyz::polymorphic<StageFunction> function,
-                           const ConstVectorRef &scale, const Scalar threshold);
+                           const ConstVectorRef &weight,
+                           const Scalar threshold);
 
   RelaxedLogBarrierCostTpl(xyz::polymorphic<Manifold> space,
                            xyz::polymorphic<StageFunction> function,
-                           const Scalar scale, const Scalar threshold);
+                           const Scalar weight, const Scalar threshold);
 
   void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                 CostDataAbstract &data) const;

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

@@ -9,17 +9,17 @@ namespace aligator {
 template <typename Scalar>
 RelaxedLogBarrierCostTpl<Scalar>::RelaxedLogBarrierCostTpl(
     xyz::polymorphic<Manifold> space, xyz::polymorphic<StageFunction> function,
-    const ConstVectorRef &scale, const Scalar threshold)
-    : Base(space, function->nu), barrier_weights_(scale), residual_(function),
+    const ConstVectorRef &weight, const Scalar threshold)
+    : Base(space, function->nu), barrier_weights_(weight), residual_(function),
       threshold_(threshold) {
-  if (scale.size() != function->nr) {
+  if (weight.size() != function->nr) {
     ALIGATOR_RUNTIME_ERROR(
-        "scale argument dimension ({:d}) != function codimension ({:d})",
-        scale.size(), function->nr);
+        "weight argument dimension ({:d}) != function codimension ({:d})",
+        weight.size(), function->nr);
   }
-  bool negs = (scale.array() <= 0.0).any();
+  bool negs = (weight.array() <= 0.0).any();
   if (negs) {
-    ALIGATOR_RUNTIME_ERROR("scale coefficients must be > 0.");
+    ALIGATOR_RUNTIME_ERROR("weight coefficients must be > 0.");
   }
   if (threshold_ <= 0.) {
     ALIGATOR_RUNTIME_ERROR("threshold must be > 0.");
@@ -29,10 +29,10 @@ RelaxedLogBarrierCostTpl<Scalar>::RelaxedLogBarrierCostTpl(
 template <typename Scalar>
 RelaxedLogBarrierCostTpl<Scalar>::RelaxedLogBarrierCostTpl(
     xyz::polymorphic<Manifold> space, xyz::polymorphic<StageFunction> function,
-    const Scalar scale, const Scalar threshold)
-    : RelaxedLogBarrierCostTpl(
-          space, function, VectorXs::Constant(function->nr, scale), threshold) {
-}
+    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,
@@ -45,7 +45,7 @@ void RelaxedLogBarrierCostTpl<Scalar>::evaluate(const ConstVectorRef &x,
   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_ += 0.5 * barrier_weights_(i) * (sq * sq - 1) - log(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]);
     }
@@ -65,7 +65,7 @@ void RelaxedLogBarrierCostTpl<Scalar>::computeGradients(
   const int nrows = residual_->nr;
   for (int i = 0; i < nrows; i++) {
     auto g_i = J.row(i);
-    if (d.residual_data->value_[i] < threshold_) {
+    if (v(i) < threshold_) {
       d.grad_.noalias() += barrier_weights_(i) * g_i * (v(i) - 2 * threshold_) /
                            (threshold_ * threshold_);
     } else {
@@ -87,7 +87,7 @@ void RelaxedLogBarrierCostTpl<Scalar>::computeHessians(
   const int nrows = residual_->nr;
   for (int i = 0; i < nrows; i++) {
     auto g_i = J.row(i); // row vector
-    if (d.residual_data->value_[i] < threshold_) {
+    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;

include/aligator/modelling/multibody/frame-collision.hxx

@@ -4,6 +4,7 @@
 #include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/collision/distance.hpp>
+#include <iostream>
 
 namespace aligator {
 
@@ -60,8 +61,8 @@ void FrameCollisionResidualTpl<Scalar>::computeJacobians(const ConstVectorRef &,
   // compute the residual derivatives
   d.Jx_.setZero();
   d.Jx_.leftCols(pin_model_.nv) =
-      -d.geometry_.distanceResults[frame_pair_id_].normal.transpose() *
-      (d.Jcol_.template topRows<3>() - d.Jcol2_.template topRows<3>());
+      d.geometry_.distanceResults[frame_pair_id_].normal.transpose() *
+      (d.Jcol2_.template topRows<3>() - d.Jcol_.template topRows<3>());
 }
 
 template <typename Scalar>