[Explicit Compliance and Safety][3/3] Introduce compliant task variants with Tasks and TVM backend support

include/mc_rbdyn/Robot.h

@@ -17,6 +17,7 @@
 #include <RBDyn/MultiBodyConfig.h>
 #include <RBDyn/MultiBodyGraph.h>
 
+#include <Eigen/src/Core/Matrix.h>
 #include <memory>
 #include <optional>
 #include <unordered_map>
@@ -1114,6 +1115,42 @@ struct MC_RBDYN_DLLAPI Robot
    */
   mc_tvm::Convex & tvmConvex(const std::string & name) const;
 
+  /** @name External Torques
+   *
+   * These functions are used to get or set:
+   * - The estimation of external torques being applied on the robot and its 'equivalent' acceleration
+   * - The additional feedforward compensation torque and its 'equivalent' acceleration
+   *
+   * @{
+   */
+
+  /** Set the external torques */
+  void setExternalTorques(const Eigen::VectorXd & torques);
+
+  /** Get the external torques */
+  const Eigen::VectorXd & externalTorques(void) const;
+
+  /** Set the external torques equivalent accelerations */
+  void setExternalTorquesAcc(const Eigen::VectorXd & accelerations);
+
+  /** Get the external torques equivalent accelerations */
+  const Eigen::VectorXd & externalTorquesAcc(void) const;
+
+  /** Set the compensation torques */
+  void setCompensationTorques(const Eigen::VectorXd & torques);
+
+  /** Get the compensation torques */
+  const std::optional<Eigen::VectorXd> & compensationTorques(void) const;
+
+  /** Set the compensation torques equivalent accelerations */
+  void setCompensationTorquesAcc(const Eigen::VectorXd & accelerations);
+
+  /** Get the compensation torques equivalent accelerations */
+  const std::optional<Eigen::VectorXd> & compensationTorquesAcc(void) const;
+
+  /** @} */
+  /* End of External Forces group */
+
 private:
   Robots * robots_;
   unsigned int robots_idx_;
@@ -1149,6 +1186,14 @@ struct MC_RBDYN_DLLAPI Robot
   std::unordered_map<std::string, RobotFramePtr> frames_;
   /** Mass of this robot */
   double mass_ = 0.0;
+  /** Non modeled external forces acting on the robot **/
+  Eigen::VectorXd externalTorques_;
+  /** Joint accelerations from external forces acting on the robot **/
+  Eigen::VectorXd externalTorquesEquivalentAcc_;
+  /** External forces to be compensated in the commanded torque **/
+  std::optional<Eigen::VectorXd> externalTorqueCompensation_ = std::nullopt;
+  /** Joint accelerations from the compensation in the commanded torque **/
+  std::optional<Eigen::VectorXd> compensationEquivalentAcc_ = std::nullopt;
 
 protected:
   struct NewRobotToken

include/mc_rtc/Configuration.h

@@ -1807,11 +1807,11 @@ template<>
 struct formatter<mc_rtc::Configuration> : public formatter<string_view>
 {
   template<typename FormatContext>
-  #if FMT_VERSION <= 9 * 10000
-    auto format(const mc_rtc::Configuration & c, FormatContext & ctx)
-  #else
-      auto format(const mc_rtc::Configuration & c, FormatContext & ctx) const -> decltype(ctx.out())
-  #endif
+#if FMT_VERSION <= 9 * 10000
+  auto format(const mc_rtc::Configuration & c, FormatContext & ctx)
+#else
+  auto format(const mc_rtc::Configuration & c, FormatContext & ctx) const -> decltype(ctx.out())
+#endif
   {
     return formatter<string_view>::format(static_cast<std::string>(c), ctx);
   }

include/mc_solver/DynamicsConstraint.h

@@ -28,8 +28,15 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
    * \param robotIndex The index of the robot affected by this constraint
    * \param timeStep Solver timestep
    * \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, bool infTorque = false);
+  DynamicsConstraint(const mc_rbdyn::Robots & robots,
+                     unsigned int robotIndex,
+                     double timeStep,
+                     bool infTorque = false,
+                     bool compensateExtTorques = false);
 
   /** Constructor
    * Builds a damped joint limits constraint and a motion constr depending on
@@ -42,13 +49,25 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
    * offset}
    * \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,
                      double velocityPercent = 1.0,
-                     bool infTorque = false);
+                     bool infTorque = false,
+                     bool compensateExtTorques = false);
+
+  /** \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
+   */
+  void update(QPSolver & solver) override;
 
   /** Returns the tasks::qp::MotionConstr
    *

include/mc_solver/QPSolver.h

@@ -344,11 +344,11 @@ template<>
 struct formatter<mc_solver::QPSolver::Backend> : public formatter<string_view>
 {
   template<typename FormatContext>
-  #if FMT_VERSION <= 9 * 10000
-    auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx)
-  #else
-    auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out())
-  #endif
+#if FMT_VERSION <= 9 * 10000
+  auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx)
+#else
+  auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out())
+#endif
   {
     using Backend = mc_solver::QPSolver::Backend;
     switch(backend)

include/mc_tasks/CompliantEndEffectorTask.h

@@ -0,0 +1,80 @@
+/*
+ * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+#include <mc_tasks/EndEffectorTask.h>
+#include <RBDyn/MultiBody.h>
+#include <Eigen/src/Core/Matrix.h>
+
+namespace mc_tasks
+{
+
+/*! \brief Controls an end-effector
+ *
+ * This task is a thin wrapper around the appropriate tasks in Tasks.
+ * The task objective is given in the world frame. For relative control
+ * see mc_tasks::RelativeCompliantEndEffectorTask
+ */
+struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask
+{
+public:
+  /*! \brief Constructor
+   *
+   * \param bodyName Name of the body to control
+   *
+   * \param robots Robots controlled by this task
+   *
+   * \param robotIndex Index of the robot controlled by this task
+   *
+   * \param stiffness Task stiffness
+   *
+   * \param weight Task weight
+   *
+   */
+  CompliantEndEffectorTask(const std::string & bodyName,
+                           const mc_rbdyn::Robots & robots,
+                           unsigned int robotIndex,
+                           double stiffness,
+                           double weight);
+
+  /** Change acceleration
+   *
+   * \p refAccel Should be of size 6
+   */
+  void refAccel(const Eigen::Vector6d & refAccel) noexcept;
+
+  // Set the compliant behavior of the task
+  void makeCompliant(bool compliance);
+  void setComplianceVector(Eigen::Vector6d gamma);
+
+  // Get compliance state of the task
+  bool isCompliant(void);
+  Eigen::Vector6d getComplianceVector(void);
+
+  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
+
+protected:
+  void addToSolver(mc_solver::QPSolver & solver) override;
+
+  void update(mc_solver::QPSolver & solver) override;
+
+  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
+
+  Eigen::Matrix6d compliant_matrix_;
+
+  const mc_rbdyn::Robot & robot_;
+  mc_tvm::Robot & tvm_robot_;
+
+  unsigned int rIdx_;
+
+  std::string bodyName_;
+  const mc_rbdyn::RobotFrame & frame_;
+
+  rbd::Jacobian * jac_;
+
+  Eigen::Vector6d refAccel_;
+};
+
+} // namespace mc_tasks

include/mc_tasks/CompliantOrientationTask.h

@@ -0,0 +1,59 @@
+/*
+ * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+#include <mc_tasks/OrientationTask.h>
+#include <Eigen/src/Core/Matrix.h>
+
+namespace mc_tasks
+{
+
+struct MC_TASKS_DLLAPI CompliantOrientationTask : public OrientationTask
+{
+public:
+  CompliantOrientationTask(const std::string & bodyName_,
+                           const mc_rbdyn::Robots & robots,
+                           unsigned int robotIndex,
+                           double stiffness,
+                           double weight);
+
+  /** Change reference acceleration
+   *
+   * \p refAccel Should be of size nrDof
+   */
+  void refAccel(const Eigen::Vector3d & refAccel) noexcept;
+
+  // Set task to be compliant or not
+  void makeCompliant(bool compliance);
+  void setComplianceVector(Eigen::Vector3d gamma);
+  // Get compliance state of the task
+  bool isCompliant(void);
+  Eigen::Vector3d getComplianceVector(void);
+
+  // void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
+
+protected:
+  void addToSolver(mc_solver::QPSolver & solver) override;
+
+  void update(mc_solver::QPSolver & solver) override;
+
+  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
+
+  Eigen::Matrix3d Gamma_;
+
+  const mc_rbdyn::Robot & robot_;
+  mc_tvm::Robot & tvm_robot_;
+
+  unsigned int rIdx_;
+
+  std::string bodyName_;
+  const mc_rbdyn::RobotFrame & frame_;
+
+  rbd::Jacobian * jac_;
+
+  Eigen::Vector3d refAccel_;
+};
+
+} // namespace mc_tasks

include/mc_tasks/CompliantPositionTask.h

@@ -0,0 +1,59 @@
+/*
+ * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+#include <mc_tasks/PositionTask.h>
+#include <Eigen/src/Core/Matrix.h>
+
+namespace mc_tasks
+{
+
+struct MC_TASKS_DLLAPI CompliantPositionTask : public PositionTask
+{
+public:
+  CompliantPositionTask(const std::string & bodyName_,
+                        const mc_rbdyn::Robots & robots,
+                        unsigned int robotIndex,
+                        double stiffness,
+                        double weight);
+
+  /** Change reference acceleration
+   *
+   * \p refAccel Should be of size nrDof
+   */
+  void refAccel(const Eigen::Vector3d & refAccel) noexcept;
+
+  // Set task to be compliant or not
+  void makeCompliant(bool compliance);
+  void setComplianceVector(Eigen::Vector3d gamma);
+  // Get compliance state of the task
+  bool isCompliant(void);
+  Eigen::Vector3d getComplianceVector(void);
+
+  // void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
+
+protected:
+  void addToSolver(mc_solver::QPSolver & solver) override;
+
+  void update(mc_solver::QPSolver & solver) override;
+
+  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
+
+  Eigen::Matrix3d Gamma_;
+
+  const mc_rbdyn::Robot & robot_;
+  mc_tvm::Robot & tvm_robot_;
+
+  unsigned int rIdx_;
+
+  std::string bodyName_;
+  const mc_rbdyn::RobotFrame & frame_;
+
+  rbd::Jacobian * jac_;
+
+  Eigen::Vector3d refAccel_;
+};
+
+} // namespace mc_tasks

include/mc_tasks/CompliantPostureTask.h

@@ -0,0 +1,42 @@
+/*
+ * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+#include <mc_tasks/PostureTask.h>
+
+namespace mc_tasks
+{
+
+struct MC_TASKS_DLLAPI CompliantPostureTask : public PostureTask
+{
+public:
+  CompliantPostureTask(const mc_solver::QPSolver & solver, unsigned int rIndex, double stiffness, double weight);
+
+  /** Change reference acceleration
+   *
+   * \p refAccel Should be of size nrDof
+   */
+  void refAccel(const Eigen::VectorXd & refAccel) noexcept;
+
+  // Set task to be compliant or not
+  void makeCompliant(bool compliance);
+  void makeCompliant(Eigen::VectorXd gamma);
+  // Get compliance state of the task
+  bool isCompliant(void);
+
+protected:
+  void update(mc_solver::QPSolver & solver);
+
+  void addToGUI(mc_rtc::gui::StateBuilder & gui);
+
+  Eigen::VectorXd gamma_;
+
+  const mc_rbdyn::Robot & robot_;
+  mc_tvm::Robot & tvm_robot_;
+
+  Eigen::VectorXd refAccel_;
+};
+
+} // namespace mc_tasks