[ImpedanceTask] Fix use of targetSurface, add targetFrame

arntanguy · Jan 22, 2024 · c26dbc4 · c26dbc4
1 parent 4b63e3e
commit c26dbc4
Show file tree

Hide file tree

Showing 4 changed files with 33 additions and 6 deletions.
diff --git a/include/mc_tasks/ImpedanceTask.h b/include/mc_tasks/ImpedanceTask.h
@@ -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

diff --git a/include/mc_tasks/TransformTask.h b/include/mc_tasks/TransformTask.h
@@ -7,6 +7,7 @@
 #include <mc_tasks/TrajectoryTaskGeneric.h>
 
 #include <mc_rbdyn/RobotFrame.h>
+#include <SpaceVecAlg/SpaceVecAlg>
 
 namespace mc_tasks
 {
@@ -56,14 +57,14 @@ 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 & pos);
 
   /**
    * @brief Targets a robot surface with an optional offset.
@@ -76,7 +77,9 @@ 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
@@ -87,7 +90,18 @@ 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);
+  void targetFrame(const mc_rbdyn::Frame & targetFrame, 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
+   */
+  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(); }

diff --git a/src/mc_tasks/ImpedanceTask.cpp b/src/mc_tasks/ImpedanceTask.cpp
@@ -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

diff --git a/src/mc_tasks/TransformTask.cpp b/src/mc_tasks/TransformTask.cpp
@@ -167,6 +167,11 @@ void TransformTask::targetSurface(unsigned int robotIndex,
   target(robots.robot(robotIndex).frame(surfaceName), offset);
 }
 
+void TransformTask::targetFrame(const mc_rbdyn::Frame & targetFrame, const sva::PTransformd & offset)
+{
+  target(targetFrame, offset);
+}
+
 void TransformTask::target(const mc_rbdyn::Frame & frame, const sva::PTransformd & offset)
 {
   target(offset * frame.position());