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

…commands

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 linear 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/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 > servo_params.scale.linear)
+      {
+        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 > servo_params.scale.rotational)
+      {
+        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