Python GripUtils.close_gripper方法代码示例

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import close_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 close_gripper [as 别名]
def open_sock():
    x=0.5
    y=0.0
    z=TABLE_HEIGHT+0.4
    frame="base_footprint"
    #old strategy
    GripUtils.go_to(x=x,y=y+0.007,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/8,grip=True,frame=frame,arm="r",dur=5.0)
    GripUtils.go_to(x=x,y=y+0.05,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=False,frame=frame,arm="l",dur=5.0)
    GripUtils.go_to(x=x,y=y-0.004,z=z+0.01,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=False,frame=frame,arm="l",dur=5.0)
    GripUtils.go_to(x=x,y=y-0.0035,z=z+0.001,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=True,frame=frame,arm="l",dur=2.0) #changed from -0.007 to -0.0065
    
    #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0)
    #GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)    
    #GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
    #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0)
    #GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0)
    #GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0)
    #GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0)
    
    
    GripUtils.close_gripper("l",extra_tight=True)
    open_amt = 0.025
    GripUtils.go_to_multi   (x_l=x,y_l=y+open_amt,z_l=z+0.01,roll_l=pi/2,yaw_l=-pi/2,pitch_l=0,grip_l=True,frame_l=frame
                            ,x_r=x,y_r=y-open_amt,z_r=z+0.01,roll_r=pi/2,yaw_r=pi/2,pitch_r=0,grip_r=True,frame_r=frame
                            ,dur=5.0)

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import close_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 close_gripper [as 别名]
def take_off_dopple_pair():
    x = DOPPLE_X
    y = DOPPLE_Y
    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="r",grip=False,frame=frame,dur=5.0)
    GripUtils.close_gripper("r",extra_tight=False)
    GripUtils.go_to(x=x,y=y,z=z+0.1,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
    GripUtils.go_to(x=x,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
    GripUtils.go_to(x=x-0.2,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import close_gripper [as 别名]
def take_out_cloth():
    print "Taking out cloth"
    RosUtils.call_service("move_torso",MoveTorso,height=0.01)
    DryerNavigationUtils.goToPosition("enter_dryer")
    GripUtils.go_to(x=-0.1,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=False,arm="l",frame="dryer")
    DryerNavigationUtils.goToPosition("into_dryer")
    GripUtils.go_to(x=0.15,y=-0.33,z=-0.65,roll=0,pitch=pi/2,yaw=0,grip=False,arm="l",frame="dryer")
    GripUtils.close_gripper("l")
    GripUtils.go_to(x=0.15,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=True,arm="l",frame="dryer")
    DryerNavigationUtils.goToPosition("enter_dryer")
    RosUtils.call_service("move_torso",MoveTorso,height=0.3)
    return True

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import close_gripper [as 别名]
 def execute(self, userdata):
    
     process_mono = rospy.ServiceProxy("clump_center_node_white/process_mono",ProcessMono)
     resp = process_mono("wide_stereo/left")
     pt = resp.pts3d[0]
     z_offset = 0.06
     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.close_gripper("l")
     while not GripUtils.has_object("l") and not rospy.is_shutdown():
         z_offset -= 0.02
         if(z_offset < 0):
             return FAILURE
         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.close_gripper("l")
     return SUCCESS

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import close_gripper [as 别名]
def pickup_sock(sock):
    z_off = 0.0
    """
    while not rospy.is_shutdown():
        StanceUtils.call_stance("close_right",5.0)
        (grip_pt,angle) = get_grip_point(sock)
        GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off)
        rospy.sleep(5.0)
        z_off = float(raw_input("What is the new z_offset?"))
        StanceUtils.call_stance("arms_up",5.0)
    """
    (grip_pt,angle) = get_grip_point(sock)
    if MODE==THICK_SOCK:
        OFFSET = 0.02
    else:
        OFFSET = 0.025
    
    y_offset = OFFSET*cos(angle)
    x_offset = OFFSET*sin(angle)
    #x_offset += 0.005
    z_offset = 0.02
    pitch = pi/2
    roll = pi/2
    yaw = pi/2-angle
    GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
    z_offset = -0.02
    z_offset -= 0.001
    GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
    #Drag thin socks
    if MODE == THIN_SOCK:
        y_offset += 0.02
        GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
    while not GripUtils.has_object("r"):
        StanceUtils.call_stance("open_right",2.0)
        pitch -= ANGLE_INCREMENT
        y_offset -= 0.005*cos(angle)
        x_offset -= 0.005*sin(angle)
        z_offset += 0.0015
        GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0)
        GripUtils.close_gripper("r",extra_tight=True)
        break
    return grip_pt

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