Python GripUtils.open_gripper方法代码示例

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]
 def execute(self, userdata):
     if self.side == 'l':
         arm = 'l'
         processor = 'far_left_finder_node'
         yaw = -pi/2
     else:
         arm = 'r'
         processor = 'far_right_finder_node'
         yaw = pi/2
     process_mono = rospy.ServiceProxy("%s/process_mono"%processor,ProcessMono)
     resp = process_mono("wide_stereo/left")
     pt = resp.pts3d[0]
     pt_opp = resp.pts3d[1]
     userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping
     homogeneity = resp.params[resp.param_names.index("homogeneity")]
     if homogeneity > 0.13:
         print "Too homogeneous: %f"%homogeneity
         return FAILURE 
     userdata.cloth_width = sqrt( (pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2 )
     #userdata.cloth_width = abs(pt_opp.point.y - pt.point.y)
     userdata.cloth_height = None
     
     if arm == "l":
         x_offset = -0.02 #was -0.01
     else:
         x_offset = -0.01
     if not GripUtils.grab_point(pt,roll=-pi/2,yaw=yaw,arm=arm,x_offset=x_offset):
         return FAILURE
     else:
         if self.let_go:
             GripUtils.open_gripper(opp_arm(arm))
         return SUCCESS

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]
def take_off_dopple(pt,arm):
    x = DOPPLE_X
    y = DOPPLE_Y
    if arm == "l":
        yaw = -pi/2
        y -= 0.005
    else:
        yaw = pi/2
        y += 0.005
    z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045
    frame = "base_footprint"
    GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm=arm,grip=False,frame=frame,dur=5.0)
    GripUtils.close_gripper(arm,extra_tight=False)
    GripUtils.go_to(x=x,y=y,z=z+0.1,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
    GripUtils.go_to(x=x,y=y,z=z+0.2,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
    roll = 0
    if arm=="l":
        yaw = -pi/2
    else:
        yaw = pi/2
    GripUtils.go_to(x=x-0.15,y=y,z=z+0.2,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0)
    #Spreads out
    GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,z_offset = 0.2,y_offset=0.2,dur=5.0) # previously z_o= 0.08 y_o 0.1
    GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/2,arm=arm,grip=True,dur=7.5,y_offset = -0.2,z_offset=0.015)
    GripUtils.open_gripper(arm)
    GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/4,arm=arm,grip=False,dur=2.5,y_offset = -0.2,z_offset=0.025)
    StanceUtils.call_stance("arms_up",5.0)

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

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]
 def add_default_stances(self):
     rospy.Service("stances/pause", ExecuteStance, self.pause)
     rospy.Service("stances/open_left", ExecuteStance, lambda req: GripUtils.open_gripper("l"))
     rospy.Service("stances/open_right", ExecuteStance, lambda req: GripUtils.open_gripper("r"))
     rospy.Service("stances/open_both", ExecuteStance, lambda req: GripUtils.open_gripper("b"))
     rospy.Service("stances/close_left", ExecuteStance, lambda req: GripUtils.close_gripper("l"))
     rospy.Service("stances/close_right", ExecuteStance, lambda req: GripUtils.close_gripper("r"))
     rospy.Service("stances/close_both", ExecuteStance, lambda req: GripUtils.close_gripper("b"))
     self.default_stances = ()

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]
 def execute(self, userdata):
    
     process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono)
     resp = process_mono("wide_stereo/left")
     pt = resp.pts3d[0]
     z_offset = 0.16
     GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",z_offset=z_offset,grip=False,dur=5.0)
     GripUtils.open_gripper("l")
     return SUCCESS

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]

#.........这里部分代码省略.........
    #new_z_l = TABLE_HEIGHT + 0.05
    #new_z_r = TABLE_HEIGHT + 0.05
    #    
    #GripUtils.go_to_multi   (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
    #                            ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
    #                            ,dur=5)  
                                
    new_z_l = TABLE_HEIGHT + 0.1
    new_z_r = TABLE_HEIGHT + 0.1
        
    GripUtils.go_to_multi   (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
                                ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
                                ,dur=3)                                                              

    new_z_l = TABLE_HEIGHT + 0.2
    new_z_r = TABLE_HEIGHT + 0.2
        
    GripUtils.go_to_multi   (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
                                ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
                                ,dur=3)

    new_z_l = TABLE_HEIGHT + 0.05
    new_z_r = TABLE_HEIGHT + 0.05
        
    GripUtils.go_to_multi   (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
                                ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
                                ,dur=3)    

    new_z_l = TABLE_HEIGHT + 0.15
    new_z_r = TABLE_HEIGHT + 0.15
        
    GripUtils.go_to_multi   (x_l=x_l+currentXoffset,y_l=y_l+currentYoffset,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l-pi/8,grip_l=grip_l,frame_l=frame_l
                                ,x_r=x_r+currentXoffset,y_r=y_r+currentYoffset,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r-pi/8,grip_r=grip_r,frame_r=frame_r
                                ,dur=3)

    #amplitude= 2.0
    #new_z_l -= 0.5*amplitude*z_step;
    #new_z_r += 0.5*amplitude*z_step;
    # new wiggle algorithm
    #for i in range(max_iters):
    #    if (i>3):
    #        new_z_l += 0.5*amplitude*z_step;
    #        new_z_r -= 0.5*amplitude*z_step;
    #        amplitude= 5.0
    #        new_z_l -= 0.5*amplitude*z_step;
    #        new_z_r += 0.5*amplitude*z_step;
    #    new_z_l += amplitude*z_step
    #    new_z_r -= amplitude*z_step
    #    new_z_l -= 0.25*z_step
    #    new_z_r -= 0.25*z_step
    #    GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
    #                            ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
    #                            ,dur=2)
    #
    #    new_z_l -= amplitude*z_step
    #    new_z_r += amplitude*z_step
    #    new_z_l -= 0.25*z_step
    #    new_z_r -= 0.25*z_step        
    #    GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
    #                            ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
    #                            ,dur=2)    
    
    
    
#    for i in range(max_iters):
#        roll_l = roll_r = pi/2
#        new_z_l -= z_step
#        new_z_r -= z_step
#        GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
#                            ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
#                            ,dur=0.75*(i/3 + 1))
#        
#        x_l += x_step
#        x_r -= x_step
#        roll_l = 2*pi/5
#        roll_r = 3 *pi/5
#        GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
#                            ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
#                            ,dur=0.75)
#        x_l -= 2*x_step
#        x_r += 2*x_step
#        GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
#                            ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
#                            ,dur=0.75)
#        roll_l = roll_r = pi/2
#        x_l += x_step
#        x_r -= x_step
#        if back_up:
#            GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_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,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
#                            ,dur=0.75*(i/3 + 1))
#
#    #End down
#    new_z_l -= z_step
#    new_z_r -= z_step
#    GripUtils.go_to_multi   (x_l=x_l,y_l=y_l,z_l=new_z_l,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame_l
#                            ,x_r=x_r,y_r=y_r,z_r=new_z_r,roll_r=roll_r,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame_r
#                            ,dur=5.0)

    GripUtils.open_gripper("b")

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]
def go_to_folding_station():
    print "Going to folding station"
    DryerNavigationUtils.dryerToTable1()
    GripUtils.go_to(arm="l",x=0.5,y=0,z=0.35,roll=0,yaw=0,pitch=pi/2,grip=True,frame="table_height")
    GripUtils.open_gripper("l")
    return True

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import open_gripper [as 别名]
 def execute(self, userdata):
     if not GripUtils.open_gripper(self.arm):
         return FAILURE
     else:
         return SUCCESS