Пример #1
0
def plate_grasps(plate_pose_stamped, grasp_reference_frame):
    plate_pose = tl.transform_pose(grasp_reference_frame,
                                   plate_pose_stamped.header.frame_id,
                                   plate_pose_stamped.pose)
    #this is the object in the frame of the gripper
    grasp_pose = Pose()
    grasps = []
    rospy.loginfo('plate pose = \n' + str(plate_pose))
    origin = Pose()
    origin.orientation.w = 1.0
    grasp_pose.position.x = GRASP_DIST
    for i in range(NPLATEGRASPS):
        alpha = 2.0 * np.pi / NPLATEGRASPS * i
        grasp_pose.orientation = gt.multiply_quaternions(
            Quaternion(x=0.707, w=0.707),
            Quaternion(z=np.sin(alpha / 2.0), w=np.cos(alpha / 2.0)))
        grasp = om.Grasp()

        grasp.grasp_pose = gt.transform_pose(
            gt.inverse_transform_pose(origin, grasp_pose), plate_pose)
        grasp.min_approach_distance = 0.0
        grasp.desired_approach_distance = 0.3
        grasps.append(grasp)

    return grasps
Пример #2
0
def get_gripper(arm, obj, goal):
    '''
    finds the pose of the gripper for an object in its hand at the given goal pose
    
    @type obj: arm_navigation_msgs.msg.attached_collision_object
    @param obj: the object being held by the gripper
    @type goal: PoseStamped
    @param goal: goal pose for the object in torso_lift_link
    
    @rtype PoseStamped
    @returns pose of the gripper for the given object at the given goal 
    '''
    #    print 'Getting the gripper.  Oh yeah'

    # first get the object's current pose
    obj_pose = tf.transform_pose("/torso_lift_link", obj.object.header.frame_id, \
                                     obj.object.poses[0])
    obj_pose_stamped = PoseStamped()
    obj_pose_stamped.header.frame_id = "/torso_lift_link"
    obj_pose_stamped.pose = obj_pose
    #        self.make_marker(obj_pose_stamped, namespace='obj_pose', color=(1.0,1.0,1.0,0.8))

    # next get the gripper's current pose
    gripper_pose_stamped = arm.get_hand_frame_pose(\
        frame_id="/torso_lift_link")
    gripper_pose = gripper_pose_stamped.pose
    #        self.make_marker(obj_pose_stamped, namespace='gripper_pose', color=(1.0,0.0,0.0,0.8), mtype=Marker.ARROW)

    # the position of the object when the wrist is the origin
    # grasp = gripper_pose^{-1}*obj_pose
    grasp = gt.inverse_transform_pose(obj_pose, gripper_pose)

    #we need the position of the gripper when the object is at goal
    #gripper_goal = goal*grasp^{-1}

    origin = Pose()
    origin.orientation.w = 1

    #this is grasp^{-1}
    grasp_inv = gt.inverse_transform_pose(origin, grasp)

    #this is goal*grasp_inv
    gripper_goal = gt.transform_pose(grasp_inv, goal.pose)
    gripper_ps = PoseStamped()
    gripper_ps.header.frame_id = goal.header.frame_id
    gripper_ps.pose = gripper_goal

    return gripper_ps
Пример #3
0
    def transform_pose(self, new_frame_id, old_frame_id, pose, robot_state):
        """
        Transforms a pose defined in old_frame_id into new_frame_id according to robot state.
        
        **Args:**

            **new_frame_id (string):** new frame id

            **old_frame_id (string):** old frame id
            
            **pose (geometry_msgs.msg.Pose):** pose defined in old_frame_id

            **robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation
          
        **Returns:**
            A geometry_msgs.msg.Pose defined in new_frame_id

        **Raises:**

            **exceptions.TransformStateError:** if there is an error getting the transform
        """
        trans = self.get_transform(old_frame_id, new_frame_id, robot_state)
        return gt.transform_pose(pose, conv.transform_to_pose(trans.transform))
    def transform_pose(self, new_frame_id, old_frame_id, pose, robot_state):
        '''
        Transforms a pose defined in old_frame_id into new_frame_id according to robot state.
        
        **Args:**

            **new_frame_id (string):** new frame id

            **old_frame_id (string):** old frame id
            
            **pose (geometry_msgs.msg.Pose):** pose defined in old_frame_id

            **robot_state (arm_navigation_msg.msg.RobotState):** Robot state to use for transformation
          
        **Returns:**
            A geometry_msgs.msg.Pose defined in new_frame_id

        **Raises:**

            **exceptions.TransformStateError:** if there is an error getting the transform
        '''
        trans = self.get_transform(old_frame_id, new_frame_id, robot_state)
        return gt.transform_pose(pose, conv.transform_to_pose(trans.transform))
Пример #5
0
def free_spots_from_dimensions(table, object_dims, blocking_objs, res=0.1):
    '''
    Currently works only with a single shape

    Assumes table is in xy plane
    '''
    #find the lower left corner of the table in the header frame
    #we want everything relative to this point
    table_corners = gt.bounding_box_corners(table.shapes[0])
    lower_left = Pose()
    lower_left.position = gt.list_to_point(table_corners[0])
    lower_left.orientation.w = 1.0
    #rospy.loginfo('Table corners are:')
    #for c in table_corners: rospy.loginfo('\n'+str(c))
    #rospy.loginfo('Lower left =\n'+str(lower_left))
    #rospy.loginfo('Table header =\n'+str(table.header))
    #rospy.loginfo('Table pose =\n'+str(table.poses[0]))
    #this is the position of the minimum x, minimum y point in the table's header frame
    table_box_origin = gt.transform_pose(lower_left, table.poses[0])
    tr = gt.inverse_transform_list(gt.transform_list(table_corners[-1], table.poses[0]), table_box_origin)
    table_dims = (tr[0], tr[1])
    tbos = PoseStamped()
    tbos.header = table.header
    tbos.pose = table_box_origin
    marray.markers.append(vt.marker_at(tbos, ns='table_origin', mtype=Marker.CUBE, r=1.0))
    max_box = PointStamped()
    max_box.header = table.header
    max_box.point = gt.list_to_point(gt.transform_list([table_dims[0], table_dims[1], 0], table_box_origin)) 
    marray.markers.append(vt.marker_at_point(max_box, ns='table_max', mtype=Marker.CUBE, r=1.0))

    #rospy.loginfo('Table box origin is '+str(table_box_origin)+' dimensions are '+str(table_dims))

                
    locs_on_table =  _get_feasible_locs(table_dims, object_dims, res)
    for i, l in enumerate(locs_on_table):
        pt = Point()
        pt.x = l[0]
        pt.y = l[1]
        mpt = PointStamped()
        mpt.header = table.header
        mpt.point = gt.transform_point(pt, table_box_origin)
        marray.markers.append(vt.marker_at_point(mpt, mid=i, ns='locations', r=1.0, g=1.0, b=0.0))

    feasible_locs = []
    #check that these points really are on the table
    for i, l in enumerate(locs_on_table):
        pt = Point()
        pt.x = l[0]
        pt.y = l[1]
        #this point is now defined relative to the origin of the table (rather than its minimum x, minimum y point)
        table_pt = gt.inverse_transform_point(gt.transform_point(pt, table_box_origin), table.poses[0])
        if point_on_table(table_pt, table.shapes[0]):
            feasible_locs.append(l)
            marray.markers[i+2].color.r = 0.0
            marray.markers[i+2].color.b = 1.0
    rospy.loginfo('Testing '+str(len(feasible_locs))+' locations')
    if not feasible_locs:
        return feasible_locs

    forbidden=[]
    for i, o in enumerate(blocking_objs):
        ofcs = gt.bounding_box_corners(o.shapes[0])
        objpose = tl.transform_pose(table.header.frame_id, o.header.frame_id, o.poses[0])
        hfcs = [gt.transform_list(c, objpose) for c in ofcs]
        tfcs = [gt.inverse_transform_list(c, table_box_origin) for c in hfcs]
        oxmax = max([c[0] for c in tfcs])
        oxmin = min([c[0] for c in tfcs])
        oymax = max([c[1] for c in tfcs])
        oymin = min([c[1] for c in tfcs])
        forbidden.append(((oxmin, oymin), (oxmax - oxmin, oymax - oymin)))
        #rospy.loginfo('\n'+str(forbidden[-1]))
        ps = PoseStamped()
        ps.header = table.header
        ps.pose = objpose
        ps = PoseStamped()
        ps.header = table.header
        ps.pose.position = gt.list_to_point(gt.transform_list([oxmin, oymin, 0], table_box_origin))
        ps.pose.orientation.w = 1.0
        marray.markers.append(vt.marker_at(ps, ns='forbidden', mid=i, r=1.0, b=0.0))
        

    # Remove forbidden rectangles
    for (bottom_left, dims) in forbidden:
        _remove_feasible_locs(feasible_locs, object_dims,
                              bottom_left,
                              _add(bottom_left, dims),
                              res)
    rospy.loginfo('There are '+str(len(feasible_locs))+' possible locations')
    obj_poses = []
    for i, fl in enumerate(feasible_locs):
        table_frame_pose = Pose()
        table_frame_pose.position.x = fl[0] + object_dims[0]/2.0
        table_frame_pose.position.y = fl[1] + object_dims[1]/2.0
        table_frame_pose.orientation.w = 1.0
        pose = PoseStamped()
        pose.header = copy.deepcopy(table.header)
        pose.pose = gt.transform_pose(table_frame_pose, table_box_origin)
        obj_poses.append(pose)
        pt = PointStamped()
        pt.header = table.header
        pt.point = pose.pose.position
        marray.markers.append(vt.marker_at_point(pt, mid=i, ns='final_locations', g=1.0, b=0.0))

    #rospy.loginfo('Object poses are:')
    #for op in obj_poses: rospy.loginfo(str(op))
    for i in range(10):
        vpub.publish(marray)
        rospy.sleep(0.1)
    return obj_poses
Пример #6
0
    def add_spatula(self, arm):
        spatula = CollisionObject()
        spatula.id = "spatula"
        spatula.header.frame_id = self.wi.world_frame
        spatula.operation.operation = spatula.operation.ADD

        paddle = Shape()
        handle = Shape()
        paddle.type = paddle.BOX
        paddle.dimensions = [0.11, 0.12, 0.005]
        handle.type = handle.CYLINDER
        handle.dimensions = [0.02, 0.24]

        paddle_pose = Pose()
        handle_pose = Pose()
        paddle_pose.position.y = paddle.dimensions[1] / 2.0
        paddle_pose.orientation.w = 1.0

        angle = np.pi / 5.0
        handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
            np.pi / 2.0 - angle)
        handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
            np.pi / 2.0 - angle)
        handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
        handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)

        spatula.shapes = [paddle, handle]
        spatula.poses = [paddle_pose, handle_pose]

        #this is the grasp transformation
        pos_on_handle = handle.dimensions[1] - 0.1
        inv_grasp = Transform()
        grasp = RigidGrasp()
        #really should be calculating this...
        inv_grasp.translation.y = GRIPPER_LENGTH
        inv_grasp.translation.z = pos_on_handle / 2.0
        #flip 90 degrees
        inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
        inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
        g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
        origin = Pose()
        origin.orientation.w = 1.0
        grasp.transform = pose_to_transform(
            gt.inverse_transform_pose(origin, g))
        grasp.touch_links = [arm[0] + '_end_effector']
        grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
        grasp.min_approach_distance = 0
        grasp.desired_approach_distance = 0.15
        grasp.min_distance_from_surface = -1

        spat_p = copy.deepcopy(spatula)

        wtrans = Pose()
        wtrans.orientation.x = np.sin(angle / 2.0)
        wtrans.orientation.w = np.cos(angle / 2.0)
        if self.world == -1:
            wtrans.position.x = 3
            wtrans.position.y = -2.8
            wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['far_corner']
        elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
            wtrans.position.x = -1.7
            wtrans.position.y = 2
            wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
            ss = ['door_table']
        else:
            wtrans.position.x = 0.6
            wtrans.position.y = -0.3
            wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
                0]
            ss = ['center_table']
            if self.world == -4 or self.world == -5:
                wtrans.position.y = 0
                rot = Quaternion()
                rot.z = np.sin(np.pi / 2.0)
                rot.w = np.cos(np.pi / 2.0)
                wtrans.orientation = gt.multiply_quaternions(
                    rot, wtrans.orientation)
                if self.world == -5:
                    wtrans.position.x = 0

        for i in range(len(spat_p.poses)):
            spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)

        self.wi.add_object(spat_p)
        return ObjectType(type="SpatulaObject",
                          collision_object=spatula,
                          parameters=ss,
                          numeric_parameters=paddle.dimensions +
                          handle.dimensions + [angle]), [grasp]