From 8008c27183b35cde998cfe5f9b3bce9bec657611 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 20 Sep 2024 00:32:28 -0400 Subject: [PATCH] [Servo] Use velocity scaling properly in Cartesian and pose tracking commands --- .../moveit_servo/config/servo_parameters.yaml | 13 +++-- moveit_ros/moveit_servo/package.xml | 1 + moveit_ros/moveit_servo/src/utils/command.cpp | 51 ++++++++++++++++--- 3 files changed, 52 insertions(+), 13 deletions(-) diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index 497252714c..cb862b9628 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/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 } diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index b6468437af..1a84749dc6 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -45,6 +45,7 @@ joy launch_param_builder moveit_configs_utils + moveit_ros_visualization robot_state_publisher tf2_ros diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index fe4cb62546..713a435a2e 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/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 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