[Explicit Compliance and Safety][2/3] Add optional feedforward compensation torques to DynamicConstraint in TVM and Tasks backends

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_tvm/DynamicFunction.h

@@ -36,7 +36,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun
   SET_UPDATES(DynamicFunction, Jacobian, B)
 
   /** Construct the equation of motion for a given robot */
-  DynamicFunction(const mc_rbdyn::Robot & robot);
+  DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateExternalForces = false);
 
   /** Add a contact to the function
    *
@@ -73,6 +73,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun
   void updateb();
 
   const mc_rbdyn::Robot & robot_;
+  const bool compensateExternalForces_;
 
   /** Holds data for the force part of the motion equation */
   struct ForceContact

include/mc_tvm/Robot.h

@@ -16,6 +16,8 @@
 
 #include <RBDyn/FD.h>
 
+#include <optional>
+
 namespace mc_tvm
 {
 
@@ -47,8 +49,8 @@ namespace mc_tvm
  */
 struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
 {
-  SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C)
-  SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C)
+  SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C, ExternalForces)
+  SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C, ExternalForces)
 
   friend struct mc_rbdyn::Robot;
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -96,6 +98,11 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
   /** Access q second derivative (joint acceleration) */
   inline tvm::VariablePtr & alphaD() noexcept { return ddq_; }
 
+  /** Access joint acceleration from external forces (const) */
+  inline const Eigen::VectorXd & alphaDExternal() const noexcept { return ddq_ext_; }
+  /** Access joint acceleration from external forces */
+  inline Eigen::VectorXd & alphaDExternal() noexcept { return ddq_ext_; }
+
   /** Access floating-base variable (const) */
   inline const tvm::VariablePtr & qFloatingBase() const noexcept { return q_fb_; }
   /** Access free-flyer variable */
@@ -134,6 +141,21 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
   /** Access tau variable */
   inline tvm::VariablePtr & tau() { return tau_; }
 
+  /** Access tau external variable (const) */
+  inline const Eigen::VectorXd & tauExternal() const noexcept { return tau_ext_; }
+  /** Access tau external variable */
+  inline Eigen::VectorXd & tauExternal() { return tau_ext_; }
+
+  /** Access tau compensation (const) */
+  inline const std::optional<Eigen::VectorXd> & tauCompensation() const noexcept { return tau_comp_; }
+  /** Access tau compensation */
+  inline std::optional<Eigen::VectorXd> & tauCompensation() noexcept { return tau_comp_; }
+
+  /** Access joint acceleration from compensation torques (const) */
+  inline const std::optional<Eigen::VectorXd> & alphaDCompensation() const noexcept { return ddq_comp_; }
+  /** Access joint acceleration from compensation torques */
+  inline std::optional<Eigen::VectorXd> & alphaDCompensation() noexcept { return ddq_comp_; }
+
   /** Returns the CoM algorithm associated to this robot (const) */
   inline const CoM & comAlgo() const noexcept { return *com_; }
 
@@ -203,8 +225,16 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
   tvm::VariablePtr dq_;
   /** Double derivative of q */
   tvm::VariablePtr ddq_;
+  /** Joint acceleration from external forces */
+  Eigen::VectorXd ddq_ext_;
+  /** Joint acceleration from compensation torques */
+  std::optional<Eigen::VectorXd> ddq_comp_;
   /** Tau variable */
   tvm::VariablePtr tau_;
+  /** Tau external variable */
+  Eigen::VectorXd tau_ext_;
+  /** Tau compensation variable */
+  std::optional<Eigen::VectorXd> tau_comp_;
   /** Normal accelerations of the bodies */
   std::vector<sva::MotionVecd> normalAccB_;
   /** Forward dynamics algorithm associated to this robot */
@@ -225,6 +255,7 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node<Robot>
   void updateNormalAcceleration();
   void updateH();
   void updateC();
+  void updateExternalForces();
 };
 
 } // namespace mc_tvm

src/mc_rbdyn/Robot.cpp

@@ -18,10 +18,13 @@
 
 #include <RBDyn/CoM.h>
 #include <RBDyn/FA.h>
+#include <RBDyn/FD.h>
 #include <RBDyn/FK.h>
 #include <RBDyn/FV.h>
 #include <RBDyn/NumericalIntegration.h>
 
+#include <Eigen/src/Core/Matrix.h>
+#include <optional>
 #include <sch/S_Object/S_Cylinder.h>
 #include <sch/S_Object/S_Superellipsoid.h>
 
@@ -510,6 +513,9 @@ Robot::Robot(NewRobotToken,
   flexibility_ = module_.flexibility();
 
   zmp_ = Eigen::Vector3d::Zero();
+
+  externalTorques_ = Eigen::VectorXd::Zero(mb().nrDof());
+  externalTorquesEquivalentAcc_ = Eigen::VectorXd::Zero(mb().nrDof());
 }
 
 Robot::~Robot()
@@ -1567,4 +1573,56 @@ mc_tvm::Convex & Robot::tvmConvex(const std::string & name) const
   return *it->second;
 }
 
+void Robot::setExternalTorques(const Eigen::VectorXd & torques)
+{
+  externalTorques_.noalias() = torques;
+}
+
+const Eigen::VectorXd & Robot::externalTorques(void) const
+{
+  return externalTorques_;
+}
+
+void Robot::setExternalTorquesAcc(const Eigen::VectorXd & accelerations)
+{
+  externalTorquesEquivalentAcc_.noalias() = accelerations;
+}
+
+const Eigen::VectorXd & Robot::externalTorquesAcc(void) const
+{
+  return externalTorquesEquivalentAcc_;
+}
+
+void Robot::setCompensationTorques(const Eigen::VectorXd & torques)
+{
+  if(!externalTorqueCompensation_) { externalTorqueCompensation_.emplace(torques.size()); }
+  else if(externalTorqueCompensation_->size() == torques.size())
+  {
+    externalTorqueCompensation_->resize(torques.size());
+  }
+
+  externalTorqueCompensation_->noalias() = torques;
+}
+
+const std::optional<Eigen::VectorXd> & Robot::compensationTorques(void) const
+{
+  return externalTorqueCompensation_;
+}
+
+void Robot::setCompensationTorquesAcc(const Eigen::VectorXd & accelerations)
+{
+  if(!externalTorqueCompensation_) { compensationEquivalentAcc_.emplace(accelerations.size()); }
+  else if(externalTorqueCompensation_->size() == accelerations.size())
+  {
+
+    compensationEquivalentAcc_->resize(accelerations.size());
+  }
+  compensationEquivalentAcc_->noalias() = accelerations;
+}
+
+const std::optional<Eigen::VectorXd> & Robot::compensationTorquesAcc(void) const
+{
+  return compensationEquivalentAcc_;
+}
+
 } // namespace mc_rbdyn

src/mc_rbdyn/configuration_io.cpp

@@ -29,11 +29,11 @@ template<>
 struct formatter<rbd::Joint::Type> : public formatter<string_view>
 {
   template<typename FormatContext>
-  #if FMT_VERSION <= 9 * 10000
-    auto format(const rbd::Joint::Type & t, FormatContext & ctx)
-  #else
-    auto format(const rbd::Joint::Type & t, FormatContext & ctx) const -> decltype(ctx.out())
-  #endif
+#if FMT_VERSION <= 9 * 10000
+  auto format(const rbd::Joint::Type & t, FormatContext & ctx)
+#else
+  auto format(const rbd::Joint::Type & t, FormatContext & ctx) const -> decltype(ctx.out())
+#endif
   {
     switch(t)
     {