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..5360596a56 100644 --- 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 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());