Python GripUtils.grab_points方法代码示例

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_points [as 别名]
 def execute(self,userdata):
     pt_bl = userdata.bl
     pt_tl = userdata.tl
     pt_br = userdata.br
     pt_tr = userdata.tr
     #if not GripUtils.go_to_pts(pt_tl, -pi/2, -pi/3, pi/4, pt_tr, pi/2, pi/3, pi/4, x_offset_l=-0.04,\
     #        x_offset_r=-0.04):
     #    return FAILURE
     #FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE
     '''
     if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04):
         return FAILURE
     if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04):
         return FAILURE
     '''
     #FIXME: For some reason large offsets required, #DEBUG
     if not GripUtils.grab_points(pt_tl,roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.05, z_offset_l=-0.0025
                                 ,point_r=pt_tr,roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.06,
                                 y_offset_r=0.00, y_offset_l=-0.00, z_offset_r=0.01):
         return FAILURE
     (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)
     x_l = (tl_x+bl_x)/2.0
     y_l = (tl_y+bl_y)/2.0
     z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0
     x_r = (tr_x+br_x)/2.0
     y_r = (tr_y+br_y)/2.0
     z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0
     pitch_l = pitch_r = pi/4
     roll_l = 0
     roll_r = 0
     yaw_l=-pi/2
     yaw_r= pi/2
     grip_l=grip_r=True
     frame_l=frame_r = pt_bl.header.frame_id
     if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
                                     ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
                                     ,dur=4.0):
         return_val = FAILURE
     print "Folding down!"
     x_l = bl_x + 0.02 # overshoots fold down
     x_r = br_x + 0.02
     y_l = bl_y-0.01
     y_r = br_y+0.01
     z_l = z_r = bl_z + 0.02
     yaw_l = -3*pi/4
     yaw_r = 3*pi/4
     pitch_l=pitch_r = pi/4
     roll_l = pi/2
     roll_r = -pi/2
     GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
                                     ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
                                     ,dur=7.5)
     GripUtils.recall_arm("b")
     pr2_say(talk_pose2)
     return SUCCESS

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_points [as 别名]
    def executeDrag(self,gripPts,endPts,color='blue'):
        """
        drag from gripPts to endPts
        """          
        startpoints = []
        for vertex in gripPts:
            pt_world = self.convert_to_world_frame(vertex)
            now = rospy.Time.now()
            self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))            
            pt_transformed = self.listener.transformPoint("base_footprint",pt_world)                        
            startpoints.append(pt_transformed)

        

        #print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z
        

        if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True,
                                     point_r=startpoints[1],roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005):
            print "failure"
            return False
        else:
            print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z
            print "done"

        midpoints = []
        for vertex in endPts:
            pt_world = self.dupl_PointStamped(vertex)#self.convert_to_world_frame(vertex)            
            midpoints.append(pt_world)

        frame_l = frame_r = midpoints[0].header.frame_id
        if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=-pi/2,yaw_l=-pi/2,pitch_l=pi/4,grip_l=True,frame_l=frame_l
                                                                      ,x_r=midpoints[1].point.x, y_r= midpoints[1].point.y ,z_r= midpoints[1].point.z ,roll_r=pi/2,yaw_r=pi/2,pitch_r=pi/4,grip_r=True,frame_r=frame_r,dur=5):
            print "failure"
            #return False
        else:
            print "dragging done"

        if (color == 'blue'):
            GripUtils.open_grippers()
            
        return True

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_points [as 别名]
    def executeFold(self,gripPts,endPts,color_current='blue',color_next='blue'):
        """
        execute a fold 
        """

        #--- TODO--- convert gripPts,endPts to current frame of robot
        gripPts_new = []
        for pt in gripPts:
            if pt == None:
                continue
            pt_world = self.convert_to_world_frame(pt)            
            now = rospy.Time.now()
            self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))            
            pt_transformed = self.listener.transformPoint("base_footprint",pt_world)            
            gripPts_new.append(pt_transformed)

        endPts_new = []
        for pt in endPts:
            if pt == None:
                continue
            pt_world = self.convert_to_world_frame(pt)            
            now = rospy.Time.now()
            self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0))            
            pt_transformed = self.listener.transformPoint("base_footprint",pt_world)            
            endPts_new.append(pt_transformed)

        gripPts = gripPts_new
        endPts = endPts_new

        if(gripPts[1] == None):
            self.executeFold_left(gripPt=gripPts[0],endPt=endPts[0],color_current=color_current,color_next=color_next)
            return

        elif(gripPts[0] == None):
            self.executeFold_right(gripPt=gripPts[1],endPt=endPts[1],color_current=color_current,color_next=color_next)
            return

        # if blue fold, move arms to gripping position. (if red fold, arms already holding cloth at gripping position)

        if (color_current == 'blue'):
            if (len(gripPts) > 2) or (len(endPts) >  2):            
                rospy.loginfo("Requires too many grippers")
                return False
            startpoints = []
            for pt in gripPts:
                #pt_world = self.convert_to_world_frame(pt)
                pt_world = self.dupl_PointStamped(pt)
                startpoints.append(pt_world)
            
        # determine approach yaws        
        #yaw_l = -pi/3 if gripPts[0].point.x < endPts[0].point.x else -4*pi/3 # left gripper
        #yaw_r = pi/3 if gripPts[1].point.x > endPts[1].point.x else  4*pi/3  # right gripper                
          
            
        # move both arms
            if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True,
                                         point_r=startpoints[1],roll_r=pi/2,yaw_r=pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005):
                print "failure"
                return False
                         
        midpoints1 = []        
        midpoints = []
        for pt in gripPts:
            pt_world = pt# self.convert_to_world_frame(pt)            
            midpoints1.append(pt_world)
        i = 0
        for pt in endPts:
            pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt)
            pt_world.point.x = (pt_world.point.x + midpoints1[i].point.x)/2.0
            pt_world.point.y = (pt_world.point.y + midpoints1[i].point.y)/2.0
            pt_world.point.z = pt_world.point.z + 0.1
            midpoints.append(pt_world)        
            i +=1
        pitch_l = pitch_r = pi/4
        roll_l = -pi/2
        roll_r = pi/2
        yaw_l=-pi/2
        yaw_r= pi/2
        grip_l=grip_r=True
        frame_l=frame_r = midpoints[0].header.frame_id
        if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l,
                                        x_r=midpoints[1].point.x,y_r=midpoints[1].point.y,z_r=midpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r,dur=7.5):
            print "failure"
            #return False        
        endpoints = []
        for pt in endPts:
             pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt)
             endpoints.append(pt_world)
        frame_l=frame_r = endpoints[0].header.frame_id
        if not GripUtils.go_to_multi (x_l=endpoints[0].point.x,y_l=endpoints[0].point.y,z_l=endpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l
                                        ,x_r=endpoints[1].point.x,y_r=endpoints[1].point.y,z_r=endpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r
                                        ,dur=7.5):
            print "failure"
            #return False

        if(color_next == 'blue'):
            initRobot()
        return True