mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
JointInterpolationPlanner: Check joint bounds (#505)
This commit is contained in:
parent
81a5a6bc22
commit
06b7b77e9e
@ -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<TimeParameterizationPtr>("time_parameterization");
|
||||
|
||||
Loading…
Reference in New Issue
Block a user