fix joint_interpolation

- return a trajectory in any case (even if there is no motion needed)
- check feasability of goal pose
This commit is contained in:
Robert Haschke 2020-03-04 10:05:36 +01:00
parent 9ee653453d
commit 8debe68f99

View File

@ -64,16 +64,16 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
const moveit::core::RobotState& to_state = to->getCurrentState(); const moveit::core::RobotState& to_state = to->getCurrentState();
for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels()) for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels())
d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm)); d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm));
if (d < 1e-6)
return true; // return empty result trajectory: no motion needed
result = std::make_shared<robot_trajectory::RobotTrajectory>(from->getRobotModel(), jmg); result = std::make_shared<robot_trajectory::RobotTrajectory>(from->getRobotModel(), jmg);
// 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()))
return false;
moveit::core::RobotState waypoint(from_state); moveit::core::RobotState waypoint(from_state);
double delta = props.get<double>("max_step") / d; double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
for (double t = delta; t < 1.0; t += delta) { for (double t = delta; t < 1.0; t += delta) {
from_state.interpolate(to_state, t, waypoint); from_state.interpolate(to_state, t, waypoint);
result->addSuffixWayPoint(waypoint, t); result->addSuffixWayPoint(waypoint, t);
@ -83,7 +83,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
} }
// add goal point // add goal point
result->addSuffixWayPoint(to->getCurrentState(), 1.0); result->addSuffixWayPoint(to_state, 1.0);
if (from->isStateColliding(to_state, jmg->getName()))
return false;
// add timing, TODO: use a generic method to add timing via plugins // add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing; trajectory_processing::IterativeParabolicTimeParameterization timing;