From 487816f42ae485766e51fd05d24b57af5ef005db Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Mon, 22 Jan 2024 18:21:41 +0100 Subject: [PATCH 1/2] [ImpedanceTask] Fix use of targetSurface, add targetFrame --- include/mc_tasks/ImpedanceTask.h | 14 +++++++++++++- include/mc_tasks/TransformTask.h | 22 ++++++++++++++++++---- src/mc_tasks/ImpedanceTask.cpp | 4 ++-- src/mc_tasks/TransformTask.cpp | 5 +++++ 4 files changed, 38 insertions(+), 7 deletions(-) diff --git a/include/mc_tasks/ImpedanceTask.h b/include/mc_tasks/ImpedanceTask.h index d8da163b89..7c7dc66761 100644 --- a/include/mc_tasks/ImpedanceTask.h +++ b/include/mc_tasks/ImpedanceTask.h @@ -227,10 +227,22 @@ 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 + { + mc_rtc::log::info("ImpedanceTask::target"); + 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 index 3cdb878178..96ad5b7960 100644 --- a/include/mc_tasks/TransformTask.h +++ b/include/mc_tasks/TransformTask.h @@ -7,6 +7,7 @@ #include #include +#include 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 index 2de467a4cf..9fb9f2b549 100644 --- a/src/mc_tasks/ImpedanceTask.cpp +++ b/src/mc_tasks/ImpedanceTask.cpp @@ -143,7 +143,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 - target(compliancePose()); // represented in the world 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 diff --git a/src/mc_tasks/TransformTask.cpp b/src/mc_tasks/TransformTask.cpp index 7438b03a04..29697b4385 100644 --- 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()); From e19764b4277ef6a45b9f7e71154b28a3479d2c10 Mon Sep 17 00:00:00 2001 From: Arnaud TANGUY Date: Tue, 23 Jan 2024 16:00:53 +0100 Subject: [PATCH 2/2] [ImpedanceTask] Add targetVel, targetFrameVelocity --- include/mc_tasks/ImpedanceTask.h | 8 ++------ include/mc_tasks/TransformTask.h | 21 +++++++++++++++++++-- src/mc_tasks/ImpedanceTask.cpp | 2 +- src/mc_tasks/TransformTask.cpp | 22 +++++++++++++++++++--- 4 files changed, 41 insertions(+), 12 deletions(-) diff --git a/include/mc_tasks/ImpedanceTask.h b/include/mc_tasks/ImpedanceTask.h index 7c7dc66761..aacbf9a3d2 100644 --- 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 index 96ad5b7960..195b3d46d1 100644 --- a/include/mc_tasks/TransformTask.h +++ b/include/mc_tasks/TransformTask.h @@ -7,7 +7,6 @@ #include #include -#include namespace mc_tasks { @@ -64,7 +63,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 +97,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 index 9fb9f2b549..02c4ef20ce 100644 --- 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 index 29697b4385..a7cfa3d1bc 100644 --- a/src/mc_tasks/TransformTask.cpp +++ b/src/mc_tasks/TransformTask.cpp @@ -12,6 +12,7 @@ #include +#include #include #include #include @@ -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());