Support multi-DOF joints in Servo

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -320,15 +320,20 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
   }
 
   // Now get the scaling factor from joint velocity limits.
-  for (size_t i = 0; i < joint_bounds.size(); ++i)
+  size_t idx = 0;
+  for (const auto& joint_bound : joint_bounds)
   {
-    const auto joint_bound = (joint_bounds[i])->front();
-    if (joint_bound.velocity_bounded_ && velocities(i) != 0.0)
+    for (const auto& variable_bound : *joint_bound)
     {
-      // Find the ratio of clamped velocity to original velocity
-      const auto bounded_vel = std::clamp(velocities(i), joint_bound.min_velocity_, joint_bound.max_velocity_);
-      const auto joint_scaling_factor = bounded_vel / velocities(i);
-      min_scaling_factor = std::min(min_scaling_factor, joint_scaling_factor);
+      const auto& target_vel = velocities(idx);
+      if (variable_bound.velocity_bounded_ && target_vel != 0.0)
+      {
+        // Find the ratio of clamped velocity to original velocity
+        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;
     }
   }
 

moveit_ros/moveit_servo/tests/test_utils.cpp

@@ -53,7 +53,8 @@ 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());
@@ -68,17 +69,17 @@ TEST(ServoUtilsUnitTests, JointLimitVelocityScaling)
 
   // Create incoming velocities with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05
   // Scale down all other joint velocities by 0.3 to keep it within limits.
-  incoming_velocities(0) *= 1.1;   // This joint is not velocity bounded, so it not accounted for in scaling.
-  incoming_velocities(1) *= 1.05;  // This joint is velocity bounded, and should be the limiting joint in scaling.
+  incoming_velocities(0) *= 1.1;
+  incoming_velocities(1) *= 1.05;
   incoming_velocities.tail<5>() *= 0.7;
 
   constexpr double tol = 0.001;
 
-  // The resulting scaling factor from joints should be 1 / 1.05 = 0.95238
+  // 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);
-  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;
@@ -91,7 +92,7 @@ TEST(ServoUtilsUnitTests, JointLimitVelocityScaling)
   user_velocity_override = 1.0;
   scaling_factor =
       moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override);
-  ASSERT_NEAR(scaling_factor, 0.95238, tol);
+  ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
 }
 
 TEST(ServoUtilsUnitTests, validVector)
@@ -164,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 };
@@ -190,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 };
@@ -217,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 };