diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 1acbe6d02e..d96490a3c8 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -42,7 +42,9 @@ planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or ' ## Other frames 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 seen from a stationary frame. +# If true, twist commands are given in a stationary frame; otherwise, they are in a moving frame. +# See https://github.com/ros-planning/moveit2/issues/2150 for more details +command_frame_is_stationary: true ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 2d470a00b0..1766d70c24 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -42,7 +42,9 @@ planning_frame: panda_link0 # The MoveIt planning frame. Often 'base_link' or ' ## Other frames 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. +# If true, twist commands are given in a stationary frame; otherwise, they are in a moving frame. +# See https://github.com/ros-planning/moveit2/issues/2150 for more details +command_frame_is_stationary: true ## Configure handling of singularities and joint limits lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index df30a1287f..8735b1e657 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -490,6 +490,8 @@ const TwistCommand Servo::toPlanningFrame(const TwistCommand& command) tf2::doTransform(reordered_twist, reordered_twist, body_to_space_tf); } +// else: the command is in the planning frame so no transformation is needed + // Reverse again to make it (linear, angular) Eigen::VectorXd transformed_twist(6); transformed_twist.head<3>() = reordered_twist.tail<3>();