mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
525ac10bbf
commit
564f3b0514
@ -109,7 +109,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
const std::string& group = props.get<std::string>("group");
|
const std::string& group = props.get<std::string>("group");
|
||||||
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
|
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group);
|
||||||
if (!jmg) {
|
if (!jmg) {
|
||||||
ROS_WARN_STREAM("MoveRelative: invalid joint model group: " << group);
|
ROS_WARN_STREAM("MoveRelative: invalid joint model group: " << group);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -127,7 +127,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
if (link_name.empty())
|
if (link_name.empty())
|
||||||
link_name = solvers::getEndEffectorLink(jmg);
|
link_name = solvers::getEndEffectorLink(jmg);
|
||||||
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) {
|
||||||
ROS_WARN_STREAM("MoveRelative: no or invalid link name specified: " << link_name);
|
ROS_WARN_STREAM("MoveRelative: no or invalid link name specified: " << link_name);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -211,15 +211,24 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
|
|||||||
props.get<moveit_msgs::Constraints>("path_constraints"));
|
props.get<moveit_msgs::Constraints>("path_constraints"));
|
||||||
|
|
||||||
// min_distance reached?
|
// min_distance reached?
|
||||||
if (success && min_distance > 0.0) {
|
if (min_distance > 0.0) {
|
||||||
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
double distance = 0.0;
|
||||||
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||||
if (use_rotation_distance) {
|
const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint();
|
||||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link);
|
||||||
success = rotation.angle() > min_distance;
|
if (use_rotation_distance) {
|
||||||
} else
|
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||||
success = (reached_pose.translation() - link_pose.translation()).norm() > min_distance;
|
distance = rotation.angle();
|
||||||
} else if (!success && min_distance == 0.0) { // if min_distance is zero, we succeed in any case
|
} else
|
||||||
|
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;
|
success = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user