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

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

@@ -166,16 +166,16 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
                                        const moveit::core::JointBoundsVector& joint_bounds, double scaling_override);
 
 /**
- * \brief Finds the joint variables that are exceeding allowable position limits.
+ * \brief Finds the joint indices that are exceeding allowable position limits in at least one variable.
  * @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 joint variables that are violating the specified position limits.
+ * @return The joint indices that violate the specified position limits.
  */
-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<size_t> jointsToHalt(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

@@ -334,14 +334,14 @@ void Servo::setCommandType(const CommandType& command_type)
   expected_command_type_ = command_type;
 }
 
-KinematicState Servo::haltJoints(const std::vector<size_t>& joint_variables_to_halt,
-                                 const KinematicState& current_state, const KinematicState& target_state) const
+KinematicState Servo::haltJoints(const std::vector<size_t>& joint_indices_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 auto idx : joint_variables_to_halt)
+  for (const auto idx : joint_indices_to_halt)
   {
     halting_joint_names << bounded_state.joint_names[idx] + " ";
   }
@@ -361,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector<size_t>& joint_variables_to_h
     // Halt only the joints that are out of bounds
     bounded_state.positions = target_state.positions;
     bounded_state.velocities = target_state.velocities;
-    for (const auto idx : joint_variables_to_halt)
+    for (const auto idx : joint_indices_to_halt)
     {
       bounded_state.positions[idx] = current_state.positions[idx];
       bounded_state.velocities[idx] = 0.0;
@@ -534,14 +534,14 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot
     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<size_t> joint_variables_to_halt =
-        jointVariablesToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_);
+    const std::vector<size_t> joint_indices_to_halt =
+        jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_);
 
     // Apply halting if any joints need to be halted.
-    if (!joint_variables_to_halt.empty())
+    if (!joint_indices_to_halt.empty())
     {
       servo_status_ = StatusCode::JOINT_BOUND;
-      target_state = haltJoints(joint_variables_to_halt, current_state, target_state);
+      target_state = haltJoints(joint_indices_to_halt, current_state, target_state);
     }
   }
 

moveit_ros/moveit_servo/src/utils/common.cpp

@@ -408,33 +408,38 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
   return min_scaling_factor;
 }
 
-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<size_t> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+                                 const moveit::core::JointBoundsVector& joint_bounds,
+                                 const std::vector<double>& margins)
 {
-  std::vector<size_t> variable_idxs_to_halt;
+  std::vector<size_t> joint_indices_to_halt;
 
   // Now get the scaling factor from joint velocity limits.
-  size_t idx = 0;
+  size_t joint_idx = 0;
+  size_t variable_idx = 0;
   for (const auto& joint_bound : joint_bounds)
   {
     for (const auto& variable_bound : *joint_bound)
     {
       if (variable_bound.position_bounded_)
       {
         const bool approaching_negative_bound =
-            velocities[idx] < 0 && positions[idx] < (variable_bound.min_position_ + margins[idx]);
+            velocities[variable_idx] < 0 &&
+            positions[variable_idx] < (variable_bound.min_position_ + margins[variable_idx]);
         const bool approaching_positive_bound =
-            velocities[idx] > 0 && positions[idx] > (variable_bound.max_position_ - margins[idx]);
+            velocities[variable_idx] > 0 &&
+            positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]);
         if (approaching_negative_bound || approaching_positive_bound)
         {
-          variable_idxs_to_halt.push_back(idx);
+          joint_indices_to_halt.push_back(joint_idx);
+          break;  // No need to add the same joint index more than once.
         }
       }
-      ++idx;
+      ++variable_idx;
     }
+    ++joint_idx;
   }
-  return variable_idxs_to_halt;
+  return joint_indices_to_halt;
 }
 
 /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/