コード例 #1
0
    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();
    }
コード例 #2
0
    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();
    }