[Servo] Support single-element joint limit margins vector and fix joi…

…nt halting logic for multi-DOF Joints (#2970)

* [Servo] Support single-element joint limit margins vector and fix joint halting logic for multi-DOF joints

* Actually change the default value

* Add missing period to error message

* We actually do want to halt joint indices, not variable indices

* Fix wording in parameter description

* Fix indices extraction

* Fix comment

* Update moveit_ros/moveit_servo/src/utils/common.cpp

* Remove other instance of unused variable

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -38,13 +38,16 @@ is_primary_planning_scene_monitor: true
 check_octomap_collisions: false  # Check collision against the octomap (if a 3D sensor plugin is available)
 
 ## MoveIt properties
-move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
+move_group_name: panda_arm  # Often 'manipulator' or 'arm'
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  10.0  # Start decelerating when the condition number hits this (close to singularity)
+lower_singularity_threshold: 10.0  # Start decelerating when the condition number hits this (close to singularity)
 hard_stop_singularity_threshold: 30.0  # Stop when the condition number hits this
-joint_limit_margins: [0.12, 0.12, 0.12, 0.12, 0.12, 0.12, 0.12]  # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger.
 leaving_singularity_threshold_multiplier: 2.0  # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620)
+# Added as a buffer to joint variable position bounds [in that joint variable's respective units].
+# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group.
+# If moving quickly, make these values larger.
+joint_limit_margins: [0.12]
 
 ## Topic names
 cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -326,8 +326,8 @@ servo:
 
   joint_limit_margins: {
     type: double_array,
-    default_value: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-    description: "Added as a buffer to joint limits [radians]. If moving quickly, make this larger.",
+    default_value: [0.1],
+    description: "Added as a buffer to joint variable position bounds [in that joint variable's respective units]. Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group. If moving quickly, make these values larger.",
     validation: {
       lower_element_bounds<>: 0.0
     }
@@ -336,7 +336,7 @@ servo:
   override_velocity_scaling_factor: {
     type: double,
     default_value: 0.0,
-    description: "Scaling factor when servo overrides the velocity (eg: near singularities)",
+    description: "Scaling factor when servo overrides the velocity (e.g, near singularities)",
     validation: {
       bounds<>: [0.0, 1.0]
     }

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -37,13 +37,16 @@ smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"
 is_primary_planning_scene_monitor: true
 
 ## MoveIt properties
-move_group_name:  panda_arm  # Often 'manipulator' or 'arm'
+move_group_name: panda_arm  # Often 'manipulator' or 'arm'
 
 ## Configure handling of singularities and joint limits
-lower_singularity_threshold:  17.0  # Start decelerating when the condition number hits this (close to singularity)
+lower_singularity_threshold: 17.0  # Start decelerating when the condition number hits this (close to singularity)
 hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
-joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]  # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger.
 leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620)
+# Added as a buffer to joint variable position bounds [in that joint variable's respective units].
+# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group.
+# If moving quickly, make these values larger.
+joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
 
 ## Topic names
 cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -189,7 +189,7 @@ class Servo
    * @param servo_params The servo parameters
    * @return True if parameters are valid, else False
    */
-  bool validateParams(const servo::Params& servo_params) const;
+  bool validateParams(const servo::Params& servo_params);
 
   /**
    * \brief Updates the servo parameters and performs validations.
@@ -208,7 +208,7 @@ class Servo
    * @param target_state The target kinematic state.
    * @return The bounded kinematic state.
    */
-  KinematicState haltJoints(const std::vector<int>& joints_to_halt, const KinematicState& current_state,
+  KinematicState haltJoints(const std::vector<size_t>& joints_to_halt, const KinematicState& current_state,
                             const KinematicState& target_state) const;
 
   // Variables
@@ -232,6 +232,9 @@ class Servo
 
   // Map between joint subgroup names and corresponding joint name - move group indices map
   std::unordered_map<std::string, JointNameToMoveGroupIndexMap> joint_name_to_index_maps_;
+
+  // The current joint limit safety margins for each active joint position variable.
+  std::vector<double> joint_limit_margins_;
 };
 
 }  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/utils/common.hpp

@@ -166,15 +166,16 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
                                        const moveit::core::JointBoundsVector& joint_bounds, double scaling_override);
 
 /**
- * \brief Finds the joints that are exceeding allowable position limits.
+ * \brief Finds the joint variable indices corresponding to joints exceeding allowable position limits.
  * @param positions The joint positions.
  * @param velocities The current commanded velocities.
  * @param joint_bounds The allowable limits for the robot joints.
  * @param margins Additional buffer on the actual joint limits.
- * @return The joints that are violating the specified position limits.
+ * @return The joint variable indices that violate the specified position limits.
  */
-std::vector<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
-                              const moveit::core::JointBoundsVector& joint_bounds, const std::vector<double>& margins);
+std::vector<size_t> jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+                                         const moveit::core::JointBoundsVector& joint_bounds,
+                                         const std::vector<double>& margins);
 
 /**
  * \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped.

moveit_ros/moveit_servo/src/servo.cpp

@@ -182,7 +182,7 @@ void Servo::setCollisionChecking(const bool check_collision)
   check_collision ? collision_monitor_->start() : collision_monitor_->stop();
 }
 
-bool Servo::validateParams(const servo::Params& servo_params) const
+bool Servo::validateParams(const servo::Params& servo_params)
 {
   bool params_valid = true;
   auto robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
@@ -247,30 +247,27 @@ bool Servo::validateParams(const servo::Params& servo_params) const
                                      << servo_params.move_group_name << "'" << check_yaml_string);
     params_valid = false;
   }
-  if (servo_params.joint_limit_margins.size() !=
-      robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount())
-  {
-    RCLCPP_ERROR_STREAM(
-        logger_,
-        "The parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the "
-        "move group.  The size of 'joint_limit_margins' is '"
-            << servo_params.joint_limit_margins.size() << "' but the number of joints of the move group '"
-            << servo_params.move_group_name << "' is '"
-            << robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount() << "'"
-            << check_yaml_string);
 
-    params_valid = false;
+  const auto num_dofs = robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount();
+  if (servo_params.joint_limit_margins.size() == 1u)
+  {
+    joint_limit_margins_.clear();
+    for (size_t idx = 0; idx < num_dofs; ++idx)
+    {
+      joint_limit_margins_.push_back(servo_params.joint_limit_margins[0]);
+    }
   }
-  if (servo_params.joint_limit_margins.size() !=
-      robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount())
+  else if (servo_params.joint_limit_margins.size() == num_dofs)
   {
-    RCLCPP_ERROR(logger_,
-                 "Parameter 'joint_limit_margins' must have the same number of elements as the number of joints in the "
-                 "move_group. "
-                 "Size of 'joint_limit_margins' is '%li', but number of joints in '%s' is '%i'. "
-                 "Check the parameters YAML file used to launch this node.",
-                 servo_params.joint_limit_margins.size(), servo_params.move_group_name.c_str(),
-                 robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount());
+    joint_limit_margins_ = servo_params.joint_limit_margins;
+  }
+  else
+  {
+    RCLCPP_ERROR_STREAM(
+        logger_, "The parameter 'joint_limit_margins' must have either a single element or the same number of "
+                 "elements as the degrees of freedom in the active joint group. The size of 'joint_limit_margins' is '"
+                     << servo_params.joint_limit_margins.size() << "' but the number of degrees of freedom in group '"
+                     << servo_params.move_group_name << "' is '" << num_dofs << "'." << check_yaml_string);
     params_valid = false;
   }
 
@@ -337,14 +334,14 @@ void Servo::setCommandType(const CommandType& command_type)
   expected_command_type_ = command_type;
 }
 
-KinematicState Servo::haltJoints(const std::vector<int>& joints_to_halt, const KinematicState& current_state,
-                                 const KinematicState& target_state) const
+KinematicState Servo::haltJoints(const std::vector<size_t>& joint_variables_to_halt,
+                                 const KinematicState& current_state, const KinematicState& target_state) const
 {
   KinematicState bounded_state(target_state.joint_names.size());
   bounded_state.joint_names = target_state.joint_names;
 
   std::stringstream halting_joint_names;
-  for (const int idx : joints_to_halt)
+  for (const auto idx : joint_variables_to_halt)
   {
     halting_joint_names << bounded_state.joint_names[idx] + " ";
   }
@@ -364,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector<int>& joints_to_halt, const K
     // Halt only the joints that are out of bounds
     bounded_state.positions = target_state.positions;
     bounded_state.velocities = target_state.velocities;
-    for (const int idx : joints_to_halt)
+    for (const auto idx : joint_variables_to_halt)
     {
       bounded_state.positions[idx] = current_state.positions[idx];
       bounded_state.velocities[idx] = 0.0;
@@ -533,15 +530,15 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     // Compute velocities based on smoothed joint positions
     target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period;
 
-    // Check if any joints are going past joint position limits
-    const std::vector<int> joints_to_halt =
-        jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins);
+    // Check if any joints are going past joint position limits.
+    const std::vector<size_t> joint_variables_to_halt =
+        jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_);
 
     // Apply halting if any joints need to be halted.
-    if (!joints_to_halt.empty())
+    if (!joint_variables_to_halt.empty())
     {
       servo_status_ = StatusCode::JOINT_BOUND;
-      target_state = haltJoints(joints_to_halt, current_state, target_state);
+      target_state = haltJoints(joint_variables_to_halt, current_state, target_state);
     }
   }
 

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -408,24 +408,46 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
   return min_scaling_factor;
 }
 
-std::vector<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
-                              const moveit::core::JointBoundsVector& joint_bounds, const std::vector<double>& margins)
+std::vector<size_t> jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+                                         const moveit::core::JointBoundsVector& joint_bounds,
+                                         const std::vector<double>& margins)
 {
-  std::vector<int> joint_idxs_to_halt;
-  for (size_t i = 0; i < joint_bounds.size(); i++)
+  std::vector<size_t> variable_indices_to_halt;
+
+  // Now get the scaling factor from joint velocity limits.
+  size_t variable_idx = 0;
+  for (const auto& joint_bound : joint_bounds)
   {
-    const auto joint_bound = (joint_bounds[i])->front();
-    if (joint_bound.position_bounded_)
+    bool halt_joint = false;
+    for (const auto& variable_bound : *joint_bound)
     {
-      const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margins[i]);
-      const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margins[i]);
-      if (negative_bound || positive_bound)
+      // First, loop through all the joint variables to see if the entire joint should be halted.
+      if (variable_bound.position_bounded_)
+      {
+        const bool approaching_negative_bound =
+            velocities[variable_idx] < 0 &&
+            positions[variable_idx] < (variable_bound.min_position_ + margins[variable_idx]);
+        const bool approaching_positive_bound =
+            velocities[variable_idx] > 0 &&
+            positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]);
+        if (approaching_negative_bound || approaching_positive_bound)
+        {
+          halt_joint |= true;
+        }
+      }
+      ++variable_idx;
+
+      // If the joint needs to be halted, add all variable indices corresponding to that joint.
+      if (halt_joint)
       {
-        joint_idxs_to_halt.push_back(i);
+        for (size_t k = variable_idx - joint_bound->size(); k < variable_idx; ++k)
+        {
+          variable_indices_to_halt.push_back(k);
+        }
       }
     }
   }
-  return joint_idxs_to_halt;
+  return variable_indices_to_halt;
 }
 
 /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/