Fix velocity scaling factor calculations and support multi-DOF joints…

… in Servo (#2540)

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -163,7 +163,8 @@ void ServoNode::pauseServo(const std::shared_ptr<std_srvs::srv::SetBool::Request
   else
   {
     // Reset the smoothing plugin with the robot's current state in case the robot moved between pausing and unpausing.
-    servo_->resetSmoothing(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();

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<double> 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<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,

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<double, 6> 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<double, 6> 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<double, 6> cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };