[Servo] Use velocity scaling properly in Cartesian and pose tracking commands

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -28,7 +28,7 @@ publish_joint_accelerations: false
 
 ## Plugins for smoothing outgoing commands
 use_smoothing: true
-smoothing_filter_plugin_name: "online_signal_smoothing::RuckigFilterPlugin"
+smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
 
 # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
 # which other nodes can use as a source for information about the planning environment.

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -86,24 +86,23 @@ servo:
     linear: {
       type: double,
       default_value: 0.4,
-      description: "Max linear velocity. Unit is [m/s]. Only used for Cartesian commands."
+      description: "Max linear velocity in m/s if using speed units or tracking a pose, else a scaling factor on the unitless commanded velocity. Used for Cartesian and pose tracking commands."
     }
     rotational: {
       type: double,
       default_value: 0.8,
-      description: "Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands."
+      description: "Max angular velocity in rad/s if using speed units or tracking a pose, else a scaling factor on the unitless commanded velocity. Used for Cartesian and pose tracking commands."
     }
     joint: {
       type: double,
       default_value: 0.5,
-      description: "Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic."
+      description: "Joint angular/linear velocity scale. Only used for unitless joint commands."
     }
 
-
   incoming_command_timeout: {
     type: double,
     default_value: 0.1,
-    description: "Commands will be discarded if it is older than the timeout.\
+    description: "Commands will be discarded if they are older than the timeout, in seconds.\
                    Important because ROS may drop some messages."
   }
 
@@ -123,7 +122,7 @@ servo:
     linear_tolerance: {
         type: double,
         default_value: 0.001,
-        description: "The allowable linear error when tracking a pose.",
+        description: "The allowable linear error, in meters, when tracking a pose.",
         validation: {
           gt<>: 0.0
         }
@@ -132,7 +131,7 @@ servo:
     angular_tolerance: {
         type: double,
         default_value: 0.01,
-        description: "The allowable angular error when tracking a pose.",
+        description: "The allowable angular error, in radians, when tracking a pose.",
         validation: {
           gt<>: 0.0
         }

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -12,10 +12,10 @@ incoming_command_timeout: 0.5 # seconds
 command_in_type: "unitless" # "unitless" in the range [-1:1], as if from joystick. "speed_units" cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear:  1.0  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
+  linear: 0.2  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.2  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
-  joint: 1.0
+  joint: 0.5
 
 # What type of topic does your robot driver expect?
 # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
@@ -56,7 +56,7 @@ status_topic: ~/status # Publish status to this topic
 command_out_topic: /panda_arm_controller/joint_trajectory # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
+check_collisions: true  # Check collisions?
 collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
 scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]

moveit_ros/moveit_servo/package.xml

@@ -45,6 +45,7 @@
   <exec_depend>joy</exec_depend>
   <exec_depend>launch_param_builder</exec_depend>
   <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>moveit_ros_visualization</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
 

moveit_ros/moveit_servo/src/utils/command.cpp

@@ -159,7 +159,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
   const bool is_zero = command.velocities.isZero();
   if (!is_zero && is_planning_frame && valid_command)
   {
-    // Compute the Cartesian position delta based on incoming command, assumed to be in m/s
+    // Compute the Cartesian position delta based on incoming twist command.
     cartesian_position_delta = command.velocities * servo_params.publish_period;
     // This scaling is supposed to be applied to the command.
     // But since it is only used here, we avoid creating a copy of the command,
@@ -169,6 +169,19 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
       cartesian_position_delta.head<3>() *= servo_params.scale.linear;
       cartesian_position_delta.tail<3>() *= servo_params.scale.rotational;
     }
+    else if (servo_params.command_in_type == "speed_units")
+    {
+      const auto linear_speed_scale = command.velocities.head<3>().norm() / servo_params.scale.linear;
+      if (linear_speed_scale > 1.0)
+      {
+        cartesian_position_delta.head<3>() /= linear_speed_scale;
+      }
+      const auto angular_speed_scale = command.velocities.tail<3>().norm() / servo_params.scale.rotational;
+      if (angular_speed_scale > 1.0)
+      {
+        cartesian_position_delta.tail<3>() /= angular_speed_scale;
+      }
+    }
 
     // Compute the required change in joint angles.
     const auto delta_result =
@@ -178,7 +191,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
     {
       joint_position_delta = delta_result.second;
       // Get velocity scaling information for singularity.
-      const std::pair<double, StatusCode> singularity_scaling_info =
+      const auto singularity_scaling_info =
           velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params);
       // Apply velocity scaling for singularity, if there was any scaling.
       if (singularity_scaling_info.second != StatusCode::NO_WARNING)
@@ -227,11 +240,27 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
 
     // Compute linear and angular change needed.
     const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform(ee_frame) };
-    const Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation());
-    const Eigen::Quaterniond q_error = q_target * q_current.inverse();
-    const Eigen::AngleAxisd angle_axis_error(q_error);
+    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();
+
+    // Limit the commands by the maximum linear and angular speeds provided.
+    const auto linear_speed_scale =
+        (translation_error.norm() / servo_params.publish_period) / servo_params.scale.linear;
+    if (linear_speed_scale > 1.0)
+    {
+      translation_error /= linear_speed_scale;
+    }
+    const auto angular_speed_scale =
+        (std::abs(q_target.angularDistance(q_current)) / servo_params.publish_period) / servo_params.scale.rotational;
+    if (angular_speed_scale > 1.0)
+    {
+      q_target = q_current.slerp(1.0 / angular_speed_scale, q_target);
+    }
 
-    cartesian_position_delta.head<3>() = command.pose.translation() - ee_pose.translation();
+    // Compute the Cartesian deltas from the velocity-scaled values.
+    const auto angle_axis_error = Eigen::AngleAxisd(q_target * q_current.inverse());
+    cartesian_position_delta.head<3>() = translation_error;
     cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
 
     // Compute the required change in joint angles.
@@ -241,6 +270,16 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
     if (status != StatusCode::INVALID)
     {
       joint_position_delta = delta_result.second;
+      // Get velocity scaling information for singularity.
+      const auto singularity_scaling_info =
+          velocityScalingFactorForSingularity(robot_state, cartesian_position_delta, servo_params);
+      // Apply velocity scaling for singularity, if there was any scaling.
+      if (singularity_scaling_info.second != StatusCode::NO_WARNING)
+      {
+        status = singularity_scaling_info.second;
+        RCLCPP_WARN_STREAM(getLogger(), SERVO_STATUS_CODE_MAP.at(status));
+        joint_position_delta *= singularity_scaling_info.first;
+      }
     }
   }
   else

moveit_ros/moveit_servo/tests/test_integration.cpp

@@ -66,8 +66,8 @@ TEST_F(ServoCppFixture, JointJogTest)
 
   // Check against manually verified value
   double delta = next_state.positions[6] - curr_state.positions[6];
-  constexpr double tol = 0.00001;
-  ASSERT_NEAR(delta, 0.02, tol);
+  constexpr double tol = 1.0e-5;
+  ASSERT_NEAR(delta, 0.01, tol);
 }
 
 TEST_F(ServoCppFixture, TwistTest)
@@ -89,9 +89,9 @@ TEST_F(ServoCppFixture, TwistTest)
   ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
 
   // Check against manually verified value
-  constexpr double expected_delta = -0.001693;
+  constexpr double expected_delta = -0.000338;
   double delta = next_state.positions[6] - curr_state.positions[6];
-  constexpr double tol = 0.00001;
+  constexpr double tol = 1.0e-5;
   ASSERT_NEAR(delta, expected_delta, tol);
 }
 
@@ -114,9 +114,9 @@ TEST_F(ServoCppFixture, NonPlanningFrameTwistTest)
   ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
 
   // Check against manually verified value
-  constexpr double expected_delta = 0.001693;
+  constexpr double expected_delta = 0.000338;
   double delta = next_state.positions[6] - curr_state.positions[6];
-  constexpr double tol = 0.00001;
+  constexpr double tol = 1.0e-5;
   ASSERT_NEAR(delta, expected_delta, tol);
 }
 
@@ -145,9 +145,9 @@ TEST_F(ServoCppFixture, PoseTest)
   ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING);
 
   // Check against manually verified value
-  constexpr double expected_delta = 0.057420;
+  constexpr double expected_delta = 0.003364;
   double delta = next_state.positions[6] - curr_state.positions[6];
-  constexpr double tol = 0.00001;
+  constexpr double tol = 1.0e-5;
   ASSERT_NEAR(delta, expected_delta, tol);
 }
 

moveit_ros/moveit_servo/tests/test_ros_integration.cpp

@@ -90,15 +90,15 @@ TEST_F(ServoRosFixture, testJointJog)
 
   std::fill(jog_cmd.velocities.begin(), jog_cmd.velocities.end(), 0.0);
 
-  jog_cmd.velocities[6] = 1.0;
+  jog_cmd.velocities[6] = 0.5;
 
   size_t count = 0;
   while (rclcpp::ok() && count < NUM_COMMANDS)
   {
     jog_cmd.header.stamp = servo_test_node_->now();
     joint_jog_publisher->publish(jog_cmd);
     count++;
-    rclcpp::sleep_for(std::chrono::milliseconds(200));
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
   }
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);
@@ -137,7 +137,7 @@ TEST_F(ServoRosFixture, testTwist)
     twist_cmd.header.stamp = servo_test_node_->now();
     twist_publisher->publish(twist_cmd);
     count++;
-    rclcpp::sleep_for(std::chrono::milliseconds(200));
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
   }
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);
@@ -162,13 +162,13 @@ TEST_F(ServoRosFixture, testPose)
   geometry_msgs::msg::PoseStamped pose_cmd;
   pose_cmd.header.frame_id = "panda_link0";  // Planning frame
 
-  pose_cmd.pose.position.x = 0.3;
-  pose_cmd.pose.position.y = 0.0;
+  pose_cmd.pose.position.x = 0.2;
+  pose_cmd.pose.position.y = -0.2;
   pose_cmd.pose.position.z = 0.6;
-  pose_cmd.pose.orientation.x = 0.7;
-  pose_cmd.pose.orientation.y = -0.7;
-  pose_cmd.pose.orientation.z = -0.000014;
-  pose_cmd.pose.orientation.w = -0.0000015;
+  pose_cmd.pose.orientation.x = 0.7071;
+  pose_cmd.pose.orientation.y = -0.7071;
+  pose_cmd.pose.orientation.z = 0.0;
+  pose_cmd.pose.orientation.w = 0.0;
 
   ASSERT_NE(state_count_, 0);
 
@@ -178,7 +178,7 @@ TEST_F(ServoRosFixture, testPose)
     pose_cmd.header.stamp = servo_test_node_->now();
     pose_publisher->publish(pose_cmd);
     count++;
-    rclcpp::sleep_for(std::chrono::milliseconds(200));
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
   }
 
   ASSERT_GE(traj_count_, NUM_COMMANDS);