Merge pull request #208 from Jmeyer1292/feature/seperate_joint_limit_…

…check

Added a Separate Check For Joint Limits

descartes_moveit/include/descartes_moveit/moveit_state_adapter.h

@@ -110,7 +110,15 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    * called previously in order to enable collision checks, otherwise it will return false.
    * @param joint_pose the joint values at which check for collisions will be made
    */
-  bool isInCollision(const std::vector<double> &joint_pose) const;
+  bool isInCollision(const std::vector<double>& joint_pose) const;
+
+  /**
+   * @brief Checks to see if the given joint_pose state is inside the bounds for the initialized
+   * robot model's active joints.
+   * @param joint_pose The pose to check; should be the same length as your groups active num active joints
+   * @return true if the @e joint_pose is in bounds, false otherwise
+   */
+  bool isInLimits(const std::vector<double>& joint_pose) const;
 
   /**
    * Maximum joint velocities (rad/s) for each joint in the chain. Used for checking in

descartes_moveit/src/moveit_state_adapter.cpp

@@ -253,6 +253,11 @@ bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) co
   return in_collision;
 }
 
+bool MoveitStateAdapter::isInLimits(const std::vector<double> &joint_pose) const
+{
+  return joint_group_->satisfiesPositionBounds(joint_pose.data());
+}
+
 bool MoveitStateAdapter::getFK(const std::vector<double>& joint_pose, Eigen::Affine3d& pose) const
 {
   bool rtn = false;
@@ -290,13 +295,7 @@ bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
     return false;
   }
 
-  // Satisfies joint positional bounds?
-  if (!joint_group_->satisfiesPositionBounds(joint_pose.data()))
-  {
-    return false;
-  }
-  // Is in collision (if collision is active)
-  return !isInCollision(joint_pose);
+  return isInLimits(joint_pose) && !isInCollision(joint_pose);
 }
 
 bool MoveitStateAdapter::isValid(const Eigen::Affine3d& pose) const