Fix joint limit handling when velocities aren't in current state

moveit · May 12, 2021 · f032b21 · f032b21
1 parent b5ad6b0
commit f032b21
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 7 deletions.
diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp
@@ -163,7 +163,9 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou
   if (continuous_)
     return true;
   else
+  {
     return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin);
+  }
 }
 
 bool RevoluteJointModel::harmonizePosition(double* values, const JointModel::Bounds& other_bounds) const

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
@@ -156,7 +156,7 @@ class ServoCalcs
   void enforceVelLimits(Eigen::ArrayXd& delta_theta);
 
   /** \brief Avoid overshooting joint limits */
-  bool enforcePositionLimits();
+  bool enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const;
 
   /** \brief Possibly calculate a velocity scaling factor, due to proximity of
    * singularity and direction of motion

diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp
@@ -582,7 +582,7 @@ bool ServoCalcs::internalServoUpdate(Eigen::ArrayXd& delta_theta,
   composeJointTrajMessage(internal_joint_state_, joint_trajectory);
 
   // Enforce SRDF position limits, might halt if needed, set prev_vel to 0
-  if (!enforcePositionLimits())
+  if (!enforcePositionLimits(joint_trajectory))
   {
     suddenHalt(joint_trajectory);
     status_ = StatusCode::JOINT_BOUND;
@@ -809,43 +809,52 @@ void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta)
   }
 }
 
-bool ServoCalcs::enforcePositionLimits()
+bool ServoCalcs::enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const
 {
+  // Halt if we're past a joint margin and joint velocity is moving even farther past
   bool halting = false;
+  double joint_angle = 0;
 
   for (auto joint : joint_model_group_->getActiveJointModels())
   {
-    // Halt if we're past a joint margin and joint velocity is moving even farther past
-    double joint_angle = 0;
     for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c)
     {
+      // Use the most recent robot joint state
       if (original_joint_state_.name[c] == joint->getName())
       {
         joint_angle = original_joint_state_.position.at(c);
         break;
       }
     }
+
     if (!current_state_->satisfiesPositionBounds(joint, -parameters_->joint_limit_margin))
     {
       const std::vector<moveit_msgs::msg::JointLimits> limits = joint->getVariableBoundsMsg();
 
       // Joint limits are not defined for some joints. Skip them.
       if (!limits.empty())
       {
-        if ((current_state_->getJointVelocities(joint)[0] < 0 &&
+        // 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))) ||
-            (current_state_->getJointVelocities(joint)[0] > 0 &&
+            (joint_trajectory.points[0].velocities[joint_idx] > 0 &&
              (joint_angle > (limits[0].max_position - parameters_->joint_limit_margin))))
         {
           rclcpp::Clock& clock = *node_->get_clock();
           RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, ROS_LOG_THROTTLE_PERIOD,
                                       node_->get_name()
                                           << " " << joint->getName() << " close to a position limit. Halting.");
           halting = true;
+          break;
         }
       }
     }
   }
+
   return !halting;
 }