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();
}
void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
const std::string& object_id,
const std::string& link_id,
const geometry_msgs::Pose& pose_object_link)
{
state.clearAttachedBodies();
if (object_id.empty() || link_id.empty())
return;
Eigen::Affine3d T_object_link;
tf::poseMsgToEigen(pose_object_link, T_object_link);
attachObjectToState(state, object_id, link_id, T_object_link);
}
void MotionPlanningFrame::attachObjectToState(robot_state::RobotState& state,
const std::string& object_id,
const std::string& link_id,
const Eigen::Affine3d& T_object_link)
{
state.clearAttachedBodies();
if (object_id.empty())
return;
// Compute the object key.
KeyPoseMap world_state = computeWorldKeyPoseMap();
std::string object_key = computeNearestFrameKey(object_id, link_id, state, world_state);
// Extract the object from the world.
collision_detection::CollisionWorld::ObjectConstPtr object;
std::vector<shapes::ShapeConstPtr> shapes;
EigenSTL::vector_Affine3d poses;
{
planning_scene_monitor::LockedPlanningSceneRO ps = planning_display_->getPlanningSceneRO();
object = ps->getWorld()->getObject(object_key);
}
if (!object)
{
if (!object_id.empty())
ROS_WARN("Failed to attach %s to %s. Object does not exist.",
object_id.c_str(), link_id.c_str());
return;
}
// Compute poses relative to link.
shapes = object->shapes_;
poses = object->shape_poses_;
for (int i = 0; i < poses.size(); i++)
poses[i] = T_object_link * poses[0].inverse() * poses[i];
// Attach collision object to robot.
moveit_msgs::AttachedCollisionObject aco; // For dummy arguments.
state.attachBody(object_id, shapes, poses, aco.touch_links, link_id, aco.detach_posture);
// Update state.
state.update();
}
