From dbdecfbdc29a8ce68da233ef78a1d8508e8174f4 Mon Sep 17 00:00:00 2001 From: sea-bass Date: Thu, 9 Jan 2025 20:41:12 -0500 Subject: [PATCH] Fix bad logic in Servo twist speed scaling --- moveit_ros/moveit_servo/src/utils/command.cpp | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 0470edebe8..cefa1bb566 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -169,25 +169,6 @@ 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 =