Fix twist transformation equations

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -40,7 +40,9 @@ move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
 planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
-ee_frame: panda_link8  # The name of the end effector link, used to return the EE pose
+ee_frame: panda_hand  # The name of the end effector link, used to return the EE pose
+
+command_frame_is_stationary: true # Twist commands are as seem from the stationary frame.
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -21,6 +21,12 @@ servo:
                   must be passed to the node during launch time."
   }
 
+  command_frame_is_stationary: {
+    type: bool,
+    description: "This parameter is used to determine whether a given command is expressed in the space frame \
+                  or body frame and apply appropriate transformations"
+  }
+
   monitored_planning_scene_topic: {
     type: string,
     default_value: "/planning_scene",

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -41,7 +41,8 @@ move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
 planning_frame: panda_link0  # The MoveIt planning frame. Often 'base_link' or 'world'
 
 ## Other frames
-ee_frame: panda_link8  # The name of the end effector link, used to return the EE pose
+ee_frame: panda_hand  # The name of the end effector link, used to return the EE pose
+command_frame_is_stationary: true # Twist commands are as seem from the stationary frame.
 
 ## Configure handling of singularities and joint limits
 lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -134,10 +134,8 @@ class Servo
 private:
   /**
    * \brief Convert a give twist command to planning frame
-   * This implementation does not differentiate between body twist and spatial twist,
-   * i.e if the command is as seen from a stationary or non-stationary frame.
-   * The result of this transformation will only be accurate if the command frame is a stationary frame.
-   * See issue: https://github.com/ros-planning/moveit2/issues/2150
+   * The reference frame specified by `frame_id` should be a stationary frame.
+   * The command maybe expressed in the space frame basis or the body frame basis.
    * @param command The twist command to be converted
    * @return The transformed twist command.
    */

moveit_ros/moveit_servo/src/servo.cpp

@@ -466,15 +466,31 @@ KinematicState Servo::getNextJointState(const ServoInput& command)
 
 const TwistCommand Servo::toPlanningFrame(const TwistCommand& command)
 {
+  // Reverse the twist vector since we need it to be (angular, linear) for transforming using tf2.
+  Eigen::VectorXd reordered_twist(6);
+  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);
+  }
+
   if (command.frame_id != servo_params_.planning_frame)
   {
-    RCLCPP_WARN_STREAM_THROTTLE(LOGGER, *node_->get_clock(), ROS_LOG_THROTTLE_PERIOD,
-                                "Twist command is not in planning frame, transformation may not be accurate.");
+    RCLCPP_DEBUG_STREAM(LOGGER, "Transforming from " << command.frame_id << " to " << servo_params_.planning_frame);
+    const auto planning_to_command_tf =
+        transform_buffer_.lookupTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0));
+    tf2::doTransform(reordered_twist, reordered_twist, planning_to_command_tf);
   }
-  const auto command_to_planning_frame =
-      transform_buffer_.lookupTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0));
-  Eigen::VectorXd transformed_twist = command.velocities;
-  tf2::doTransform(transformed_twist, transformed_twist, command_to_planning_frame);
+
+  // Reverse again to make it (linear, angular)
+  Eigen::VectorXd transformed_twist(6);
+  transformed_twist.head<3>() = reordered_twist.tail<3>();
+  transformed_twist.tail<3>() = reordered_twist.head<3>();
   return TwistCommand{ servo_params_.planning_frame, transformed_twist };
 }
 

moveit_ros/moveit_servo/src/utils/command.cpp

@@ -274,5 +274,4 @@ JointDeltaResult jointDeltaFromIK(const Eigen::VectorXd& cartesian_position_delt
 
   return std::make_pair(status, delta_theta);
 }
-
 }  // namespace moveit_servo