diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index bf6017c741..5c5c872b9e 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -163,7 +163,8 @@ void ServoNode::pauseServo(const std::shared_ptrresetSmoothing(servo_->getCurrentRobotState()); + last_commanded_state_ = servo_->getCurrentRobotState(); + servo_->resetSmoothing(last_commanded_state_); servo_->setCollisionChecking(true); response->message = "Servoing enabled"; @@ -302,7 +303,10 @@ void ServoNode::servoLoop() { // Skip processing if servoing is disabled. if (servo_paused_) + { + servo_frequency.sleep(); continue; + } next_joint_state = std::nullopt; const CommandType expected_type = servo_->getCommandType(); diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index 8d76f0e509..ea630f4ab6 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -313,29 +313,31 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities, const moveit::core::JointBoundsVector& joint_bounds, double scaling_override) { // If override value is close to zero, user is not overriding the scaling + double min_scaling_factor = scaling_override; if (scaling_override < SCALING_OVERRIDE_THRESHOLD) { - scaling_override = 1.0; // Set to no scaling. - double bounded_vel; - std::vector velocity_scaling_factors; // The allowable fraction of computed veclocity + min_scaling_factor = 1.0; // Set to no scaling override. + } - for (size_t i = 0; i < joint_bounds.size(); i++) + // Now get the scaling factor from joint velocity limits. + size_t idx = 0; + for (const auto& joint_bound : joint_bounds) + { + for (const auto& variable_bound : *joint_bound) { - const auto joint_bound = (joint_bounds[i])->front(); - if (joint_bound.velocity_bounded_ && velocities(i) != 0.0) + const auto& target_vel = velocities(idx); + if (variable_bound.velocity_bounded_ && target_vel != 0.0) { // Find the ratio of clamped velocity to original velocity - bounded_vel = std::clamp(velocities(i), joint_bound.min_velocity_, joint_bound.max_velocity_); - velocity_scaling_factors.push_back(bounded_vel / velocities(i)); + const auto bounded_vel = std::clamp(target_vel, variable_bound.min_velocity_, variable_bound.max_velocity_); + const auto joint_scaling_factor = bounded_vel / target_vel; + min_scaling_factor = std::min(min_scaling_factor, joint_scaling_factor); } + ++idx; } - // Find the lowest scaling factor, this helps preserve Cartesian motion. - scaling_override = velocity_scaling_factors.empty() ? - scaling_override : - *std::min_element(velocity_scaling_factors.begin(), velocity_scaling_factors.end()); } - return scaling_override; + return min_scaling_factor; } std::vector jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp index bd6eb9fcfc..8a1b236dae 100644 --- a/moveit_ros/moveit_servo/tests/test_utils.cpp +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -53,11 +53,12 @@ TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) { using moveit::core::loadTestingRobotModel; moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::JointBoundsVector joint_bounds = robot_model->getActiveJointModelsBounds(); + const auto joint_model_group = robot_model->getJointModelGroup("panda_arm"); + const auto joint_bounds = joint_model_group->getActiveJointModelsBounds(); // Get the upper bound for the velocities of each joint. Eigen::VectorXd incoming_velocities(joint_bounds.size()); - for (size_t i = 0; i < joint_bounds.size(); i++) + for (size_t i = 0; i < joint_bounds.size(); ++i) { const auto joint_bound = (*joint_bounds[i])[0]; if (joint_bound.velocity_bounded_) @@ -72,12 +73,26 @@ TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) incoming_velocities(1) *= 1.05; incoming_velocities.tail<5>() *= 0.7; - // The resulting scaling factor selected should be approximately 0.95238 + constexpr double tol = 0.001; + + // The resulting scaling factor from joints should be 1 / 1.1 = 0.90909 double user_velocity_override = 0.0; double scaling_factor = moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); - constexpr double tol = 0.001; - ASSERT_NEAR(scaling_factor, 0.95238, tol); + ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol); + + // With a scaling override lower than the joint limit scaling, it should use the override value. + user_velocity_override = 0.5; + scaling_factor = + moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); + ASSERT_NEAR(scaling_factor, 0.5, tol); + + // With a scaling override higher than the joint limit scaling, it should still use the joint limits. + // Safety always first! + user_velocity_override = 1.0; + scaling_factor = + moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); + ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol); } TEST(ServoUtilsUnitTests, validVector) @@ -150,8 +165,7 @@ TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) servo::Params servo_params; servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); + const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); robot_state->setToDefaultValues(); Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; @@ -176,8 +190,7 @@ TEST(ServoUtilsUnitTests, HaltForSingularityScaling) servo::Params servo_params; servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); + const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); robot_state->setToDefaultValues(); Eigen::Vector cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; @@ -203,8 +216,7 @@ TEST(ServoUtilsUnitTests, LeavingSingularityScaling) servo::Params servo_params; servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); + const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name); robot_state->setToDefaultValues(); Eigen::Vector cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };