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
|
// 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");
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user