Simplification in finding joint name

moveit · May 4, 2021 · 6e5fb0b · 6e5fb0b
1 parent 8cd3537
commit 6e5fb0b
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 11 deletions.
diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp
@@ -164,8 +164,6 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou
     return true;
   else
   {
-    // std::cout << values[0] << "  " << bounds[0].min_position_ - margin << std::endl;
-    // std::cout << !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin) << std::endl;
     return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
   }
 }

diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
@@ -1496,7 +1496,6 @@ class RobotState
   }
   bool satisfiesPositionBounds(const JointModel* joint, double margin = 0.0) const
   {
-    std::cout << joint->satisfiesPositionBounds(getJointPositions(joint), margin) << std::endl;
     return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
   }
   bool satisfiesVelocityBounds(const JointModel* joint, double margin = 0.0) const

diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp
@@ -834,13 +834,10 @@ bool ServoCalcs::enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& jo
       // Joint limits are not defined for some joints. Skip them.
       if (!limits.empty())
       {
-        // Check if pending velocity command is moving past the joint limit
-        size_t joint_idx = 0;
-        for (size_t i = 0; i < joint_trajectory.joint_names.size(); ++i)
-        {
-          if (joint->getName() == joint_trajectory.joint_names[i])
-            joint_idx = i;
-        }
+        // Check if pending velocity command is moving in the right direction
+        auto joint_itr =
+            std::find(joint_trajectory.joint_names.begin(), joint_trajectory.joint_names.end(), joint->getName());
+        auto joint_idx = std::distance(joint_trajectory.joint_names.begin(), joint_itr);
 
         if ((joint_trajectory.points[0].velocities[joint_idx] < 0 &&
              (joint_angle < (limits[0].min_position + parameters_->joint_limit_margin))) ||
@@ -852,7 +849,7 @@ bool ServoCalcs::enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& jo
                                       node_->get_name()
                                           << " " << joint->getName() << " close to a position limit. Halting.");
           halting = true;
-          RCLCPP_ERROR_STREAM(LOGGER, "HALTING!");
+          break;
         }
       }
     }