[moveit_servo] fix: ensure ee_pose on planning_frame

moveit · Oct 30, 2024 · 908593b · 908593b
1 parent ae05a5e
commit 908593b
Showing 1 changed file with 2 additions and 1 deletion.
diff --git 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<double, 6> 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();