diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp
index f3c066e0bad..a68fb04ab63 100644
--- 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
index c020749db94..7c3707046b9 100644
--- 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
index 25a98008f4a..bf413e59639 100644
--- 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,22 +809,24 @@ 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();
@@ -832,9 +834,14 @@ bool ServoCalcs::enforcePositionLimits()
       // 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();
@@ -842,10 +849,12 @@ bool ServoCalcs::enforcePositionLimits()
                                       node_->get_name()
                                           << " " << joint->getName() << " close to a position limit. Halting.");
           halting = true;
+          break;
         }
       }
     }
   }
+
   return !halting;
 }