Close loop handling

include/mc_control/MCController.h

@@ -17,6 +17,7 @@
 #include <mc_rtc/log/Logger.h>
 #include <mc_rtc/unique_ptr.h>
 
+#include <mc_solver/CoincidenceConstr.h>
 #include <mc_solver/CollisionsConstraint.h>
 #include <mc_solver/CompoundJointConstraint.h>
 #include <mc_solver/ContactConstraint.h>
@@ -704,6 +705,8 @@ struct MC_CONTROL_DLLAPI MCController
   mc_rtc::unique_ptr<mc_solver::CompoundJointConstraint> compoundJointConstraint;
   /** Posture task for the main robot */
   std::shared_ptr<mc_tasks::PostureTask> postureTask;
+  /** Coincidence constraints for the main robot */
+  std::vector<std::shared_ptr<mc_solver::CoincidenceConstraint>> coincidence_constraints_;
   /* Controller's name */
   const std::string name_;
   /** Stores the loading location provided by the loader via \ref set_loading_location */
@@ -734,6 +737,8 @@ struct MC_CONTROL_DLLAPI MCController
    * The value is stored in a thread_local variable and is meant to be used in the constructor of MCController
    */
   static void set_name(std::string_view name);
+
+  void loop_closing(const mc_rbdyn::Robot & robot);
 };
 
 namespace details

include/mc_rbdyn/RobotModule.h

@@ -26,6 +26,7 @@
 #include <array>
 #include <map>
 #include <vector>
+#include <yaml-cpp/yaml.h>
 
 /* This is an interface designed to provide additionnal information about a robot */
 
@@ -273,7 +274,7 @@ struct MC_RBDYN_DLLAPI RobotModule
    * - Create a default joint order
    * - Create a default stance
    */
-  void init(const rbd::parsers::ParserResult & res);
+  void init(const rbd::parsers::ParserResult & res, const YAML::Node & res_yaml = {});
 
   /** Returns the robot's bounds obtained from parsing a urdf
    *
@@ -559,6 +560,8 @@ struct MC_RBDYN_DLLAPI RobotModule
   std::string name;
   /** Path to the robot's URDF file */
   std::string urdf_path;
+  /** Path to the robot's URDF file */
+  std::string yaml_path;
   /** Path to the robot's RSDF folder */
   std::string rsdf_dir;
   /** Path to the robot's calib folder */
@@ -627,6 +630,11 @@ struct MC_RBDYN_DLLAPI RobotModule
   DevicePtrVector _devices;
   /** \see frames() */
   std::vector<FrameDescription> _frames;
+
+  // Closed loop variables
+  std::vector<std::vector<std::string>> link_names_;
+  std::vector<std::string> types_;
+  std::vector<std::vector<std::string>> motors_;
 };
 
 typedef std::shared_ptr<RobotModule> RobotModulePtr;

include/mc_solver/CoincidenceConstr.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <mc_rtc/void_ptr.h>
+#include <mc_solver/ConstraintSet.h>
+#include <Tasks/QPCoincidenceConstr.h>
+
+namespace mc_rbdyn
+{
+struct Robots;
+}
+
+namespace mc_solver
+{
+
+struct QPSolver;
+
+/** \class CoincidenceConstraint
+ *
+ * Constraint to maintain coincidence between two points on a robot
+ */
+struct MC_SOLVER_DLLAPI CoincidenceConstraint : public ConstraintSet
+{
+public:
+  /** Constructor
+   *
+   * \param robots The robots for which the constraint will apply
+   * \param name1 Name of the first link
+   * \param name1 Name of the second link
+   * \param type Type of the joint
+   * \param dt Time step of the control
+   */
+  CoincidenceConstraint(const mc_rbdyn::Robots & robots,
+                        std::string name1,
+                        std::string name2,
+                        std::string type,
+                        const Eigen::VectorXd & joints,
+                        double dt);
+
+  void addToSolverImpl(QPSolver & solver) override;
+
+  void removeFromSolverImpl(QPSolver & solver) override;
+
+public:
+  /** Holds the constraint implementation */
+  std::string name1_;
+  std::string name2_;
+  std::string type_;
+  Eigen::VectorXd joints_;
+  double dt_;
+
+  tasks::qp::CoincidenceConstr * coincidenceConstr();
+
+private:
+  mc_rtc::void_ptr constraint_;
+};
+
+} // namespace mc_solver

src/CMakeLists.txt

@@ -432,6 +432,7 @@ set(mc_solver_SRC
     mc_solver/Update.cpp
     mc_solver/UpdateNrVars.cpp
     mc_tasks/MetaTask.cpp
+    mc_solver/CoincidenceConstr.cpp
 )
 
 set(mc_solver_HDR
@@ -459,6 +460,7 @@ set(mc_solver_HDR
     ../include/mc_solver/TasksQPSolver.h
     ../include/mc_solver/TVMQPSolver.h
     ../include/mc_tasks/MetaTask.h
+    ../include/mc_solver/CoincidenceConstr.h
 )
 
 add_library(mc_solver SHARED ${mc_solver_SRC} ${mc_solver_HDR})

src/mc_control/MCController.cpp

@@ -27,6 +27,9 @@
 #include <RBDyn/FV.h>
 
 #include <boost/filesystem.hpp>
+#include "mc_solver/CoincidenceConstr.h"
+#include <cstddef>
+#include <iostream>
 namespace bfs = boost::filesystem;
 
 #include <array>
@@ -311,6 +314,10 @@ MCController::MCController(const std::vector<std::shared_ptr<mc_rbdyn::RobotModu
   {
     for(const auto & c : *contacts) { addContact(c); }
   }
+
+  /** Load the yaml file for loop closing */
+  loop_closing(robot());
+
   mc_rtc::log::info("MCController(base) ready");
 }
 
@@ -1009,4 +1016,41 @@ mc_rtc::Configuration MCController::robot_config(const std::string & robot) cons
   return result;
 }
 
+void MCController::loop_closing(const mc_rbdyn::Robot & robot)
+{
+  try
+  {
+    for(size_t n = 0; n < robot.module().types_.size(); ++n)
+    {
+
+      // creation of the joint selector matrix
+      std::vector<std::string> motors = robot.module().motors_[n];
+      Eigen::VectorXd joints_vec = Eigen::VectorXd::Zero(robot.mb().nrDof());
+      int idx = 5; // we do not considerate the fisrt 6 values of the body pos
+      for(int i = 0; i < std::size(robot.mbc().alphaD); i++)
+      {
+        if(robot.mb().joint(i).dof() >= 1)
+        {
+          auto it = std::find(motors.begin(), motors.end(), robot.mb().joint(i).name());
+          if(it != motors.end()) { joints_vec[idx] = 1.0; }
+          idx++;
+        }
+      }
+
+      auto frame_pair = robot.module().link_names_[n];
+      std::string name1 = frame_pair[0];
+      std::string name2 = frame_pair[1];
+      std::string type = robot.module().types_[n];
+      auto coincidence_constraint =
+          std::make_shared<mc_solver::CoincidenceConstraint>(robots(), name1, name2, type, joints_vec, solver().dt());
+      coincidence_constraints_.push_back(coincidence_constraint);
+      solver().addConstraintSet(*coincidence_constraint);
+    }
+  }
+  catch(const std::exception & e)
+  {
+    mc_rtc::log::error("Failed to load constraints: {}", e.what());
+  }
+}
+
 } // namespace mc_control

src/mc_rbdyn/RobotLoader.cpp

@@ -2,6 +2,8 @@
  * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
  */
 
+#include <iostream>
+
 #include <mc_rbdyn/RobotLoader.h>
 
 #include <mc_rtc/Configuration.h>

src/mc_rbdyn/RobotModule.cpp

@@ -37,7 +37,7 @@ RobotModule::RobotModule(const std::string & name, const rbd::parsers::ParserRes
   init(res);
 }
 
-void RobotModule::init(const rbd::parsers::ParserResult & res)
+void RobotModule::init(const rbd::parsers::ParserResult & res, const YAML::Node & res_yaml)
 {
   mb = res.mb;
   mbc = res.mbc;
@@ -53,6 +53,17 @@ void RobotModule::init(const rbd::parsers::ParserResult & res)
   _collision = res.collision;
   if(_ref_joint_order.size() == 0) { make_default_ref_joint_order(); }
   expand_stance();
+
+  if(res_yaml)
+  {
+    if(res_yaml["name_mot"] && res_yaml["closed_loop"] && res_yaml["type"] && !res_yaml["name_mot"].IsNull()
+       && !res_yaml["closed_loop"].IsNull() && !res_yaml["type"].IsNull())
+    {
+      link_names_ = res_yaml["closed_loop"].as<std::vector<std::vector<std::string>>>();
+      types_ = res_yaml["type"].as<std::vector<std::string>>();
+      motors_ = res_yaml["name_mot"].as<std::vector<std::vector<std::string>>>();
+    }
+  }
 }
 
 RobotModule::Gripper::Gripper(const std::string & name, const std::vector<std::string> & joints, bool reverse_limits)

src/mc_solver/CoincidenceConstr.cpp

@@ -0,0 +1,97 @@
+/*
+ * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#include <mc_solver/CoincidenceConstr.h>
+
+#include <mc_rtc/logging.h>
+
+#include <mc_solver/ConstraintSetLoader.h>
+#include <mc_solver/QPSolver.h>
+#include <mc_solver/TVMQPSolver.h>
+#include <mc_solver/TasksQPSolver.h>
+
+#include <Tasks/QPCoincidenceConstr.h>
+#include <iostream>
+
+#include <mc_rbdyn/Robots.h>
+
+namespace mc_solver
+{
+
+static mc_rtc::void_ptr make_constraint(QPSolver::Backend backend,
+                                        int robot_index,
+                                        const std::string & name1,
+                                        const std::string & name2,
+                                        const Eigen::VectorXd & joints,
+                                        const std::string & type)
+{
+  switch(backend)
+  {
+    case QPSolver::Backend::Tasks:
+    {
+      if(type == "6d")
+      {
+        return mc_rtc::make_void_ptr<tasks::qp::FixedCoincidenceConstr>(robot_index, name1, name2, joints);
+      }
+      else if(type == "3d")
+      {
+        return mc_rtc::make_void_ptr<tasks::qp::RotationalCoincidenceConstr>(robot_index, name1, name2, joints);
+      }
+      else { mc_rtc::log::error_and_throw("[CoincidenceConstraint] Invalid coincidence type given to constructor"); }
+    }
+    case QPSolver::Backend::TVM:
+      return {nullptr, nullptr};
+    default:
+      mc_rtc::log::error_and_throw("[CoincidenceConstraint] Not implemented for solver backend: {}", backend);
+  }
+}
+
+CoincidenceConstraint::CoincidenceConstraint(const mc_rbdyn::Robots & robots,
+                                             std::string name1,
+                                             std::string name2,
+                                             std::string type,
+                                             const Eigen::VectorXd & joints,
+                                             double dt)
+: ConstraintSet(), name1_(name1), name2_(name2), type_(type), joints_(joints), dt_(dt),
+  constraint_(make_constraint(backend_, robots.robotIndex(), name1_, name2_, joints_, type_))
+{
+}
+
+void CoincidenceConstraint::addToSolverImpl(QPSolver & solver)
+{
+  switch(backend_)
+  {
+    case QPSolver::Backend::Tasks:
+      static_cast<tasks::qp::CoincidenceConstr *>(constraint_.get())
+          ->addToSolver(solver.robots().mbs(), tasks_solver(solver).solver());
+      mc_rtc::log::success("CoincidenceConstraint {} <-> {} added successfully!", name1_, name2_);
+      break;
+    case QPSolver::Backend::TVM:
+      break;
+  }
+}
+
+void CoincidenceConstraint::removeFromSolverImpl(QPSolver & solver)
+{
+  switch(backend_)
+  {
+    case QPSolver::Backend::Tasks:
+      static_cast<tasks::qp::CoincidenceConstr *>(constraint_.get())->removeFromSolver(tasks_solver(solver).solver());
+      mc_rtc::log::info("CoincidenceConstraint {} <-> {} removed", name1_, name2_);
+      break;
+    case QPSolver::Backend::TVM:
+      break;
+  }
+}
+
+tasks::qp::CoincidenceConstr * CoincidenceConstraint::coincidenceConstr()
+{
+  if(backend_ != QPSolver::Backend::Tasks)
+  {
+    mc_rtc::log::error_and_throw("[CoincidenceConstraint] coincidenceConstr() is only usable for the Tasks backend");
+  }
+  return static_cast<tasks::qp::CoincidenceConstr *>(constraint_.get());
+}
+
+} // namespace mc_solver

src/mc_solver/ConstraintSet.cpp

@@ -5,6 +5,7 @@
 #include <mc_solver/ConstraintSet.h>
 
 #include <mc_solver/TasksQPSolver.h>
+#include <iostream>
 
 namespace mc_solver
 {

src/mc_solver/ContactConstraint.cpp

@@ -11,6 +11,7 @@
 
 #include <Tasks/QPContactConstr.h>
 #include <Tasks/QPMotionConstr.h>
+#include <iostream>
 
 namespace mc_solver
 {