C++ RobotState::getGlobalLinkTransform方法代码示例

    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);
}