From 06b7b77e9e5d51d3eb3ca045099edcb549d32c22 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer <31753650+mechwiz@users.noreply.github.com> Date: Wed, 8 Nov 2023 17:45:41 -0500 Subject: [PATCH] JointInterpolationPlanner: Check joint bounds (#505) --- core/src/solvers/joint_interpolation.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index f7b717c7..a0184b65 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -77,7 +77,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr // add first point result->addSuffixWayPoint(from->getCurrentState(), 0.0); - if (from->isStateColliding(from_state, jmg->getName())) + if (from->isStateColliding(from_state, jmg->getName()) || !from_state.satisfiesBounds(jmg)) return false; moveit::core::RobotState waypoint(from_state); @@ -86,13 +86,13 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr from_state.interpolate(to_state, t, waypoint); result->addSuffixWayPoint(waypoint, t); - if (from->isStateColliding(waypoint, jmg->getName())) + if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg)) return false; } // add goal point result->addSuffixWayPoint(to_state, 1.0); - if (from->isStateColliding(to_state, jmg->getName())) + if (from->isStateColliding(to_state, jmg->getName()) || !to_state.satisfiesBounds(jmg)) return false; auto timing = props.get("time_parameterization");