JointInterpolationPlanner: Check joint bounds (#505)

This commit is contained in:
Michael Wiznitzer 2023-11-08 17:45:41 -05:00 committed by GitHub
parent 81a5a6bc22
commit 06b7b77e9e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -77,7 +77,7 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
// add first point // add first point
result->addSuffixWayPoint(from->getCurrentState(), 0.0); 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; return false;
moveit::core::RobotState waypoint(from_state); moveit::core::RobotState waypoint(from_state);
@ -86,13 +86,13 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
from_state.interpolate(to_state, t, waypoint); from_state.interpolate(to_state, t, waypoint);
result->addSuffixWayPoint(waypoint, t); result->addSuffixWayPoint(waypoint, t);
if (from->isStateColliding(waypoint, jmg->getName())) if (from->isStateColliding(waypoint, jmg->getName()) || !waypoint.satisfiesBounds(jmg))
return false; return false;
} }
// add goal point // add goal point
result->addSuffixWayPoint(to_state, 1.0); 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; return false;
auto timing = props.get<TimeParameterizationPtr>("time_parameterization"); auto timing = props.get<TimeParameterizationPtr>("time_parameterization");