Python GripUtils.recall_arm方法代码示例

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import recall_arm [as 别名]
 def execute(self, userdata):
     process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono)
     resp = process_mono("wide_stereo/left")
     pt_l = resp.pts3d[0]
     pt_r = resp.pts3d[1]
     pt_tl = resp.pts3d[2]
     pt_tr = resp.pts3d[3]
     #Compute approximate widths
     towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 )
     towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 )
     userdata.cloth_width = towel_width
     userdata.cloth_height = towel_height
     if resp.params[resp.param_names.index("left")] == 0:
         left = False
     else:
         left = True
     print "Correct?"
     tf = raw_input()
     if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'):
         left = not left
     if left:
         GripUtils.recall_arm("l")
         if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005):
             return SUCCESS
         else:
             return FAILURE
     else:
         
         GripUtils.recall_arm("r")
         if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005):
             return SUCCESS
         else:
             return FAILURE

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import recall_arm [as 别名]
    def execute(self, userdata):

        initial_separation = 0.11
        print 'Smooth distance: %d' % self.distance
        if self.arm=="b":
            rospy.loginfo('Self.arm is b')
            outcome = SUCCESS
            #Put arms together, with a gap of initial_separation between grippers
            if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location,
                    roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.05
                    ,link_frame_l="l_wrist_back_frame",
                    roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.05
                    ,link_frame_r="r_wrist_back_frame",dur=4.0):
                outcome = FAILURE
            rospy.loginfo('Smooth on table goto 1: Success is %s', outcome==SUCCESS)
            if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location,
                    roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.00,
                    link_frame_l="l_wrist_back_frame",
                    roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.00,
                    link_frame_r="r_wrist_back_frame",dur=2.0):
                outcome = FAILURE
            rospy.loginfo('Smooth on table goto 2: Success is %s', outcome==SUCCESS)
            if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location,
                    roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,
                    y_offset_l=(self.distance+initial_separation)/2.0, z_offset_l=0.00,
                    link_frame_l="l_wrist_back_frame",
                    roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,
                    y_offset_r=-1*(self.distance+initial_separation)/2.0, z_offset_r=0.00,
                    link_frame_r="r_wrist_back_frame",dur=2.0):
                outcome = FAILURE
            rospy.loginfo('Smooth on table goto 3: Success is %s', outcome==SUCCESS)
            GripUtils.recall_arm("b")
            return outcome
        else:
            #Right is negative in the y axis
            if self.arm=="r":
                y_multiplier = -1
            else:
                y_multiplier = 1
            rospy.loginfo('arm is %s, y multiplier is %s' % (self.arm, y_multiplier))
            print 'Location: %s' % str(self.location)
            if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
                    z_offset=0.05,arm=self.arm,
                    link_frame="%s_wrist_back_frame"%self.arm,dur=4.0, verbose=True):
                return FAILURE
            rospy.loginfo('Step 2')
            if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
                    z_offset=-0.01,arm=self.arm,
                    link_frame="%s_wrist_back_frame"%self.arm,dur=2.0, verbose=True):
                #return FAILURE
                pass
            rospy.loginfo('Step 3')
            print 'Offset: %f' % (y_multiplier*self.distance) 
            if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
                    y_offset=y_multiplier*self.distance,z_offset=-0.01,arm=self.arm,
                    link_frame="%s_wrist_back_frame"%self.arm,dur=2.0, verbose=True):
                return FAILURE
            rospy.loginfo('Step 4.  Done!')
        return SUCCESS    

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import recall_arm [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 recall_arm [as 别名]
def smooth(arm='b'):
    location = PointStamped()
    location.point.x = 0.5
    location.header.frame_id = "table_height"
    distance = TABLE_WIDTH/3
    initial_separation = 0.11
    GripUtils.recall_arm(arm)
    if arm == 'b':
        #Put arms together, with a gap of initial_separation between grippers
        if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location,
                roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.05
                ,link_frame_l="l_wrist_back_frame",
                roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.05
                ,link_frame_r="r_wrist_back_frame",dur=4.0):
            success = False
        if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location,
                roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=-0.03, 
                link_frame_l="l_wrist_back_frame",
                roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=-0.03, 
                link_frame_r="r_wrist_back_frame",dur=2.0):
            success = False
        if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location,
                roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,
                y_offset_l=(distance+initial_separation)/2.0, z_offset_l=-0.03,
                link_frame_l="l_wrist_back_frame",
                roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,
                y_offset_r=-1*(distance+initial_separation)/2.0, z_offset_r=-0.03,
                link_frame_r="r_wrist_back_frame",dur=2.0):
            success = False
    else:
        #Right is negative in the y axis
        if arm=="r":
            y_multiplier = -1
        else:
            y_multiplier = 1
        location.point.y -= y_multiplier*distance/2
        if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
                z_offset=0.05,arm=arm,
                link_frame="%s_wrist_back_frame"%arm,dur=4.0):
            success = False
        if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
                z_offset=-0.01,arm=arm,
                link_frame="%s_wrist_back_frame"%arm,dur=2.0):
            success = False
        if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2,
                y_offset=y_multiplier*distance,z_offset=-0.01,arm=arm,
                link_frame="%s_wrist_back_frame"%arm,dur=2.0):
            success = False
    GripUtils.recall_arm(arm)
    return True

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import recall_arm [as 别名]
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

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import recall_arm [as 别名]
 def execute(self,userdata):
     GripUtils.recall_arm('b')
     return SUCCESS

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import recall_arm [as 别名]
    def execute(self, userdata):

        if GripUtils.recall_arm(self.arm):
            return SUCCESS
        else:
            return FAILURE