From d5f763ed18b4a0cedcb2efc40961885f30387146 Mon Sep 17 00:00:00 2001 From: Dongya Jiang <1983534+patrickKXMD@users.noreply.github.com> Date: Thu, 31 Oct 2024 15:41:41 +0800 Subject: [PATCH] [moveit_servo] fix: ensure ee_pose on planning_frame (#3046) Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- moveit_ros/moveit_servo/src/utils/command.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index c9337deb88..0470edebe8 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -245,7 +245,8 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co Eigen::Vector cartesian_position_delta; // Compute linear and angular change needed. - const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(ee_frame) }; + const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(planning_frame).inverse() * + robot_state->getGlobalLinkTransform(ee_frame) }; const Eigen::Quaterniond q_current(ee_pose.rotation()); Eigen::Quaterniond q_target(command.pose.rotation()); Eigen::Vector3d translation_error = command.pose.translation() - ee_pose.translation();