fix MoveRelative

Ignore success of planner_->plan() when min_distance is specified (and >= 0).
In this case, compute the achieved distance myself.
This commit is contained in:
Robert Haschke 2018-04-09 15:44:27 +02:00
parent 525ac10bbf
commit 564f3b0514

View File

@ -211,15 +211,24 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
props.get<moveit_msgs::Constraints>("path_constraints"));
// min_distance reached?
if (success && min_distance > 0.0) {
if (min_distance > 0.0) {
double distance = 0.0;
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
success = rotation.angle() > min_distance;
distance = rotation.angle();
} else
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
distance = (reached_pose.translation() - link_pose.translation()).norm();
}
success = distance >= min_distance;
if (!success) {
char msg[100];
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
trajectory.setName(msg);
}
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
success = true;
}