Replaced single value joint_limit_margin with list of joint_limit_mar…

…gin (#2576)

* Replaced joint_limit_margin with list of margins: joint_limit_margin. Enabling setting individual margins for each joint.

* Dimension comment update

* Adding a dimension check within the validateParams() function of servo.cpp to give a clear error message if the size of joint_limit_margis does not match the number of joints of the move_group

* Formatting fix

Co-authored-by: Sebastian Castro <[email protected]>

* Fix panda_simulated_config.yaml

---------

Co-authored-by: AndyZe <[email protected]>
Co-authored-by: Sebastian Castro <[email protected]>

moveit_ros/moveit_servo/config/panda_simulated_config.yaml

@@ -41,7 +41,7 @@ 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)
 hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
-joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+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/ros-planning/moveit2/pull/620)
 
 ## Topic names

moveit_ros/moveit_servo/config/servo_parameters.yaml

@@ -306,12 +306,12 @@ servo:
     description: "Halt all joints in cartesian mode, else halt only the joints at their limit"
   }
 
-  joint_limit_margin: {
-    type: double,
-    default_value: 0.1,
+  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.",
     validation: {
-      gt<>: 0.0
+      lower_element_bounds<>: 0.0
     }
   }
 

moveit_ros/moveit_servo/config/test_config_panda.yaml

@@ -42,7 +42,7 @@ 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)
 hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
-joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+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/ros-planning/moveit2/pull/620)
 
 ## Topic names

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

@@ -153,11 +153,11 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
  * @param positions The joint positions.
  * @param velocities The current commanded velocities.
  * @param joint_bounds The allowable limits for the robot joints.
- * @param margin Additional buffer on the actual joint limits.
+ * @param margins Additional buffer on the actual joint limits.
  * @return The joints that are violating the specified position limits.
  */
 std::vector<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
-                              const moveit::core::JointBoundsVector& joint_bounds, double margin);
+                              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

@@ -246,6 +246,18 @@ bool Servo::validateParams(const servo::Params& servo_params) const
                  servo_params.active_subgroup.c_str(), servo_params.move_group_name.c_str());
     params_valid = false;
   }
+  if (servo_params.joint_limit_margins.size() !=
+      robot_state->getJointModelGroup(servo_params.move_group_name)->getActiveVariableCount())
+  {
+    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());
+    params_valid = false;
+  }
 
   return params_valid;
 }
@@ -503,7 +515,7 @@ KinematicState Servo::getNextJointState(const ServoInput& command)
 
     // 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_margin);
+        jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins);
 
     // Apply halting if any joints need to be halted.
     if (!joints_to_halt.empty())

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -341,16 +341,16 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
 }
 
 std::vector<int> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
-                              const moveit::core::JointBoundsVector& joint_bounds, double margin)
+                              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++)
   {
     const auto joint_bound = (joint_bounds[i])->front();
     if (joint_bound.position_bounded_)
     {
-      const bool negative_bound = velocities[i] < 0 && positions[i] < (joint_bound.min_position_ + margin);
-      const bool positive_bound = velocities[i] > 0 && positions[i] > (joint_bound.max_position_ - margin);
+      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)
       {
         joint_idxs_to_halt.push_back(i);