From cb38a87a7346b960bf44de1c0a8f8bcc460fcf2a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 13 Mar 2018 02:16:35 +0100 Subject: [PATCH] CartesianPath: return a partial trajectory in case of failure --- core/src/solvers/cartesian_path.cpp | 7 ++++--- core/src/stages/move_to.cpp | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index 163d0af2..f0c98d01 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -97,7 +97,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, props.get("step_size"), props.get("jump_threshold"), isValid); - if (achieved_fraction >= props.get("min_fraction")) { + + if (!trajectory.empty()) { result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg)); for (const auto& waypoint : trajectory) result->addSuffixWayPoint(waypoint, 0.0); @@ -106,9 +107,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from, timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), props.get("max_acceleration_scaling_factor")); - return true; } - return false; + + return achieved_fraction >= props.get("min_fraction"); } } } } diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 6b547605..79662370 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -86,7 +86,7 @@ void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) } bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, - SubTrajectory &trajectory, Direction dir) { + SubTrajectory &solution, Direction dir) { scene = state.scene()->diff(); assert(scene->getRobotModel()); @@ -163,7 +163,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP if (success || (robot_trajectory && storeFailures())) { scene->setCurrentState(robot_trajectory->getLastWayPoint()); if (dir == BACKWARD) robot_trajectory->reverse(); - trajectory.setTrajectory(robot_trajectory); + solution.setTrajectory(robot_trajectory); } return success;