From 7d3693baa157357c488cf1232177cbff641a3fc6 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Tue, 24 Sep 2024 02:44:05 -0400 Subject: [PATCH] [Servo] Use velocity scaling properly in Cartesian and pose tracking commands (#3007) * [Servo] Use velocity scaling properly in Cartesian and pose tracking commands * Tweak tests * Update moveit_ros/moveit_servo/config/servo_parameters.yaml Co-authored-by: Sebastian Jahr * Tweak configs * Reduce joint velocity and delay in tests * Reduce scalings * Fix expected value with lower speeds * Coding from your phone is a bad idea * Update expected values and tolerances * Protect against division by zero * More time before integration test starts --------- Co-authored-by: Sebastian Jahr --- .../config/panda_simulated_config.yaml | 2 +- .../moveit_servo/config/servo_parameters.yaml | 13 ++-- .../config/test_config_panda.yaml | 8 +-- moveit_ros/moveit_servo/package.xml | 1 + moveit_ros/moveit_servo/src/utils/command.cpp | 63 +++++++++++++++++-- .../launch/servo_cpp_integration.test.py | 2 +- .../moveit_servo/tests/test_integration.cpp | 16 ++--- .../tests/test_ros_integration.cpp | 20 +++--- 8 files changed, 88 insertions(+), 37 deletions(-) diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 980370ca18..ea6e6147e3 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/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. diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index 497252714c..b16fc6bf0c 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 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 } diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 34e735875a..5ccb464924 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/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] 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..c9337deb88 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,25 @@ 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") + { + if (servo_params.scale.linear > 0.0) + { + 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; + } + } + if (servo_params.scale.rotational > 0.0) + { + 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 +197,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 +246,33 @@ 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. + if (servo_params.scale.linear > 0.0) + { + 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; + } + } + if (servo_params.scale.rotational > 0.0) + { + 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 +282,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 diff --git a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py index fd14c8bb9d..bf5aa82085 100644 --- a/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py +++ b/moveit_ros/moveit_servo/tests/launch/servo_cpp_integration.test.py @@ -112,7 +112,7 @@ def generate_test_description(): joint_state_broadcaster_spawner, panda_arm_controller_spawner, test_container, - launch.actions.TimerAction(period=2.0, actions=[servo_gtest]), + launch.actions.TimerAction(period=5.0, actions=[servo_gtest]), launch_testing.actions.ReadyToTest(), ] ), { diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index 3fd5029c18..d04587a9ab 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/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); } diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 3022b8a278..4485200b7e 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -90,7 +90,7 @@ 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) @@ -98,7 +98,7 @@ TEST_F(ServoRosFixture, testJointJog) 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);