[ImpedanceTask] Add targetVel, targetFrameVelocity

arntanguy · Jan 23, 2024 · f8cf95f · f8cf95f
1 parent b716264
commit f8cf95f
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 11 deletions.
diff --git a/include/mc_tasks/ImpedanceTask.h b/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_; }
@@ -235,11 +235,7 @@ struct MC_TASKS_DLLAPI ImpedanceTask : TransformTask
   using TransformTask::refVelB;
 
   /* \brief Same as targetPose(const sva::PTransformd &) */
-  void target(const sva::PTransformd & pos) override
-  {
-    mc_rtc::log::info("ImpedanceTask::target");
-    targetPose(pos);
-  }
+  void target(const sva::PTransformd & pos) override { targetPose(pos); }
 
   /* \brief Same as targetPose() */
   sva::PTransformd target() const override { return targetPose(); }

diff --git a/include/mc_tasks/TransformTask.h b/include/mc_tasks/TransformTask.h
@@ -64,7 +64,13 @@ struct MC_TASKS_DLLAPI TransformTask : public TrajectoryTaskGeneric
    * \param pos Target in world frame
    *
    */
-  virtual 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.
@@ -92,6 +98,18 @@ struct MC_TASKS_DLLAPI TransformTask : public TrajectoryTaskGeneric
    */
   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
    *

diff --git a/src/mc_tasks/ImpedanceTask.cpp b/src/mc_tasks/ImpedanceTask.cpp
@@ -142,7 +142,7 @@ 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
+  TransformTask::refVelB(T_0_s * (targetVelW_ + deltaCompVelW_)); // represented in the surface frame
   TransformTask::target(compliancePose()); // represented in the world frame
 }
 

diff --git a/src/mc_tasks/TransformTask.cpp b/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,21 +146,28 @@ 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)
@@ -172,6 +180,14 @@ void TransformTask::targetFrame(const mc_rbdyn::Frame & targetFrame, const sva::
   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());