void MotionPlanningFrame::saveObjectToAction(apc_msgs::PrimitiveAction& action,
const std::string& object_id,
const robot_state::RobotState& start_state,
const robot_state::RobotState& goal_state)
{
// Clear attached object information.
action.object_id = "";
action.attached_link_id = "";
action.object_trajectory.poses.clear();
if (object_id.empty())
return;
if (action.group_id.empty())
action.group_id = planning_display_->getCurrentPlanningGroup();
Eigen::Affine3d T_object_start_link;
Eigen::Affine3d T_object_goal_link;
std::string eef_link = computeEefLink(action.group_id);
// If the start state and goal state have attached objects
// matching this object ID, we extract the object poses from
// the attached objects instead of from the nearest.
if (start_state.getAttachedBody(object_id)) {
ROS_DEBUG("Saving attached bodies to action");
APC_ASSERT(goal_state.getAttachedBody(object_id),
"Failed to find corresponding attached body %s on goal state", object_id.c_str());
APC_ASSERT(eef_link == start_state.getAttachedBody(object_id)->getAttachedLink()->getName(),
"Mismatch between attached link %s and computed link %s",
start_state.getAttachedBody(object_id)->getAttachedLink()->getName().c_str(),
eef_link.c_str());
APC_ASSERT(eef_link == goal_state.getAttachedBody(object_id)->getAttachedLink()->getName(),
"Mismatch between attached link %s and computed link %s",
goal_state.getAttachedBody(object_id)->getAttachedLink()->getName().c_str(),
eef_link.c_str());
T_object_start_link = start_state.getAttachedBody(object_id)->getFixedTransforms()[0];
T_object_goal_link = goal_state.getAttachedBody(object_id)->getFixedTransforms()[0];
}
// Else, we compute the relative poses to the nearest instance of object ID.
else {
ROS_DEBUG("Saving nearest bodies to action");
KeyPoseMap world_state = computeWorldKeyPoseMap();
Eigen::Affine3d T_object = computeNearestFrameKeyPose(object_id, eef_link, start_state, world_state);
// Get object transform relative to the end-effector link.
Eigen::Affine3d T_start_inv = start_state.getGlobalLinkTransform(eef_link).inverse();
T_object_start_link = T_start_inv * T_object;
Eigen::Affine3d T_goal_inv = goal_state.getGlobalLinkTransform(eef_link).inverse();
T_object_goal_link = T_start_inv * T_object;
}
// Write to object.
action.object_id = object_id;
action.attached_link_id = eef_link;
action.object_trajectory.poses.resize(2);
tf::poseEigenToMsg(T_object_start_link, action.object_trajectory.poses[0]);
tf::poseEigenToMsg(T_object_goal_link, action.object_trajectory.poses[1]);
}
void MotionPlanningFrame::computeAttachNearestObjectToStateMatchingId(const std::string& object_id,
const std::string& group_id,
const KeyPoseMap& world_state,
robot_state::RobotState& robot_state)
{
// Remove any previously attached bodies.
robot_state.clearAttachedBodies();
// Get the end-effector link.
std::string eef_link_id = computeEefLink(group_id);
// Get an object key of object ID in the world.
std::string object_key = computeNearestObjectKey(object_id, eef_link_id, robot_state, world_state);
// Get object transform relative to the end-effector link.
Eigen::Affine3d T_object_world = world_state.find(object_key)->second;
Eigen::Affine3d T_eef_inv = robot_state.getGlobalLinkTransform(eef_link_id).inverse();
Eigen::Affine3d T_object_eef = T_eef_inv * T_object_world;
// Attach object to robot state.
planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
collision_detection::CollisionWorld::ObjectConstPtr object = ps->getWorld()->getObject(object_key);
APC_ASSERT(object,
"Failed to find object key %s in world", object_key.c_str());
std::vector<shapes::ShapeConstPtr> object_shapes = object->shapes_;
EigenSTL::vector_Affine3d object_poses = object->shape_poses_;
for (int i = 0; i < object_poses.size(); i++)
object_poses[i] = T_object_eef * object_poses[0].inverse() * object_poses[i];
moveit_msgs::AttachedCollisionObject aco; // For dummy arguments.
robot_state.attachBody(object_id, object_shapes, object_poses, aco.touch_links, eef_link_id, aco.detach_posture);
robot_state.update();
}
/// Create an OrientationConstraint message that constrains the given link to its
/// current orientation in state within the given tolerance.
moveit_msgs::OrientationConstraint createOrientationConstraint(const std::string& link_name,
const robot_state::RobotState& state,
double tol)
{
if (!state.getRobotModel()->hasLinkModel(link_name))
throw std::runtime_error("Link does not exist");
// Getting link pose from state
geometry_msgs::Pose pose_msg;
Eigen::Affine3d pose = state.getGlobalLinkTransform(link_name);
tf::poseEigenToMsg(pose, pose_msg);
return createOrientationConstraint(link_name, pose_msg, tol);
}