mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
CartesianPath: return a partial trajectory in case of failure
This commit is contained in:
parent
81d88911a9
commit
cb38a87a73
@ -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");
|
||||||
}
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -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;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user