Second order vel-damper support

Author: mathieu-celerier

include/mc_rbdyn/Collision.h

@@ -28,9 +28,11 @@ struct MC_RBDYN_DLLAPI Collision
             const std::optional<std::vector<std::string>> & r1Joints = {},
             const std::optional<std::vector<std::string>> & r2Joints = {},
             bool r1JointsInactive = false,
-            bool r2JointsInactive = false)
+            bool r2JointsInactive = false,
+            double m = 0.0,
+            double lambda = 0.0)
   : body1(b1), body2(b2), iDist(i), sDist(s), damping(d), r1Joints(r1Joints), r2Joints(r2Joints),
-    r1JointsInactive(r1JointsInactive), r2JointsInactive(r2JointsInactive)
+    r1JointsInactive(r1JointsInactive), r2JointsInactive(r2JointsInactive), overDamping(m), lambda(lambda)
   {
   }
   std::string body1; /** First body in the constraint */
@@ -50,6 +52,8 @@ struct MC_RBDYN_DLLAPI Collision
       r2Joints; /** Active/Inactive joints in the second robot, ignored if r1 == r2 */
   bool r1JointsInactive = false; /** When true the selected joints in r1ActiveJoints are considered inactive */
   bool r2JointsInactive = false; /** When true the selected joints in r2ActiveJoints are considered inactive */
+  double overDamping; /** Damping ratio **/
+  double lambda; /** Lambda parameter **/
   inline bool isNone() { return body1 == "NONE" && body2 == "NONE"; }
 
   bool operator==(const Collision & rhs) const;

include/mc_solver/CollisionsConstraint.h

@@ -83,6 +83,10 @@ struct MC_SOLVER_DLLAPI CollisionsConstraint : public ConstraintSet
    */
   void addCollisions(QPSolver & solver, const std::vector<mc_rbdyn::Collision> & cols);
 
+  void editCollisions(QPSolver & solver, const std::vector<mc_rbdyn::Collision> & cols);
+
+  void setCollisionsDampers(QPSolver & solver, const std::vector<double> & dampers);
+
   /** Returns true if the given collision is in this constraint */
   bool hasCollision(const std::string & c1, const std::string & c2) const noexcept;
 
@@ -100,6 +104,8 @@ struct MC_SOLVER_DLLAPI CollisionsConstraint : public ConstraintSet
    */
   inline void automaticMonitor(bool a) noexcept { autoMonitor_ = a; }
 
+  inline void logCollisions(bool a) noexcept { logCollisions_ = a; }
+
   void addToSolverImpl(QPSolver & solver) override;
 
   void update(QPSolver & solver) override;
@@ -132,14 +138,18 @@ struct MC_SOLVER_DLLAPI CollisionsConstraint : public ConstraintSet
   std::pair<int, mc_rbdyn::Collision> __popCollId(const std::string & name1, const std::string & name2);
   /** Actually adds the collision to the constraint, handles id creation and wildcard support */
   void __addCollision(mc_solver::QPSolver & solver, const mc_rbdyn::Collision & col);
+  void __editCollision(mc_solver::QPSolver & solver, const mc_rbdyn::Collision & col);
 
   /* Internal management for collision display */
   bool autoMonitor_ = true;
+  bool logCollisions_ = true;
   std::unordered_set<int> monitored_;
   std::shared_ptr<mc_rtc::gui::StateBuilder> gui_;
+  std::shared_ptr<mc_rtc::Logger> logger_;
   std::vector<std::string> category_;
   void addMonitorButton(int collId, const mc_rbdyn::Collision & col);
   void toggleCollisionMonitor(int collId, const mc_rbdyn::Collision * col = nullptr);
+  void addLogs(int collId, const mc_rbdyn::Collision & col);
 };
 
 } // namespace mc_solver

include/mc_solver/DynamicsConstraint.h

@@ -61,6 +61,29 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
                      bool infTorque = false,
                      bool compensateExtTorques = false);
 
+  /** Constructor
+   * Builds a damped joint limits constraint and a motion constr depending on
+   * the nature of the robot
+   * See tasks::qp::MotionConstr for details on the latter one
+   * \param robots The robots including the robot affected by this constraint
+   * \param robotIndex The index of the robot affected by this constraint
+   * \param damperSecond Value of the damper {interaction distance, safety distance,
+   * offset, damping ratio, lambda}
+   * \param velocityPercent Maximum joint velocity percentage, 0.5 is advised
+   * \param infTorque If true, ignore the torque limits set in the robot model
+   * \param compensateExtTorques If true, compensates external disturbances using a feedworward torque signal. The
+   * constraint will search for the compensation value in robot by calling `compensationTorques()` method, if not an
+   * estimation of external torques acting on the robot will be used by calling `externalTorques()` method.
+   */
+  DynamicsConstraint(const mc_rbdyn::Robots & robots,
+                     unsigned int robotIndex,
+                     double timeStep,
+                     const std::array<double, 3> & damper,
+                     const std::array<double, 2> & damperSecond,
+                     double velocityPercent = 1.0,
+                     bool infTorque = false,
+                     bool compensateExternalForces = false);
+
   /** Returns the tasks::qp::MotionConstr
    *
    * This assumes the backend was Tasks

include/mc_solver/KinematicsConstraint.h

@@ -45,6 +45,23 @@ struct MC_SOLVER_DLLAPI KinematicsConstraint : public ConstraintSet
                        const std::array<double, 3> & damper,
                        double velocityPercent = 0.5);
 
+  /** Damped constraint constructor
+   * Builds a damped joint limits constraint
+   * See tasks::qp::DamperJointLimitsConstr documentation for detail
+   * \param robots The robots including the robot affected by this constraint
+   * \param robotIndex The index of the robot affected by this constraint
+   * \param timeStep Solver timestep
+   * \param damper Value of the damper {interaction distance, safety distance, offset}
+   * \param damperSecond Value of the damper {damping ratio, lambda}
+   * \param velocityPercent Maximum joint velocity percentage, 0.5 is advised
+   */
+  KinematicsConstraint(const mc_rbdyn::Robots & robots,
+                       unsigned int robotIndex,
+                       double timeStep,
+                       const std::array<double, 3> & damper,
+                       const std::array<double, 2> & damperSecond,
+                       double velocityPercent = 0.5);
+
 protected:
   /** Implementation of mc_solver::ConstraintSet::addToSolver */
   void addToSolverImpl(mc_solver::QPSolver & solver) override;

src/mc_solver/CollisionsConstraint.cpp

@@ -19,6 +19,10 @@
 
 #include <Tasks/QPConstr.h>
 
+#include "mc_rbdyn/Collision.h"
+#include "mc_rtc/log/Logger.h"
+#include "mc_solver/QPSolver.h"
+#include <algorithm>
 #include <tvm/task_dynamics/VelocityDamper.h>
 
 #include "utils/jointsToSelector.h"
@@ -113,12 +117,16 @@ struct TVMCollisionConstraint
   void addCollision(TVMQPSolver & solver, CollisionData & data)
   {
     const auto & col = data.collision;
-    data.task = solver.problem().add(
-        data.function >= 0.,
-        tvm::task_dynamics::VelocityDamper(
-            solver.dt(), {col.iDist, col.sDist, col.damping, mc_solver::CollisionsConstraint::defaultDampingOffset},
-            tvm::constant::big_number),
-        {tvm::requirements::PriorityLevel(0)});
+    mc_rtc::log::info("[CollisionsConstraint] Adding collision constraint with xsi={}, xsioff={}, m={}, lambda={}",
+                      col.damping, mc_solver::CollisionsConstraint::defaultDampingOffset, col.overDamping, col.lambda);
+    data.task =
+        solver.problem().add(data.function >= 0.,
+                             tvm::task_dynamics::VelocityDamper(solver.dt(),
+                                                                {col.iDist, col.sDist, col.damping,
+                                                                 mc_solver::CollisionsConstraint::defaultDampingOffset,
+                                                                 col.overDamping, col.lambda},
+                                                                tvm::constant::big_number),
+                             {tvm::requirements::PriorityLevel(0)});
   }
 };
 
@@ -368,6 +376,54 @@ void CollisionsConstraint::__addCollision(mc_solver::QPSolver & solver, const mc
       break;
   }
   addMonitorButton(collId, col);
+  if(logCollisions_) addLogs(collId, col);
+}
+
+void CollisionsConstraint::__editCollision(mc_solver::QPSolver & solver, const mc_rbdyn::Collision & col)
+{
+  // Check for the correct backend
+  if(backend_ != QPSolver::Backend::TVM)
+  {
+    mc_rtc::log::error("[CollisionsConstraint] Editing collisions is only supported for TVM backend currently.");
+    return;
+  }
+
+  // Find the collision in the existing set of constraints
+  auto collIdIt = collIdDict.find(__keyByNames(col.body1, col.body2));
+  if(collIdIt == collIdDict.end())
+  {
+    mc_rtc::log::error("[CollisionsConstraint] Attempting to edit a non-existent collision: {} - {}", col.body1,
+                       col.body2);
+    return;
+  }
+
+  // Get the current collision data
+  int collId = collIdIt->second.first;
+
+  // Remove from collision vector
+  // cols.erase(std::find(cols.begin(), cols.end(), p.second));
+
+  if(backend_ == QPSolver::Backend::TVM)
+  {
+    // Remove the existing collision
+    tvm_constraint(constraint_)->deleteCollision(tvm_solver(solver), col);
+  }
+  else if(backend_ == QPSolver::Backend::Tasks)
+  {
+    // Remove the existing collision
+    auto collConstr = tasks_constraint(constraint_);
+    auto & qpsolver = tasks_solver(solver);
+    bool ret = collConstr->rmCollision(collId);
+    if(ret)
+    {
+      collConstr->updateNrVars({}, qpsolver.data());
+      qpsolver.updateConstrSize();
+    }
+  }
+
+  addCollision(solver, {col});
+
+  mc_rtc::log::info("[CollisionsConstraint] Edited collision: {} - {}", col.body1, col.body2);
 }
 
 void CollisionsConstraint::addMonitorButton(int collId, const mc_rbdyn::Collision & col)
@@ -384,6 +440,45 @@ void CollisionsConstraint::addMonitorButton(int collId, const mc_rbdyn::Collisio
   }
 }
 
+void CollisionsConstraint::addLogs(int collId, const mc_rbdyn::Collision & col)
+{
+  if(logger_ && inSolver_)
+  {
+    auto & logger = *logger_;
+    std::string label = col.body1 + "::" + col.body2;
+    auto addLogger = [&](auto && distance_callback, auto && di_callback, auto && ds_callback)
+    {
+      logger.addLogEntry("CollisionMonitor_" + label + "_distance",
+                         [distance_callback]() { return distance_callback(); });
+      logger.addLogEntry("CollisionMonitor_" + label + "_di", [di_callback]() { return di_callback(); });
+      logger.addLogEntry("CollisionMonitor_" + label + "_ds", [ds_callback]() { return ds_callback(); });
+    };
+    // Add the monitor
+    switch(backend_)
+    {
+      case QPSolver::Backend::Tasks:
+      {
+        auto collConstr = tasks_constraint(constraint_);
+        addLogger([collConstr, collId]() { return collConstr->getCollisionData(collId).distance; },
+                  [collConstr, collId]() { return collConstr->getCollisionData(collId).di; },
+                  [collConstr, collId]() { return collConstr->getCollisionData(collId).ds; });
+        break;
+      }
+      case QPSolver::Backend::TVM:
+      {
+        auto collConstr = tvm_constraint(constraint_);
+        auto fn = collConstr->getData(collId)->function;
+        addLogger([fn]() { return fn->distance(); },
+                  [collConstr, collId]() { return collConstr->getData(collId)->collision.iDist; },
+                  [collConstr, collId]() { return collConstr->getData(collId)->collision.sDist; });
+        break;
+      }
+      default:
+        break;
+    }
+  }
+}
+
 void CollisionsConstraint::toggleCollisionMonitor(int collId, const mc_rbdyn::Collision * col_p)
 {
   auto findCollisionById = [this, collId, &col_p]()
@@ -474,9 +569,36 @@ void CollisionsConstraint::addCollisions(QPSolver & solver, const std::vector<mc
   }
 }
 
+void CollisionsConstraint::editCollisions(QPSolver & solver, const std::vector<mc_rbdyn::Collision> & cols)
+{
+  // Check if the backend is TVM, as editing collisions is only supported for TVM
+  if(backend_ != QPSolver::Backend::TVM)
+  {
+    mc_rtc::log::error("[CollisionsConstraint] Editing collisions is only supported for TVM backend currently.");
+    return;
+  }
+  // Clear the existing collisions
+  // tvm_constraint(constraint_)->clear();
+  // Iterate over the list of collisions and call __editCollision for each
+  for(const auto & col : cols) { __editCollision(solver, col); }
+
+  mc_rtc::log::info("[CollisionsConstraint] Successfully edited {} collisions.", cols.size());
+}
+
+void CollisionsConstraint::setCollisionsDampers(QPSolver & solver, const std::vector<double> & dampers)
+{
+  for(auto & col : cols)
+  {
+    col.overDamping = dampers[0];
+    col.lambda = dampers[1];
+  }
+  editCollisions(solver, cols);
+}
+
 void CollisionsConstraint::addToSolverImpl(QPSolver & solver)
 {
   gui_ = solver.gui();
+  logger_ = solver.logger();
   const mc_rbdyn::Robot & r1 = solver.robots().robot(r1Index);
   const mc_rbdyn::Robot & r2 = solver.robots().robot(r2Index);
   category_ = {"Collisions", r1.name() + "/" + r2.name()};
@@ -500,7 +622,11 @@ void CollisionsConstraint::addToSolverImpl(QPSolver & solver)
     default:
       break;
   }
-  for(const auto & cols : collIdDict) { addMonitorButton(cols.second.first, cols.second.second); }
+  for(const auto & cols : collIdDict)
+  {
+    addMonitorButton(cols.second.first, cols.second.second);
+    if(logCollisions_) { addLogs(cols.second.first, cols.second.second); };
+  }
 }
 
 void CollisionsConstraint::update(QPSolver &)

src/mc_solver/DynamicsConstraint.cpp

@@ -147,6 +147,20 @@ DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots,
 {
 }
 
+DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots,
+                                       unsigned int robotIndex,
+                                       double timeStep,
+                                       const std::array<double, 3> & damper,
+                                       const std::array<double, 2> & damperSecond,
+                                       double velocityPercent,
+                                       bool infTorque,
+                                       bool compensateExtTorques)
+: KinematicsConstraint(robots, robotIndex, timeStep, damper, damperSecond, velocityPercent),
+  motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExtTorques)),
+  robotIndex_(robotIndex)
+{
+}
+
 void DynamicsConstraint::addToSolverImpl(QPSolver & solver)
 {
   KinematicsConstraint::addToSolverImpl(solver);