[Servo] Fix Twist transformation

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -40,7 +40,7 @@ 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_link8  # The name of the IK tip link
 
 ## 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

@@ -16,7 +16,8 @@ servo:
 
   ee_frame: {
     type: string,
-    description: "The name of end effector frame of your robot \
+    description: "The name of end effector frame of your robot, \
+                  this should also be the IK tip frame of your IK solver. \
                   This parameter does not have a default value and \
                   must be passed to the node during launch time."
   }
@@ -125,6 +126,15 @@ servo:
       description: "Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic."
     }
 
+
+  apply_twist_commands_about_ee_frame: {
+    type: bool,
+    default_value: true,
+    description: "If true, the angular velocity specified in the twist command is applied about the ee frame axes \
+                  if false, the twist computed will include the linear component from rotation of ee frame about the planning frame, \
+                  due to the existence of a lever between the two frames."
+  }
+
   override_velocity_scaling_factor: {
     type: double,
     default_value: 0.0,

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -41,7 +41,7 @@ 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_link8  # The name of the IK tip link
 
 ## 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/demos/servo_keyboard_input.cpp

@@ -68,6 +68,8 @@ constexpr int8_t KEYCODE_Q = 0x71;
 constexpr int8_t KEYCODE_R = 0x72;
 constexpr int8_t KEYCODE_J = 0x6A;
 constexpr int8_t KEYCODE_T = 0x74;
+constexpr int8_t KEYCODE_W = 0x77;
+constexpr int8_t KEYCODE_E = 0x65;
 }  // namespace
 
 // Some constants used in the Servo Teleop demo
@@ -77,6 +79,7 @@ const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
 const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
 const size_t ROS_QUEUE_SIZE = 10;
 const std::string PLANNING_FRAME_ID = "panda_link0";
+const std::string EE_FRAME_ID = "panda_link8";
 }  // namespace
 
 // A class for reading the key inputs from the terminal
@@ -131,9 +134,10 @@ class KeyboardServo
 
   std::shared_ptr<moveit_msgs::srv::ServoCommandType::Request> request_;
   double joint_vel_cmd_;
+  std::string command_frame_id_;
 };
 
-KeyboardServo::KeyboardServo() : joint_vel_cmd_(1.0)
+KeyboardServo::KeyboardServo() : joint_vel_cmd_(1.0), command_frame_id_{ "panda_link0" }
 {
   nh_ = rclcpp::Node::make_shared("servo_keyboard_input");
 
@@ -191,6 +195,7 @@ int KeyboardServo::keyLoop()
   puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging.");
   puts("Use 'j' to select joint jog. ");
   puts("Use 't' to select twist ");
+  puts("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame");
   puts("'Q' to quit.");
 
   for (;;)
@@ -324,6 +329,16 @@ int KeyboardServo::keyLoop()
           }
         }
         break;
+      case KEYCODE_W:
+        RCLCPP_DEBUG(nh_->get_logger(), "w");
+        RCLCPP_INFO_STREAM(nh_->get_logger(), "Command frame set to: " << PLANNING_FRAME_ID);
+        command_frame_id_ = PLANNING_FRAME_ID;
+        break;
+      case KEYCODE_E:
+        RCLCPP_DEBUG(nh_->get_logger(), "e");
+        RCLCPP_INFO_STREAM(nh_->get_logger(), "Command frame set to: " << EE_FRAME_ID);
+        command_frame_id_ = EE_FRAME_ID;
+        break;
       case KEYCODE_Q:
         RCLCPP_DEBUG(nh_->get_logger(), "quit");
         return 0;
@@ -333,7 +348,7 @@ int KeyboardServo::keyLoop()
     if (publish_twist)
     {
       twist_msg->header.stamp = nh_->now();
-      twist_msg->header.frame_id = PLANNING_FRAME_ID;
+      twist_msg->header.frame_id = command_frame_id_;
       twist_pub_->publish(std::move(twist_msg));
       publish_twist = false;
     }

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -133,11 +133,11 @@ 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
+   * \brief Convert a give twist command to planning frame,
+   * The command frame specified by `command.frame_id` is expected to be a stationary frame or end-effector frame.
+   * References:
+   * https://core.ac.uk/download/pdf/154240607.pdf
+   * https://www.seas.upenn.edu/~meam520/notes02/Forces8.pdf
    * @param command The twist command to be converted
    * @return The transformed twist command.
    */

moveit_ros/moveit_servo/src/servo.cpp

@@ -50,7 +50,6 @@ namespace
 const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_servo.servo");
 constexpr double ROBOT_STATE_WAIT_TIME = 5.0;  // seconds
 constexpr double STOPPED_VELOCITY_EPS = 1e-4;
-constexpr auto ROS_LOG_THROTTLE_PERIOD = std::chrono::milliseconds(3000).count();
 }  // namespace
 
 namespace moveit_servo
@@ -466,15 +465,47 @@ KinematicState Servo::getNextJointState(const ServoInput& command)
 
 const TwistCommand Servo::toPlanningFrame(const TwistCommand& command)
 {
+  Eigen::Vector<double, 6> transformed_twist = command.velocities;
+
   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.");
+    Eigen::Vector<double, 6> reordered_twist;
+    // (linear, angular) -> (angular, linear)
+    reordered_twist.head<3>() = command.velocities.tail<3>();
+    reordered_twist.tail<3>() = command.velocities.head<3>();
+
+    const auto planning_to_command_tf = tf2::transformToEigen(
+        transform_buffer_.lookupTransform(command.frame_id, servo_params_.planning_frame, rclcpp::Time(0)));
+
+    if (servo_params_.apply_twist_commands_about_ee_frame)
+    {
+      reordered_twist.head<3>() = planning_to_command_tf.rotation() * reordered_twist.head<3>();
+      reordered_twist.tail<3>() = planning_to_command_tf.rotation() * reordered_twist.tail<3>();
+    }
+    else
+    {
+      Eigen::MatrixXd adjoint(6, 6);
+      const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
+      const Eigen::Vector3d& translation = planning_to_command_tf.translation();
+
+      Eigen::Matrix3d skew_translation;
+      skew_translation.row(0) << 0, -translation(2), translation(1);
+      skew_translation.row(1) << translation(2), 0, -translation(0);
+      skew_translation.row(2) << -translation(1), translation(0), 0;
+
+      adjoint.topLeftCorner(3, 3) = rotation;
+      adjoint.topRightCorner(3, 3).setZero();
+      adjoint.bottomLeftCorner(3, 3) = skew_translation * rotation;
+      adjoint.bottomRightCorner(3, 3) = rotation;
+
+      reordered_twist = adjoint * reordered_twist;
+    }
+
+    // (angular, linear) -> (linear, angular)
+    transformed_twist.head<3>() = reordered_twist.tail<3>();
+    transformed_twist.tail<3>() = reordered_twist.head<3>();
   }
-  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);
+
   return TwistCommand{ servo_params_.planning_frame, transformed_twist };
 }
 

moveit_ros/moveit_servo/tests/test_integration.cpp

@@ -92,6 +92,30 @@ TEST_F(ServoCppFixture, TwistTest)
   ASSERT_NEAR(delta, expected_delta, tol);
 }
 
+TEST_F(ServoCppFixture, NonPlanningFrameTwistTest)
+{
+  moveit_servo::StatusCode status_curr, status_next, status_initial;
+  moveit_servo::TwistCommand twist_non_zero{ servo_params_.ee_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
+  moveit_servo::TwistCommand twist_zero{ servo_params_.ee_frame, { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+
+  servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
+  status_initial = servo_test_instance_->getStatus();
+  ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
+  moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(twist_zero);
+  status_curr = servo_test_instance_->getStatus();
+  ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);
+
+  moveit_servo::KinematicState next_state = servo_test_instance_->getNextJointState(twist_non_zero);
+  status_next = servo_test_instance_->getStatus();
+  ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
+
+  // Check against manually verified value
+  constexpr double expected_delta = 0.001693;
+  double delta = next_state.positions[6] - curr_state.positions[6];
+  constexpr double tol = 0.00001;
+  ASSERT_NEAR(delta, expected_delta, tol);
+}
+
 TEST_F(ServoCppFixture, PoseTest)
 {
   moveit_servo::StatusCode status_curr, status_next, status_initial;