mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveRelative: allow zero min_distance
This commit is contained in:
parent
0b8bf2b8b7
commit
b1fac6eed0
@ -214,10 +214,12 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
success = rotation.angle() > min_distance;
|
success = rotation.angle() > min_distance;
|
||||||
} else
|
} else
|
||||||
success = (reached_pose.translation() - link_pose.translation()).norm() > min_distance;
|
success = (reached_pose.translation() - link_pose.translation()).norm() > min_distance;
|
||||||
|
} else if (!success && min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||||
|
success = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// store result
|
// store result
|
||||||
if (success || (robot_trajectory && storeFailures())) {
|
if (robot_trajectory && (success || 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);
|
trajectory.setTrajectory(robot_trajectory);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user