def executeFold_right(self,gripPt,endPt,color_current='blue',color_next='blue'):
        """
        execute fold with right arm
        """
        print "points",gripPt,endPt
        if (color_current == 'blue'):
            startpoint = self.convert_to_world_frame(gripPt)    
            if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='r',
                                  roll=pi/2,yaw=pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id):
                print "failure"
                return False
        midpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
        midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2
        midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2
        midpoint.point.z = (midpoint.point.z + 0.1)
        pitch = pi/4
        roll = pi/2    
        yaw=pi/2        
        grip=True
        frame= midpoint.header.frame_id        
        
        if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
            print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
            #return False
        endpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
        frame=endpoint.header.frame_id
        if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
            print "failure"
            #return False

        if(color_next == 'blue'):
            initRobot()            
        return True
Esempio n. 2
0
    def execute(self,userdata):
        pt_bl = userdata.bl
        pt_tl = userdata.tl
        pt_br = userdata.br
        pt_tr = userdata.tr
        (bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
        (tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
        (br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
        (tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)

        ctr_l_x = .75*bl_x + .25*tl_x
        ctr_l_y = .75*bl_y + .25*tl_y
        z = bl_z
        yaw = -pi/2
        roll = -pi/2
        pitch = pi/4
        frame = pt_bl.header.frame_id

        ctr_r_x = .75*br_x + .25*tr_x
        ctr_r_y = .75*br_y + .25*tr_y + 0.01 # bit too far right
        alpha = 0.333
        ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x
        ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y
        ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x
        ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y
        up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0

        yaw = pi/2
        roll = pi/2
        pitch = pi/4

        if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y,z=z+0.0025,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame):
            GripUtils.open_gripper("r")
            GripUtils.recall_arm("r")
            return FAILURE
        pitch = pi/2
        if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
            GripUtils.open_gripper("r")
            GripUtils.recall_arm("r")
            return FAILURE

        if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0):
            GripUtils.open_gripper("r")
            GripUtils.recall_arm("r")
            return FAILURE
        z = bl_z+0.01
        pitch = 3*pi/4
        
        if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
            GripUtils.open_gripper("r")
            GripUtils.recall_arm("r")
            return FAILURE
        GripUtils.open_gripper("r")
        if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.06,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0):
            GripUtils.recall_arm("r")
            return FAILURE
        GripUtils.recall_arm("r")
        pr2_say(talk_done)
        return SUCCESS
    def executeFold_left(self,gripPt,endPt,color_current='blue',color_next='blue'):
        """ 
        execute fold with left arm
        """       
        #tmp = self.convert_to_world_frame(endPt)        
        #print "EndPts",tmp.point.x,tmp.point.y,tmp.point.z

        # if blue fold, move arm to gripping position. (if red fold, arm is already holding it at gripping position)
        if (color_current == 'blue'):
            startpoint = self.dupl_PointStamped(gripPt) #self.convert_to_world_frame(gripPt)            
            print "grabbing (x,y,z)",startpoint.point.x,startpoint.point.y,startpoint.point.z
            if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='l',
                                  roll=-pi/2,yaw=-pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id):               
                return False
        midpoint = self.dupl_PointStamped(endPt) #self.convert_to_world_frame(endPt)
        midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2
        midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2
        midpoint.point.z = (midpoint.point.z + 0.1)
        pitch = pi/4
        roll = -pi/2    
        yaw=-pi/2        
        grip=True
        frame= midpoint.header.frame_id        
        #print " going to ",midpoint.point.x,midpoint.point.y,midpoint.point.z

        if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='l',dur=7.5):
            print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
            #return False
        endpoint = self.dupl_PointStamped(endPt) #self.convert_to_world_frame(endPt)
        frame=endpoint.header.frame_id
        if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='l',dur=7.5):
            print "failure"
            #return False

        if(color_next == 'blue'):
            initRobot()            
        return True
Esempio n. 4
0
def fold_cloth(dir='l'):
    print "Folding cloth"
    _cloth_tracker.scoot(-0.1)
    GripUtils.recall_arm("b")
    (waist_l_base,waist_r_base) = get_waist_points()
    now = rospy.Time.now()
    waist_l_base.header.stamp = now
    waist_r_base.header.stamp = now
    _listener.waitForTransform("table_height",waist_l_base.header.frame_id,now,rospy.Duration(5.0))
    waist_l = _listener.transformPoint("table_height",waist_l_base)
    waist_r= _listener.transformPoint("table_height",waist_r_base)
    waist_l.point.z = 0
    waist_r.point.z = 0
    #Grab the waist point
    smooth()
    GripUtils.grab_point(waist_l,arm="l",yaw=-pi/2,x_offset=0.02)
    #Fold over
    ctr_x = (waist_l.point.x + waist_r.point.x)/2+0.02
    ctr_y = (waist_l.point.y + waist_r.point.y)/2
    ctr_z = waist_l.point.y - ctr_y

    GripUtils.go_to(    x=ctr_x,    y=ctr_y,    z=ctr_z,
                        roll=pi/2,  pitch=pi/2, yaw=-pi/2,
                        arm="l",    grip=True,  frame=waist_l.header.frame_id)

    GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=True,y_offset=0.01,x_offset=0.02)
    GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,dur=2.5)
    GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,z_offset=0.05,dur=2.5)
    GripUtils.recall_arm("b")

    #Grab waist
    scoot_amt = 0.2
    _cloth_tracker.scoot(-scoot_amt)
    ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
    ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
    ctr_z = waist_l.point.z
    GripUtils.grab(     x=ctr_x,    y=ctr_y,    z=ctr_z,
                        roll=pi/2,  pitch=pi/4, yaw=0,
                        arm="r",    frame=waist_l.header.frame_id)
    
    sweep_drag_amount = 0.95*TABLE_WIDTH
    sweep_lift_amount = 0.6
    (x,y,z,r,p,yw) = sweep_cloth_with_scoot("r", sweep_drag_amount, TABLE_WIDTH, sweep_lift_amount,roll=pi/2)
    print "y is %f"%y
    print "(%f,%f,%f,%f,%f,%f)"%(x,y,z,r,p,y)
    smooth("l")
    GripUtils.go_to(    x=x,    y=y+PANTS_LENGTH/4-0.03,    z=PANTS_LENGTH/4,
                        roll=r,  pitch=3*pi/8, yaw=yw,
                        arm="r",    grip=True,  frame="table_height", dur=2.5)
    GripUtils.go_to(    x=x,    y=y+PANTS_LENGTH/2-0.03,    z=PANTS_LENGTH/2,
                        roll=r,  pitch=pi/2, yaw=yw,
                        arm="r",    grip=True,  frame="table_height", dur=2.5)
    GripUtils.go_to(    x=x,    y=y+3*PANTS_LENGTH/4-0.03,    z=PANTS_LENGTH/4,
                        roll=r,  pitch=5*pi/8, yaw=yw,
                        arm="r",    grip=True,  frame="table_height", dur=2.5)
    GripUtils.go_to(    x=x,    y=y+PANTS_LENGTH-0.03,    z=0.01,
                        roll=r,  pitch=pi-p, yaw=yw,
                        arm="r",    grip=True,  frame="table_height", dur=2.5)
    GripUtils.go_to(    x=x,    y=y+PANTS_LENGTH+0.05,    z=0.01,
                        roll=r,  pitch=pi-p, yaw=yw,
                        arm="r",    grip=False,  frame="table_height", dur=2.5)
    GripUtils.go_to(    x=x,    y=y+PANTS_LENGTH+0.05,    z=0.1,
                        roll=r,  pitch=pi-p, yaw=yw,
                        arm="r",    grip=False,  frame="table_height", dur=2.5)
    
    GripUtils.recall_arm("r")
    smooth()
    _cloth_tracker.scoot(scoot_amt+0.08)
    return True
Esempio n. 5
0
def fold_cloth(dir='l'):
    print "Folding cloth"
    _cloth_tracker.scoot(-0.1)
    GripUtils.recall_arm("b")
    (waist_l_base, waist_r_base) = get_waist_points()
    now = rospy.Time.now()
    waist_l_base.header.stamp = now
    waist_r_base.header.stamp = now
    _listener.waitForTransform("table_height", waist_l_base.header.frame_id,
                               now, rospy.Duration(5.0))
    waist_l = _listener.transformPoint("table_height", waist_l_base)
    waist_r = _listener.transformPoint("table_height", waist_r_base)
    waist_l.point.z = 0
    waist_r.point.z = 0
    #Grab the waist point
    smooth()
    GripUtils.grab_point(waist_l, arm="l", yaw=-pi / 2, x_offset=0.02)
    #Fold over
    ctr_x = (waist_l.point.x + waist_r.point.x) / 2 + 0.02
    ctr_y = (waist_l.point.y + waist_r.point.y) / 2
    ctr_z = waist_l.point.y - ctr_y

    GripUtils.go_to(x=ctr_x,
                    y=ctr_y,
                    z=ctr_z,
                    roll=pi / 2,
                    pitch=pi / 2,
                    yaw=-pi / 2,
                    arm="l",
                    grip=True,
                    frame=waist_l.header.frame_id)

    GripUtils.go_to_pt(waist_r,
                       arm="l",
                       roll=pi / 2,
                       pitch=3 * pi / 4,
                       yaw=-pi / 2,
                       grip=True,
                       y_offset=0.01,
                       x_offset=0.02)
    GripUtils.go_to_pt(waist_r,
                       arm="l",
                       roll=pi / 2,
                       pitch=3 * pi / 4,
                       yaw=-pi / 2,
                       grip=False,
                       y_offset=-0.05,
                       x_offset=0.02,
                       dur=2.5)
    GripUtils.go_to_pt(waist_r,
                       arm="l",
                       roll=pi / 2,
                       pitch=3 * pi / 4,
                       yaw=-pi / 2,
                       grip=False,
                       y_offset=-0.05,
                       x_offset=0.02,
                       z_offset=0.05,
                       dur=2.5)
    GripUtils.recall_arm("b")

    #Grab waist
    scoot_amt = 0.2
    _cloth_tracker.scoot(-scoot_amt)
    ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
    ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
    ctr_z = waist_l.point.z
    GripUtils.grab(x=ctr_x,
                   y=ctr_y,
                   z=ctr_z,
                   roll=pi / 2,
                   pitch=pi / 4,
                   yaw=0,
                   arm="r",
                   frame=waist_l.header.frame_id)

    sweep_drag_amount = 0.95 * TABLE_WIDTH
    sweep_lift_amount = 0.6
    (x, y, z, r, p, yw) = sweep_cloth_with_scoot("r",
                                                 sweep_drag_amount,
                                                 TABLE_WIDTH,
                                                 sweep_lift_amount,
                                                 roll=pi / 2)
    print "y is %f" % y
    print "(%f,%f,%f,%f,%f,%f)" % (x, y, z, r, p, y)
    smooth("l")
    GripUtils.go_to(x=x,
                    y=y + PANTS_LENGTH / 4 - 0.03,
                    z=PANTS_LENGTH / 4,
                    roll=r,
                    pitch=3 * pi / 8,
                    yaw=yw,
                    arm="r",
                    grip=True,
                    frame="table_height",
                    dur=2.5)
    GripUtils.go_to(x=x,
                    y=y + PANTS_LENGTH / 2 - 0.03,
                    z=PANTS_LENGTH / 2,
                    roll=r,
                    pitch=pi / 2,
                    yaw=yw,
                    arm="r",
                    grip=True,
                    frame="table_height",
                    dur=2.5)
    GripUtils.go_to(x=x,
                    y=y + 3 * PANTS_LENGTH / 4 - 0.03,
                    z=PANTS_LENGTH / 4,
                    roll=r,
                    pitch=5 * pi / 8,
                    yaw=yw,
                    arm="r",
                    grip=True,
                    frame="table_height",
                    dur=2.5)
    GripUtils.go_to(x=x,
                    y=y + PANTS_LENGTH - 0.03,
                    z=0.01,
                    roll=r,
                    pitch=pi - p,
                    yaw=yw,
                    arm="r",
                    grip=True,
                    frame="table_height",
                    dur=2.5)
    GripUtils.go_to(x=x,
                    y=y + PANTS_LENGTH + 0.05,
                    z=0.01,
                    roll=r,
                    pitch=pi - p,
                    yaw=yw,
                    arm="r",
                    grip=False,
                    frame="table_height",
                    dur=2.5)
    GripUtils.go_to(x=x,
                    y=y + PANTS_LENGTH + 0.05,
                    z=0.1,
                    roll=r,
                    pitch=pi - p,
                    yaw=yw,
                    arm="r",
                    grip=False,
                    frame="table_height",
                    dur=2.5)

    GripUtils.recall_arm("r")
    smooth()
    _cloth_tracker.scoot(scoot_amt + 0.08)
    return True
Esempio n. 6
0
    def execute(self, userdata):
        pt_bl = userdata.bl
        pt_tl = userdata.tl
        pt_br = userdata.br
        pt_tr = userdata.tr
        (bl_x, bl_y, bl_z) = (pt_bl.point.x, pt_bl.point.y, pt_bl.point.z)
        (tl_x, tl_y, tl_z) = (pt_tl.point.x, pt_tl.point.y, pt_tl.point.z)
        (br_x, br_y, br_z) = (pt_br.point.x, pt_br.point.y, pt_br.point.z)
        (tr_x, tr_y, tr_z) = (pt_tr.point.x, pt_tr.point.y, pt_tr.point.z)

        ctr_l_x = .75 * bl_x + .25 * tl_x
        #Make more centered
        ctr_l_x -= 0.01
        ctr_l_y = .75 * bl_y + .25 * tl_y
        z = bl_z
        yaw = -pi / 2
        roll = -pi / 2
        pitch = pi / 4
        frame = pt_bl.header.frame_id
        if not GripUtils.grab(x=ctr_l_x,
                              y=ctr_l_y,
                              z=z,
                              roll=roll,
                              yaw=yaw,
                              pitch=pitch,
                              arm="l",
                              frame=frame):
            return FAILURE
        ctr_r_x = .75 * br_x + .25 * tr_x
        #Make more centered
        ctr_r_x -= 0.01
        ctr_r_y = .75 * br_y + .25 * tr_y
        alpha = 0.333
        ctr_ml_x = (1 - alpha) * ctr_l_x + alpha * ctr_r_x
        ctr_ml_y = (1 - alpha) * ctr_l_y + alpha * ctr_r_y
        ctr_mr_x = (1 - alpha) * ctr_r_x + alpha * ctr_l_x
        ctr_mr_y = (1 - alpha) * ctr_r_y + alpha * ctr_l_y
        up_z = z + sqrt((ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0
        pitch = pi / 2
        if not GripUtils.go_to(x=ctr_ml_x,
                               y=ctr_ml_y,
                               z=up_z,
                               roll=roll,
                               yaw=yaw,
                               pitch=pitch,
                               arm="l",
                               frame=frame,
                               grip=True,
                               dur=5.0):
            return FAILURE
        if not GripUtils.go_to(x=(ctr_ml_x + ctr_mr_x) / 2.0,
                               y=(ctr_ml_y + ctr_mr_y + 0.02) / 2.0,
                               z=(up_z + bl_z) / 2.0,
                               roll=roll,
                               yaw=yaw,
                               pitch=(pitch + 3 * pi / 4) / 2.0,
                               arm="l",
                               frame=frame,
                               grip=True,
                               dur=5.0):
            return FAILURE
        z = bl_z
        pitch = 3 * pi / 4

        if not GripUtils.go_to(x=ctr_mr_x,
                               y=ctr_mr_y + 0.02,
                               z=z,
                               roll=roll,
                               yaw=yaw,
                               pitch=pitch,
                               arm="l",
                               frame=frame,
                               grip=True,
                               dur=5.0):
            return FAILURE
        yaw = pi / 2
        roll = pi / 2
        pitch = pi / 4
        GripUtils.open_gripper("l")
        if not GripUtils.go_to(x=ctr_mr_x,
                               y=ctr_mr_y - 0.05,
                               z=z + 0.02,
                               roll=roll,
                               yaw=yaw,
                               pitch=pitch,
                               arm="l",
                               frame=frame,
                               grip=False,
                               dur=1.0):
            return FAILURE
        GripUtils.recall_arm("l")
        if not GripUtils.grab(x=ctr_r_x,
                              y=ctr_r_y - 0.01,
                              z=z,
                              roll=roll,
                              yaw=yaw,
                              pitch=pitch,
                              arm="r",
                              frame=frame):
            return FAILURE
        pitch = pi / 2
        if not GripUtils.go_to(x=ctr_mr_x,
                               y=ctr_mr_y - 0.02,
                               z=up_z,
                               roll=roll,
                               yaw=yaw,
                               pitch=pitch,
                               arm="r",
                               frame=frame,
                               grip=True,
                               dur=5.0):
            return FAILURE

        if not GripUtils.go_to(x=(ctr_mr_x + ctr_ml_x) / 2.0,
                               y=(ctr_mr_y - 0.02 + ctr_ml_y - 0.02) / 2.0,
                               z=(up_z + bl_z + 0.01) / 2.0,
                               roll=roll,
                               yaw=yaw,
                               pitch=(pitch + 3 * pi / 4) / 2.0,
                               arm="r",
                               frame=frame,
                               grip=True,
                               dur=5.0):
            return FAILURE
        z = bl_z + 0.01
        pitch = 3 * pi / 4

        if not GripUtils.go_to(x=ctr_ml_x,
                               y=ctr_ml_y - 0.02,
                               z=z,
                               roll=roll,
                               yaw=yaw,
                               pitch=pitch,
                               arm="r",
                               frame=frame,
                               grip=True,
                               dur=5.0):
            return FAILURE
        GripUtils.open_gripper("r")
        if not GripUtils.go_to(x=ctr_ml_x,
                               y=ctr_ml_y + 0.05,
                               z=z + 0.02,
                               roll=roll,
                               yaw=yaw,
                               pitch=pitch,
                               arm="r",
                               frame=frame,
                               grip=False,
                               dur=1.0):
            return FAILURE
        GripUtils.recall_arm("r")
        """
        if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
                                        ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
                                        ,dur=3.0):
            return FAILURE
        if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
                                        ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
                                        ,dur=3.0):
            return FAILURE
        GripUtils.close_gripper("b")
        if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame
                                        ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame
                                        ,dur=3.0):
            return FAILURE
        """
        return SUCCESS
Esempio n. 7
0
    def execute(self,userdata):
        pt_bl = userdata.bl
        pt_tl = userdata.tl
        pt_br = userdata.br
        pt_tr = userdata.tr
        (bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z)
        (tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z)
        (br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z)
        (tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z)
        
        ctr_l_x = .75*bl_x + .25*tl_x
        #Make more centered
        ctr_l_x -= 0.01
        ctr_l_y = .75*bl_y + .25*tl_y
        z = bl_z
        yaw = -pi/2
        roll = -pi/2
        pitch = pi/4
        frame = pt_bl.header.frame_id
        if not GripUtils.grab(x=ctr_l_x,y=ctr_l_y,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame):
            return FAILURE
        ctr_r_x = .75*br_x + .25*tr_x
        #Make more centered
        ctr_r_x -= 0.01
        ctr_r_y = .75*br_y + .25*tr_y
        alpha = 0.333
        ctr_ml_x = (1-alpha)*ctr_l_x + alpha*ctr_r_x
        ctr_ml_y = (1-alpha)*ctr_l_y + alpha*ctr_r_y
        ctr_mr_x = (1-alpha)*ctr_r_x + alpha*ctr_l_x
        ctr_mr_y = (1-alpha)*ctr_r_y + alpha*ctr_l_y
        up_z = z+sqrt( (ctr_l_x - ctr_r_x)**2 + (ctr_l_y - ctr_r_y)**2) / 3.0
        pitch = pi/2
        if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0):
            return FAILURE
        if not GripUtils.go_to(x=(ctr_ml_x+ctr_mr_x)/2.0,y=(ctr_ml_y+ctr_mr_y+0.02)/2.0,z=(up_z+bl_z)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="l",frame=frame,grip=True,dur=5.0):
            return FAILURE
        z = bl_z
        pitch = 3*pi/4
        
        if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y+0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=True,dur=5.0):
            return FAILURE
        yaw = pi/2
        roll = pi/2
        pitch = pi/4
        GripUtils.open_gripper("l")
        if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="l",frame=frame,grip=False,dur=1.0):
            return FAILURE
        GripUtils.recall_arm("l")
        if not GripUtils.grab(x=ctr_r_x,y=ctr_r_y-0.01,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame):
            return FAILURE
        pitch = pi/2
        if not GripUtils.go_to(x=ctr_mr_x,y=ctr_mr_y-0.02,z=up_z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
            return FAILURE

        if not GripUtils.go_to(x=(ctr_mr_x+ctr_ml_x)/2.0,y=(ctr_mr_y-0.02+ctr_ml_y-0.02)/2.0,z=(up_z+bl_z+0.01)/2.0,roll=roll,yaw=yaw,pitch=(pitch+3*pi/4)/2.0,arm="r",frame=frame,grip=True,dur=5.0):
            return FAILURE
        z = bl_z+0.01
        pitch = 3*pi/4
        
        if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y-0.02,z=z,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=True,dur=5.0):
            return FAILURE
        GripUtils.open_gripper("r")
        if not GripUtils.go_to(x=ctr_ml_x,y=ctr_ml_y+0.05,z=z+0.02,roll=roll,yaw=yaw,pitch=pitch,arm="r",frame=frame,grip=False,dur=1.0):
            return FAILURE
        GripUtils.recall_arm("r")
        
        """
        if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y+0.06,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
                                        ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y-0.03,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
                                        ,dur=3.0):
            return FAILURE
        if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.01,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=False,frame_l=frame
                                        ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.01,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=False,frame_r=frame
                                        ,dur=3.0):
            return FAILURE
        GripUtils.close_gripper("b")
        if not GripUtils.go_to_multi (x_l=ctr_ml_x+0.03,y_l=ctr_ml_y-0.05,z_l=z+0.1,roll_l=-pi/2,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,frame_l=frame
                                        ,x_r=ctr_mr_x+0.03,y_r=ctr_mr_y+0.05,z_r=z+0.1,roll_r= pi/2,pitch_r=pi/4,yaw_r= pi/2,grip_r=True,frame_r=frame
                                        ,dur=3.0):
            return FAILURE
        """
        return SUCCESS