fix(mc_solver): Update the dynamic constraint to make use of the new setExternalTorques funtion of Tasks MotionConstr

Author: mathieu-celerier

include/mc_solver/DynamicsConstraint.h

@@ -61,6 +61,14 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
                      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
    *
    * This assumes the backend was Tasks

src/mc_solver/DynamicsConstraint.cpp

@@ -147,6 +147,37 @@ 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::update(QPSolver & solver)
+{
+  if(backend_ == QPSolver::Backend::Tasks)
+  {
+    auto & robot = solver.robot(robotIndex_);
+    if(robot.compensationTorques())
+    {
+      static_cast<tasks::qp::MotionConstr *>(motion_constr_.get())
+          ->setExternalTorques(robot.compensationTorques().value());
+    }
+    else
+    {
+      static_cast<tasks::qp::MotionConstr *>(motion_constr_.get())->setExternalTorques(robot.externalTorques());
+    }
+  }
+}
+
 void DynamicsConstraint::addToSolverImpl(QPSolver & solver)
 {
   KinematicsConstraint::addToSolverImpl(solver);