diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h index 53494481..0182bb6d 100644 --- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h +++ b/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 &joint_pose) const; + bool isInCollision(const std::vector& 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& joint_pose) const; /** * Maximum joint velocities (rad/s) for each joint in the chain. Used for checking in diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp index 1ae160c2..168c7017 100644 --- a/descartes_moveit/src/moveit_state_adapter.cpp +++ b/descartes_moveit/src/moveit_state_adapter.cpp @@ -253,6 +253,11 @@ bool MoveitStateAdapter::isInCollision(const std::vector& joint_pose) co return in_collision; } +bool MoveitStateAdapter::isInLimits(const std::vector &joint_pose) const +{ + return joint_group_->satisfiesPositionBounds(joint_pose.data()); +} + bool MoveitStateAdapter::getFK(const std::vector& joint_pose, Eigen::Affine3d& pose) const { bool rtn = false; @@ -290,13 +295,7 @@ bool MoveitStateAdapter::isValid(const std::vector& 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