[moveit_servo] fix: ensure ee_pose on planning_frame (moveit#3046)

Co-authored-by: Sebastian Castro <[email protected]>
rhaschke · Oct 31, 2024 · d5f763e · d5f763e
1 parent 08da0b6
commit d5f763e
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();