From 8008c27183b35cde998cfe5f9b3bce9bec657611 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 20 Sep 2024 00:32:28 -0400 Subject: [PATCH 01/10] [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 From 75ddd6572e8ca698be4da9a8205a48c257a5d46b Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 20 Sep 2024 01:17:21 -0400 Subject: [PATCH 02/10] Tweak tests --- moveit_ros/moveit_servo/src/utils/command.cpp | 4 ++-- moveit_ros/moveit_servo/tests/test_integration.cpp | 2 +- .../moveit_servo/tests/test_ros_integration.cpp | 12 ++++++------ 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 713a435a2e..e2999fe52f 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -172,12 +172,12 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: 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) + 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 > servo_params.scale.rotational) + if (angular_speed_scale > 1.0) { cartesian_position_delta.tail<3>() /= angular_speed_scale; } diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index 3fd5029c18..fdefed4064 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -145,7 +145,7 @@ 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.016865; double delta = next_state.positions[6] - curr_state.positions[6]; constexpr double tol = 0.00001; 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..8ed44ba1df 100644 --- a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp @@ -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); From 73764104d2dc6a618e30e778f6d5f95127e68758 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 20 Sep 2024 07:39:58 -0400 Subject: [PATCH 03/10] Update moveit_ros/moveit_servo/config/servo_parameters.yaml Co-authored-by: Sebastian Jahr --- moveit_ros/moveit_servo/config/servo_parameters.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml index cb862b9628..b16fc6bf0c 100644 --- a/moveit_ros/moveit_servo/config/servo_parameters.yaml +++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml @@ -91,7 +91,7 @@ servo: rotational: { type: double, default_value: 0.8, - 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." + 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, From 3b2a7be2c970b1f5f99a790945249673b8fa4fec Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 20 Sep 2024 08:53:09 -0400 Subject: [PATCH 04/10] Tweak configs --- moveit_ros/moveit_servo/config/panda_simulated_config.yaml | 2 +- moveit_ros/moveit_servo/config/test_config_panda.yaml | 2 +- 2 files changed, 2 insertions(+), 2 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/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 34e735875a..28b9b55634 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -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: false # TODO: Servo integration tests seem flaky with collision checking on 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] From 38fa72882b78448c0515279bd26ae646bb0d6eb3 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 20 Sep 2024 09:04:27 -0400 Subject: [PATCH 05/10] Reduce joint velocity and delay in tests --- moveit_ros/moveit_servo/tests/test_ros_integration.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/test_ros_integration.cpp b/moveit_ros/moveit_servo/tests/test_ros_integration.cpp index 8ed44ba1df..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); @@ -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); From ac1277ab4fd89902704eda2441350a6bb67d5669 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Fri, 20 Sep 2024 09:05:19 -0400 Subject: [PATCH 06/10] Reduce scalings --- moveit_ros/moveit_servo/config/test_config_panda.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 28b9b55634..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: false # TODO: Servo integration tests seem flaky with collision checking on +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] From a2e840842ddc1f0a1bcaed9fe117f418736388fe Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 20 Sep 2024 09:19:15 -0400 Subject: [PATCH 07/10] Fix expected value with lower speeds --- moveit_ros/moveit_servo/tests/test_integration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index fdefed4064..4b1c1936df 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -145,7 +145,7 @@ TEST_F(ServoCppFixture, PoseTest) ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); // Check against manually verified value - constexpr double expected_delta = 0.016865; +constexpr double expected_delta = 0.003364; double delta = next_state.positions[6] - curr_state.positions[6]; constexpr double tol = 0.00001; ASSERT_NEAR(delta, expected_delta, tol); From d6a1b6a59d5df084f8c14b39f29caed0ca06e02e Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 20 Sep 2024 09:22:09 -0400 Subject: [PATCH 08/10] Coding from your phone is a bad idea --- moveit_ros/moveit_servo/tests/test_integration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index 4b1c1936df..002569d2d1 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/moveit_ros/moveit_servo/tests/test_integration.cpp @@ -145,7 +145,7 @@ TEST_F(ServoCppFixture, PoseTest) ASSERT_EQ(status_next, moveit_servo::StatusCode::NO_WARNING); // Check against manually verified value -constexpr double expected_delta = 0.003364; + constexpr double expected_delta = 0.003364; double delta = next_state.positions[6] - curr_state.positions[6]; constexpr double tol = 0.00001; ASSERT_NEAR(delta, expected_delta, tol); From c9bd1bb93e22c88fda6fd026a6c71c8a2525a913 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Fri, 20 Sep 2024 15:07:01 -0400 Subject: [PATCH 09/10] Update expected values and tolerances --- moveit_ros/moveit_servo/tests/test_integration.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index 002569d2d1..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); } @@ -147,7 +147,7 @@ TEST_F(ServoCppFixture, PoseTest) // Check against manually verified value 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); } From e177f4735f1360eddfef873cdd322ae80a88efec Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Sat, 21 Sep 2024 08:42:45 -0400 Subject: [PATCH 10/10] Protect against division by zero --- moveit_ros/moveit_servo/src/utils/command.cpp | 40 ++++++++++++------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index e2999fe52f..c9337deb88 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -171,15 +171,21 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit:: } 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) + if (servo_params.scale.linear > 0.0) { - cartesian_position_delta.head<3>() /= linear_speed_scale; + 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) + if (servo_params.scale.rotational > 0.0) { - cartesian_position_delta.tail<3>() /= angular_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; + } } } @@ -245,17 +251,23 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co 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) + if (servo_params.scale.linear > 0.0) { - translation_error /= linear_speed_scale; + 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) + if (servo_params.scale.rotational > 0.0) { - q_target = q_current.slerp(1.0 / angular_speed_scale, q_target); + 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); + } } // Compute the Cartesian deltas from the velocity-scaled values.