Review updates

moveit · Aug 15, 2023 · ad0e383 · ad0e383
1 parent 9a3a075
commit ad0e383
Showing 1 changed file with 15 additions and 11 deletions.
diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp
@@ -470,20 +470,24 @@ const TwistCommand Servo::toPlanningFrame(const TwistCommand& command)
   reordered_twist.tail<3>() = command.velocities.head<3>();
   reordered_twist.head<3>() = command.velocities.tail<3>();
 
-  if (!servo_params_.command_frame_is_stationary)
-  {
-    RCLCPP_DEBUG_STREAM(LOGGER, "Transforming from body frame to space frame");
-    const auto body_to_space_tf =
-        transform_buffer_.lookupTransform(command.frame_id, servo_params_.ee_frame, rclcpp::Time(0));
-    tf2::doTransform(reordered_twist, reordered_twist, body_to_space_tf);
-  }
+  const bool reference_not_planning_frame = (command.frame_id != servo_params_.planning_frame);
+  const bool command_in_space_frame = servo_params_.command_frame_is_stationary;
 
-  if (command.frame_id != servo_params_.planning_frame)
+  if (reference_not_planning_frame && command_in_space_frame)
   {
-    RCLCPP_DEBUG_STREAM(LOGGER, "Transforming from " << command.frame_id << " to " << servo_params_.planning_frame);
-    const auto planning_to_command_tf =
+    RCLCPP_DEBUG_STREAM(LOGGER, "Transforming twist from stationary frame: "
+                                    << command.frame_id << "to planning frame: " << servo_params_.planning_frame);
+    const auto command_to_planning_tf =
         transform_buffer_.lookupTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0));
-    tf2::doTransform(reordered_twist, reordered_twist, planning_to_command_tf);
+    tf2::doTransform(reordered_twist, reordered_twist, command_to_planning_tf);
+  }
+  else if (!command_in_space_frame)
+  {
+    RCLCPP_DEBUG_STREAM(LOGGER, "Transforming twist from moving (body) frame: "
+                                    << servo_params_.ee_frame << "to planning frame: " << servo_params_.planning_frame);
+    const auto body_to_space_tf =
+        transform_buffer_.lookupTransform(servo_params_.planning_frame, servo_params_.ee_frame, rclcpp::Time(0));
+    tf2::doTransform(reordered_twist, reordered_twist, body_to_space_tf);
   }
 
   // Reverse again to make it (linear, angular)