mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Gracefully handle NULL robot_trajectory (#469)
This commit is contained in:
parent
76d293863b
commit
78da3e46e6
@ -281,35 +281,37 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
success =
|
||||
planner_->plan(state.scene(), *link, offset, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
if (robot_trajectory) { // the following requires a robot_trajectory returned from planning
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link) * offset;
|
||||
|
||||
double distance = 0.0;
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
|
||||
double distance = 0.0;
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * ik_pose_world.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - ik_pose_world.translation()).norm();
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
success = distance >= min_distance;
|
||||
if (!success) {
|
||||
char msg[100];
|
||||
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
||||
solution.setComment(msg);
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
success = distance >= min_distance;
|
||||
if (!success) {
|
||||
char msg[100];
|
||||
snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance);
|
||||
solution.setComment(msg);
|
||||
}
|
||||
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||
success = true;
|
||||
} else if (!success)
|
||||
solution.setComment("failed to move full distance");
|
||||
|
||||
// visualize plan
|
||||
auto ns = props.get<std::string>("marker_ns");
|
||||
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
|
||||
linear, distance);
|
||||
}
|
||||
} else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
||||
success = true;
|
||||
} else if (!success)
|
||||
solution.setComment("failed to move full distance");
|
||||
|
||||
// visualize plan
|
||||
auto ns = props.get<std::string>("marker_ns");
|
||||
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), ik_pose_world, reached_pose,
|
||||
linear, distance);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user