Merge pull request #428 from arntanguy/topic/ImpedanceTargets

[ImpedanceTask] Fix use of targetSurface and implement targetFrame/targetFrameVelocity

include/mc_tasks/ImpedanceTask.h

@@ -111,7 +111,7 @@ struct MC_TASKS_DLLAPI ImpedanceTask : TransformTask
   const sva::MotionVecd & targetVel() const noexcept { return targetVelW_; }
 
   /*! \brief Set the target velocity of the surface in the world frame. */
-  void targetVel(const sva::MotionVecd & vel) { targetVelW_ = vel; }
+  void targetVel(const sva::MotionVecd & worldVel) override { targetVelW_ = worldVel; }
 
   /*! \brief Get the target acceleration of the surface in the world frame. */
   const sva::MotionVecd & targetAccel() const noexcept { return targetAccelW_; }
@@ -227,10 +227,18 @@ struct MC_TASKS_DLLAPI ImpedanceTask : TransformTask
   /** Targets of SurfaceTransformTask should not be set by the user.
    *  Instead, the user can set the targetPose, targetVel, and targetAccel.
    *  Targets of SurfaceTransformTask are determined from the target values through the impedance equation.
+   *
+   *  We override the target functions of TransformTask to set the targetPose instead.
+   *  This allows functions such as targetFrame, targetSurface to work as expected.
    */
   using TransformTask::refAccel;
   using TransformTask::refVelB;
-  using TransformTask::target;
+
+  /* \brief Same as targetPose(const sva::PTransformd &) */
+  void target(const sva::PTransformd & pos) override { targetPose(pos); }
+
+  /* \brief Same as targetPose() */
+  sva::PTransformd target() const override { return targetPose(); }
 };
 
 } // namespace force

include/mc_tasks/TransformTask.h

@@ -56,14 +56,20 @@ struct MC_TASKS_DLLAPI TransformTask : public TrajectoryTaskGeneric
   void reset() override;
 
   /*! \brief Get the task's target */
-  sva::PTransformd target() const;
+  virtual sva::PTransformd target() const;
 
   /*! \brief Set the task's target
    *
    * \param pos Target in world frame
    *
    */
-  void target(const sva::PTransformd & pos);
+  virtual void target(const sva::PTransformd & worldPos);
+
+  /*! \brief Get the task's target velocity
+   *
+   * \param vel Target velocity in world frame
+   */
+  virtual void targetVel(const sva::MotionVecd & worldVel);
 
   /**
    * @brief Targets a robot surface with an optional offset.
@@ -76,7 +82,32 @@ struct MC_TASKS_DLLAPI TransformTask : public TrajectoryTaskGeneric
    * @param offset
    *  offset defined in the target contact frame
    */
-  void targetSurface(unsigned int robotIndex, const std::string & surfaceName, const sva::PTransformd & offset);
+  void targetSurface(unsigned int robotIndex,
+                     const std::string & surfaceName,
+                     const sva::PTransformd & offset = sva::PTransformd::Identity());
+
+  /**
+   * @brief Targets a given frame with an optional offset
+   *
+   * The offset is given in the target frame
+   *
+   * \param targetFrame Target frame
+   *
+   * \param offset Offset relative to \p targetFrame
+   */
+  void targetFrame(const mc_rbdyn::Frame & targetFrame, const sva::PTransformd & offset = sva::PTransformd::Identity());
+
+  /**
+   * @brief Targets a given frame velocity with an optional offset
+   *
+   * The offset is given in the target frame
+   *
+   * \param targetFrame Target frame
+   *
+   * \param offset Offset relative to \p targetFrame
+   */
+  void targetFrameVelocity(const mc_rbdyn::Frame & targetFrame,
+                           const sva::PTransformd & offset = sva::PTransformd::Identity());
 
   /**
    * @brief Targets a given frame with an optional offset
@@ -87,7 +118,7 @@ struct MC_TASKS_DLLAPI TransformTask : public TrajectoryTaskGeneric
    *
    * \param offset Offset relative to \p targetFrame
    */
-  void target(const mc_rbdyn::Frame & frame, const sva::PTransformd & offset);
+  virtual void target(const mc_rbdyn::Frame & frame, const sva::PTransformd & offset);
 
   /*! \brief Retrieve the controlled frame name */
   inline const std::string & surface() const noexcept { return frame_->name(); }

src/mc_tasks/ImpedanceTask.cpp

@@ -142,8 +142,8 @@ void ImpedanceTask::update(mc_solver::QPSolver & solver)
 
   // 5. Set compliance values to the targets of SurfaceTransformTask
   refAccel(T_0_s * (targetAccelW_ + deltaCompAccelW_)); // represented in the surface frame
-  refVelB(T_0_s * (targetVelW_ + deltaCompVelW_)); // represented in the surface frame
-  target(compliancePose()); // represented in the world frame
+  TransformTask::refVelB(T_0_s * (targetVelW_ + deltaCompVelW_)); // represented in the surface frame
+  TransformTask::target(compliancePose()); // represented in the world frame
 }
 
 void ImpedanceTask::reset()
@@ -153,7 +153,7 @@ void ImpedanceTask::reset()
   TransformTask::reset();
 
   // Set the target and compliance poses to the SurfaceTransformTask target (i.e., the current pose)
-  targetPoseW_ = target();
+  targetPoseW_ = TransformTask::target();
   deltaCompPoseW_ = sva::PTransformd::Identity();
 
   // Reset the target and compliance velocity and acceleration to zero

src/mc_tasks/TransformTask.cpp

@@ -12,6 +12,7 @@
 
 #include <mc_rbdyn/rpy_utils.h>
 
+#include <mc_rbdyn/hat.h>
 #include <mc_rtc/ConfigurationHelpers.h>
 #include <mc_rtc/deprecated.h>
 #include <mc_rtc/gui/Transform.h>
@@ -145,28 +146,48 @@ sva::PTransformd TransformTask::target() const
   }
 }
 
-void TransformTask::target(const sva::PTransformd & pose)
+void TransformTask::target(const sva::PTransformd & worldPose)
 {
   switch(backend_)
   {
     case Backend::Tasks:
-      tasks_error(errorT)->target(pose);
+      tasks_error(errorT)->target(worldPose);
       break;
     case Backend::TVM:
-      tvm_error(errorT)->pose(pose);
+      tvm_error(errorT)->pose(worldPose);
       break;
     default:
       break;
   }
 }
 
+void TransformTask::targetVel(const sva::MotionVecd & worldVec)
+{
+  auto X_0_f = frame_->position();
+  auto velB = X_0_f * worldVec;
+  refVelB(velB);
+}
+
 void TransformTask::targetSurface(unsigned int robotIndex,
                                   const std::string & surfaceName,
                                   const sva::PTransformd & offset)
 {
   target(robots.robot(robotIndex).frame(surfaceName), offset);
 }
 
+void TransformTask::targetFrame(const mc_rbdyn::Frame & targetFrame, const sva::PTransformd & offset)
+{
+  target(targetFrame, offset);
+}
+
+void TransformTask::targetFrameVelocity(const mc_rbdyn::Frame & targetFrame, const sva::PTransformd & offset)
+{
+  auto vel = targetFrame.velocity();
+  auto X_0_f = targetFrame.position();
+  vel.linear() += -mc_rbdyn::hat(X_0_f.rotation().transpose() * offset.translation()) * vel.angular();
+  targetVel(vel);
+}
+
 void TransformTask::target(const mc_rbdyn::Frame & frame, const sva::PTransformd & offset)
 {
   target(offset * frame.position());