Fix indices extraction

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 indices that are exceeding allowable position limits in at least one variable.
+ * \brief Finds the joint variable indices correspond 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 joint indices that violate the specified position limits.
+ * @return The joint variable indices that violate the specified position limits.
  */
-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> 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

@@ -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_indices_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 auto idx : joint_indices_to_halt)
+  for (const auto idx : joint_variables_to_halt)
   {
     halting_joint_names << bounded_state.joint_names[idx] + " ";
   }
@@ -361,7 +361,7 @@ KinematicState Servo::haltJoints(const std::vector<size_t>& joint_indices_to_hal
     // 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_indices_to_halt)
+    for (const auto idx : joint_variables_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_indices_to_halt =
-        jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, joint_limit_margins_);
+    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 (!joint_indices_to_halt.empty())
+    if (!joint_variables_to_halt.empty())
     {
       servo_status_ = StatusCode::JOINT_BOUND;
-      target_state = haltJoints(joint_indices_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,19 +408,21 @@ double jointLimitVelocityScalingFactor(const Eigen::VectorXd& velocities,
   return min_scaling_factor;
 }
 
-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> jointVariablesToHalt(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities,
+                                         const moveit::core::JointBoundsVector& joint_bounds,
+                                         const std::vector<double>& margins)
 {
-  std::vector<size_t> joint_indices_to_halt;
+  std::vector<size_t> variable_indices_to_halt;
 
   // Now get the scaling factor from joint velocity limits.
   size_t joint_idx = 0;
   size_t variable_idx = 0;
   for (const auto& joint_bound : joint_bounds)
   {
+    bool halt_joint = false;
     for (const auto& variable_bound : *joint_bound)
     {
+      // First, loop through all the variables to see if
       if (variable_bound.position_bounded_)
       {
         const bool approaching_negative_bound =
@@ -431,15 +433,24 @@ std::vector<size_t> jointsToHalt(const Eigen::VectorXd& positions, const Eigen::
             positions[variable_idx] > (variable_bound.max_position_ - margins[variable_idx]);
         if (approaching_negative_bound || approaching_positive_bound)
         {
-          joint_indices_to_halt.push_back(joint_idx);
-          break;  // No need to add the same joint index more than once.
+          halt_joint |= true;
         }
       }
       ++variable_idx;
+
+      // If the joint needs to be halted, add all variable indices corresponding to that joint.
+      if (halt_joint)
+      {
+        for (size_t k = variable_idx - joint_bound->size(); k < variable_idx; ++k)
+        {
+          variable_indices_to_halt.push_back(k);
+        }
+      }
     }
+
     ++joint_idx;
   }
-  return joint_indices_to_halt;
+  return variable_indices_to_halt;
 }
 
 /** \brief Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped **/