Merge pull request #381 from gergondet/topic/UpdateCollisionConstraint

binding/python/include/mc_control_fsm_wrapper.hpp

@@ -24,10 +24,10 @@ template<typename T, typename U>
 controller_cb make_controller_cb(T python_trampoline, U cb)
 {
   return [python_trampoline, cb](mc_control::fsm::Controller & ctl) { return python_trampoline(cb, ctl); };
-};
+}
 
 template<typename T, typename U>
 run_cb make_run_cb(T python_trampoline, U cb)
 {
   return [python_trampoline, cb](mc_control::fsm::Controller & ctl) { return python_trampoline(cb, ctl); };
-};
+}

binding/python/include/mc_rtc_wrapper.hpp

@@ -20,7 +20,7 @@ template<typename retT, typename T, typename U>
 std::function<retT()> make_log_callback(T fn, U arg)
 {
   return [fn, arg] { return fn(arg); };
-};
+}
 
 #define MAKE_LOG_HELPER(NAME, TYPE)          \
   template<typename T, typename U>           \
@@ -64,12 +64,12 @@ mc_rtc::Configuration get_as_config(const T & v)
   return conf("v");
 }
 
-mc_rtc::Configuration ConfigurationFromData(const std::string & data)
+inline mc_rtc::Configuration ConfigurationFromData(const std::string & data)
 {
   return mc_rtc::Configuration::fromData(data);
 }
 
-void set_loader_debug_suffix(const std::string & suffix)
+inline void set_loader_debug_suffix(const std::string & suffix)
 {
   mc_rtc::Loader::debug_suffix = suffix;
 }

doc/_data/schemas/ConstraintSet/CollisionsConstraint.json

@@ -6,6 +6,12 @@
     "type": { "enum": ["collision"] },
     "r1": { "type": "string", "default": "MainRobot", "description": "Name of the first robot involved in the collision" },
     "r2": { "type": "string", "default": "MainRobot", "description": "Name of the second robot involved in the collision" },
+    "automaticMonitor":
+    {
+      "type": "boolean",
+      "default": true,
+      "description": "If true automatically display collision monitors as constraints get activated. Otherwise those monitors are managed manually"
+    },
     "useCommon":
     {
       "type": "boolean",

include/mc_solver/CollisionsConstraint.h

@@ -89,8 +89,21 @@ struct MC_SOLVER_DLLAPI CollisionsConstraint : public ConstraintSet
   /** Remove all collisions from the constraint */
   void reset();
 
+  /** Get the automated monitoring setting */
+  inline bool automaticMonitor() const noexcept { return autoMonitor_; }
+
+  /** Set the automated monitoring setting
+   *
+   * If true collisions, monitors are automatically added/removed depending on the collision activation
+   *
+   * If false, monitors are managed by the user
+   */
+  inline void automaticMonitor(bool a) noexcept { autoMonitor_ = a; }
+
   void addToSolverImpl(QPSolver & solver) override;
 
+  void update(QPSolver & solver) override;
+
   void removeFromSolverImpl(QPSolver & solver) override;
 
 public:
@@ -121,11 +134,12 @@ struct MC_SOLVER_DLLAPI CollisionsConstraint : public ConstraintSet
   void __addCollision(mc_solver::QPSolver & solver, const mc_rbdyn::Collision & col);
 
   /* Internal management for collision display */
+  bool autoMonitor_ = true;
   std::unordered_set<int> monitored_;
   std::shared_ptr<mc_rtc::gui::StateBuilder> gui_;
   std::vector<std::string> category_;
   void addMonitorButton(int collId, const mc_rbdyn::Collision & col);
-  void toggleCollisionMonitor(int collId);
+  void toggleCollisionMonitor(int collId, const mc_rbdyn::Collision * col = nullptr);
 };
 
 } // namespace mc_solver

include/mc_solver/ConstraintSet.h

@@ -25,6 +25,14 @@ struct MC_SOLVER_DLLAPI ConstraintSet
   /** This is called by \ref mc_solver::QPSolver when the constraint is added to the problem */
   void addToSolver(mc_solver::QPSolver & solver);
 
+  /** \brief Update the constraint
+   *
+   * This is called at every iteration of the controller once the constraint has been added to a solver
+   *
+   * \param solver Solver in which the constraint has been inserted
+   */
+  virtual void update(QPSolver &) {}
+
   /** This is called by \ref mc_solver::QPSolver when the constraint is removed from the problem */
   void removeFromSolver(mc_solver::QPSolver & solver);
 

include/mc_solver/QPSolver.h

@@ -218,10 +218,13 @@ struct MC_SOLVER_DLLAPI QPSolver
   virtual void setContacts(ControllerToken, const std::vector<mc_rbdyn::Contact> & contacts) = 0;
 
   /** Returns the current set of contacts */
-  const std::vector<mc_rbdyn::Contact> & contacts() const;
+  inline const std::vector<mc_rbdyn::Contact> & contacts() const noexcept { return contacts_; }
+
+  /** Returns the current set of constraints */
+  inline const std::vector<mc_solver::ConstraintSet *> & constraints() const noexcept { return constraints_; }
 
   /** Returns the MetaTasks currently in the solver */
-  const std::vector<mc_tasks::MetaTask *> & tasks() const;
+  inline const std::vector<mc_tasks::MetaTask *> & tasks() const noexcept { return metaTasks_; }
 
   /** Desired resultant of contact force in robot surface frame
    * \param contact Contact for which the force is desired.
@@ -308,6 +311,9 @@ struct MC_SOLVER_DLLAPI QPSolver
   /** Storage for shared_pointer on tasks */
   std::vector<std::shared_ptr<void>> shPtrTasksStorage;
 
+  /** Holds ConstraintSet currently in the solver */
+  std::vector<mc_solver::ConstraintSet *> constraints_;
+
   /** Pointer to the Logger */
   std::shared_ptr<mc_rtc::Logger> logger_ = nullptr;
 

src/mc_solver/CollisionsConstraint.cpp

@@ -183,7 +183,7 @@ bool CollisionsConstraint::removeCollision(QPSolver & solver, const std::string
   auto p = __popCollId(b1Name, b2Name);
   if(!p.second.isNone())
   {
-    if(monitored_.count(p.first)) { toggleCollisionMonitor(p.first); }
+    if(monitored_.count(p.first)) { toggleCollisionMonitor(p.first, &p.second); }
     category_.push_back("Monitors");
     std::string name = "Monitor " + p.second.body1 + "/" + p.second.body2;
     gui_->removeElement(category_, name);
@@ -245,7 +245,7 @@ bool CollisionsConstraint::removeCollisionByBody(QPSolver & solver,
         default:
           break;
       }
-      if(monitored_.count(out.first)) { toggleCollisionMonitor(out.first); }
+      if(monitored_.count(out.first)) { toggleCollisionMonitor(out.first, &out.second); }
       category_.push_back("Monitors");
       std::string name = "Monitor " + out.second.body1 + "/" + out.second.body2;
       gui_->removeElement(category_, name);
@@ -336,15 +336,7 @@ void CollisionsConstraint::__addCollision(mc_solver::QPSolver & solver, const mc
     default:
       break;
   }
-  if(solver.gui())
-  {
-    if(!gui_)
-    {
-      gui_ = solver.gui();
-      category_ = {"Collisions", r1.name() + "/" + r2.name()};
-    }
-    addMonitorButton(collId, col);
-  }
+  addMonitorButton(collId, col);
 }
 
 void CollisionsConstraint::addMonitorButton(int collId, const mc_rbdyn::Collision & col)
@@ -361,17 +353,19 @@ void CollisionsConstraint::addMonitorButton(int collId, const mc_rbdyn::Collisio
   }
 }
 
-void CollisionsConstraint::toggleCollisionMonitor(int collId)
+void CollisionsConstraint::toggleCollisionMonitor(int collId, const mc_rbdyn::Collision * col_p)
 {
-  auto findCollisionById = [this, collId]() -> const mc_rbdyn::Collision &
+  auto findCollisionById = [this, collId, &col_p]()
   {
+    if(col_p) { return; }
     for(const auto & c : collIdDict)
     {
-      if(c.second.first == collId) { return c.second.second; }
+      if(c.second.first == collId) { col_p = &c.second.second; }
     }
     mc_rtc::log::error_and_throw("[CollisionConstraint] Attempted to toggleCollisionMonitor on non-existent collision");
   };
-  const auto & col = findCollisionById();
+  findCollisionById();
+  const auto & col = *col_p;
   auto & gui = *gui_;
   std::string label = col.body1 + "::" + col.body2;
   if(monitored_.count(collId))
@@ -388,9 +382,10 @@ void CollisionsConstraint::toggleCollisionMonitor(int collId)
     auto addMonitor = [&](auto && distance_callback, auto && p1_callback, auto && p2_callback)
     {
       gui.addElement(category_, mc_rtc::gui::Label(label, [distance_callback]()
-                                                   { return fmt::format("{:0.2f} cm", distance_callback()); }));
+                                                   { return fmt::format("{:0.2f} cm", 100.0 * distance_callback()); }));
       category_.push_back("Arrows");
       gui.addElement(category_, mc_rtc::gui::Arrow(label, p1_callback, p2_callback));
+      category_.pop_back();
     };
     // Add the monitor
     switch(backend_)
@@ -399,10 +394,9 @@ void CollisionsConstraint::toggleCollisionMonitor(int collId)
       {
         auto collConstr = tasks_constraint(constraint_);
         addMonitor(
-            [collConstr, collId]() { return collConstr->getCollisionData(collId).distance * 100; },
+            [collConstr, collId]() { return collConstr->getCollisionData(collId).distance; },
             [collConstr, collId]() -> const Eigen::Vector3d & { return collConstr->getCollisionData(collId).p1; },
             [collConstr, collId]() -> const Eigen::Vector3d & { return collConstr->getCollisionData(collId).p2; });
-        category_.pop_back();
         break;
       }
       case QPSolver::Backend::TVM:
@@ -447,6 +441,11 @@ void CollisionsConstraint::addCollisions(QPSolver & solver, const std::vector<mc
 
 void CollisionsConstraint::addToSolverImpl(QPSolver & solver)
 {
+  gui_ = solver.gui();
+  const mc_rbdyn::Robot & r1 = solver.robots().robot(r1Index);
+  const mc_rbdyn::Robot & r2 = solver.robots().robot(r2Index);
+  category_ = {"Collisions", r1.name() + "/" + r2.name()};
+  gui_->addElement(category_, mc_rtc::gui::Checkbox("Automatic monitor", autoMonitor_));
   switch(backend_)
   {
     case QPSolver::Backend::Tasks:
@@ -469,6 +468,37 @@ void CollisionsConstraint::addToSolverImpl(QPSolver & solver)
   for(const auto & cols : collIdDict) { addMonitorButton(cols.second.first, cols.second.second); }
 }
 
+void CollisionsConstraint::update(QPSolver &)
+{
+  if(!autoMonitor_) { return; }
+  auto getDistance = [this](int collId)
+  {
+    switch(backend_)
+    {
+      case QPSolver::Backend::Tasks:
+      {
+        auto collConstr = tasks_constraint(constraint_);
+        return collConstr->getCollisionData(collId).distance;
+      }
+      case QPSolver::Backend::TVM:
+      {
+        auto collConstr = tvm_constraint(constraint_);
+        auto & fn = collConstr->getData(collId)->function;
+        return fn->distance();
+      }
+      default:
+        mc_rtc::log::error_and_throw("Not implemented for this backend");
+    }
+  };
+  for(const auto & [name, info] : collIdDict)
+  {
+    const auto & [collId, coll] = info;
+    auto distance = getDistance(collId);
+    if(distance < coll.iDist && !monitored_.count(collId)) { toggleCollisionMonitor(collId, &coll); }
+    if(distance > coll.iDist && monitored_.count(collId)) { toggleCollisionMonitor(collId, &coll); }
+  }
+}
+
 void CollisionsConstraint::removeFromSolverImpl(QPSolver & solver)
 {
   switch(backend_)
@@ -557,6 +587,7 @@ static auto registered = mc_solver::ConstraintSetLoader::register_load_function(
       auto ret = std::make_shared<mc_solver::CollisionsConstraint>(
           solver.robots(), robotIndexFromConfig(config, solver.robots(), "collision", false, "r1Index", "r1", ""),
           robotIndexFromConfig(config, solver.robots(), "collision", false, "r2Index", "r2", ""), solver.dt());
+      ret->automaticMonitor(config("automaticMonitor", true));
       if(ret->r1Index == ret->r2Index)
       {
         if(config("useCommon", false))

src/mc_solver/QPSolver.cpp

@@ -53,6 +53,9 @@ void QPSolver::addConstraintSet(ConstraintSet & cs)
         "[QPSolver::addConstraintSet] Constraint backend ({}) is different from this solver backend ({})", cs.backend(),
         backend_);
   }
+  auto it = std::find(constraints_.begin(), constraints_.end(), &cs);
+  if(it != constraints_.end()) { return; }
+  constraints_.push_back(&cs);
   cs.addToSolver(*this);
   if(dynamic_cast<DynamicsConstraint *>(&cs) != nullptr)
   {
@@ -68,6 +71,9 @@ void QPSolver::removeConstraintSet(ConstraintSet & cs)
         "[QPSolver::removeConstraintSet] Constraint backend ({}) is different from this solver backend ({})",
         cs.backend(), backend_);
   }
+  auto it = std::find(constraints_.begin(), constraints_.end(), &cs);
+  if(it == constraints_.end()) { return; }
+  constraints_.erase(it);
   cs.removeFromSolver(*this);
   removeDynamicsConstraint(&cs);
 }
@@ -113,16 +119,6 @@ void QPSolver::removeTask(mc_tasks::MetaTask * task)
   }
 }
 
-const std::vector<mc_rbdyn::Contact> & QPSolver::contacts() const
-{
-  return contacts_;
-}
-
-const std::vector<mc_tasks::MetaTask *> & QPSolver::tasks() const
-{
-  return metaTasks_;
-}
-
 bool QPSolver::run(FeedbackType fType)
 {
   return run_impl(fType);

src/mc_solver/TVMQPSolver.cpp

@@ -112,6 +112,7 @@ bool TVMQPSolver::run_impl(FeedbackType fType)
 
 bool TVMQPSolver::runCommon()
 {
+  for(auto & c : constraints_) { c->update(*this); }
   for(auto & t : metaTasks_)
   {
     t->update(*this);

src/mc_solver/TasksQPSolver.cpp

@@ -190,6 +190,7 @@ bool TasksQPSolver::run_impl(FeedbackType fType)
 
 bool TasksQPSolver::runOpenLoop()
 {
+  for(auto & c : constraints_) { c->update(*this); }
   for(auto & t : metaTasks_)
   {
     t->update(*this);
@@ -259,6 +260,7 @@ bool TasksQPSolver::runJointsFeedback(bool wVelocity)
       robot.forwardAcceleration();
     }
   }
+  for(auto & c : constraints_) { c->update(*this); }
   for(auto & t : metaTasks_)
   {
     t->update(*this);
@@ -313,7 +315,8 @@ bool TasksQPSolver::runClosedLoop(bool integrateControlState)
     robot.forwardAcceleration();
   }
 
-  // Update tasks from estimated robots
+  // Update tasks and constraints from estimated robots
+  for(auto & c : constraints_) { c->update(*this); }
   for(auto & t : metaTasks_)
   {
     t->update(*this);

src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp

@@ -37,7 +37,7 @@ StabilizerTask::StabilizerTask(const mc_rbdyn::Robots & robots,
   extWrenchSumLowPass_(dt, /* cutoffPeriod = */ 0.05), comOffsetLowPass_(dt, /* cutoffPeriod = */ 0.05),
   comOffsetLowPassCoM_(dt, /* cutoffPeriod = */ 1.0), comOffsetDerivator_(dt, /* timeConstant = */ 1.),
   dcmIntegrator_(dt, /* timeConstant = */ 15.), dcmDerivator_(dt, /* timeConstant = */ 1.), dt_(dt),
-  fSumFilter_(dt, /* cutoffPeriod = */ 2.), mass_(robots.robot(robotIndex).mass())
+  mass_(robots.robot(robotIndex).mass()), fSumFilter_(dt, /* cutoffPeriod = */ 2.)
 {
   type_ = "lipm_stabilizer";
   name_ = type_ + "_" + robots.robot(robotIndex).name();

src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp

@@ -501,12 +501,10 @@ void StabilizerTask::addToLogger(mc_rtc::Logger & logger)
   logger.addLogEntry(name_ + "_extWrench_comOffsetErr_ZMPLimit", this,
                      [this]() { return c_.extWrench.comOffsetErrZMPLimit; });
   logger.addLogEntry(name_ + "_extWrench_comOffsetDerivator", this, [this]() { return comOffsetDerivator_.eval(); });
-  logger.addLogEntry(name_ + "_extWrench_ZMPCoefMeasured", this,
-                     [this]() -> const double & { return zmpCoefMeasured_; });
+  logger.addLogEntry(name_ + "_extWrench_ZMPCoefMeasured", this, [this]() { return zmpCoefMeasured_; });
   logger.addLogEntry(name_ + "_df_damping", this, [this]() { return c_.dfDamping; });
   logger.addLogEntry(name_ + "_forcesSum", this, [this]() -> const Eigen::Vector3d & { return fSumFilter_.eval(); });
-  logger.addLogEntry(name_ + "_forcesSum_cutOffPeriod", this,
-                     [this]() -> const double { return c_.fSumFilterTimeConstant; });
+  logger.addLogEntry(name_ + "_forcesSum_cutOffPeriod", this, [this]() { return c_.fSumFilterTimeConstant; });
   logger.addLogEntry(name_ + "_fdqp_weights_ankleTorque", this,
                      [this]() { return std::pow(c_.fdqpWeights.ankleTorqueSqrt, 2); });
   logger.addLogEntry(name_ + "_fdqp_weights_netWrench", this,
@@ -519,7 +517,7 @@ void StabilizerTask::addToLogger(mc_rtc::Logger & logger)
                      [this]() -> Eigen::Vector2d { return c_.copFzLambda.segment(0, 2); });
   logger.addLogEntry(name_ + "_fdmpc_model_cop_left", this, [this]() { return modeledCoPLeft_; });
   logger.addLogEntry(name_ + "_fdmpc_model_cop_right", this, [this]() { return modeledCoPRight_; });
-  logger.addLogEntry(name_ + "_fdmpc_model_fz_lambda", this, [this]() -> double { return c_.copFzLambda.z(); });
+  logger.addLogEntry(name_ + "_fdmpc_model_fz_lambda", this, [this]() { return c_.copFzLambda.z(); });
   logger.addLogEntry(name_ + "_fdmpc_model_fz_left", this, [this]() { return modeledFzLeft_; });
   logger.addLogEntry(name_ + "_fdmpc_model_fz_right", this, [this]() { return modeledFzRight_; });
   logger.addLogEntry(name_ + "_fdmpc_desired_fz_left", this, [this]() { return desiredFzLeft_; });

utils/mc_bin_utils.in.cpp

@@ -183,7 +183,7 @@ int show(int argc, char * argv[])
     for(size_t i = 0; i < events.size(); ++i)
     {
       if(events[i].size() == 0) { continue; }
-      std::cout << "Events at t = " << (i * dt) << ":\n";
+      std::cout << "Events at t = " << (static_cast<double>(i) * dt) << ":\n";
       for(const auto & e : events[i])
       {
         std::cout << "- category: ";