CartesianPath: return a partial trajectory in case of failure

This commit is contained in:
Robert Haschke 2018-03-13 02:16:35 +01:00
parent 81d88911a9
commit cb38a87a73
2 changed files with 6 additions and 5 deletions

View File

@ -97,7 +97,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
props.get<double>("step_size"), props.get<double>("step_size"),
props.get<double>("jump_threshold"), props.get<double>("jump_threshold"),
isValid); isValid);
if (achieved_fraction >= props.get<double>("min_fraction")) {
if (!trajectory.empty()) {
result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg)); result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));
for (const auto& waypoint : trajectory) for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0); result->addSuffixWayPoint(waypoint, 0.0);
@ -106,9 +107,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
timing.computeTimeStamps(*result, timing.computeTimeStamps(*result,
props.get<double>("max_velocity_scaling_factor"), props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor")); props.get<double>("max_acceleration_scaling_factor"));
return true;
} }
return false;
return achieved_fraction >= props.get<double>("min_fraction");
} }
} } } } } }

View File

@ -86,7 +86,7 @@ void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model)
} }
bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene,
SubTrajectory &trajectory, Direction dir) { SubTrajectory &solution, Direction dir) {
scene = state.scene()->diff(); scene = state.scene()->diff();
assert(scene->getRobotModel()); assert(scene->getRobotModel());
@ -163,7 +163,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
if (success || (robot_trajectory && storeFailures())) { if (success || (robot_trajectory && storeFailures())) {
scene->setCurrentState(robot_trajectory->getLastWayPoint()); scene->setCurrentState(robot_trajectory->getLastWayPoint());
if (dir == BACKWARD) robot_trajectory->reverse(); if (dir == BACKWARD) robot_trajectory->reverse();
trajectory.setTrajectory(robot_trajectory); solution.setTrajectory(robot_trajectory);
} }
return success; return success;