Python GripUtils.go_to_pt方法代码示例

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import go_to_pt [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 go_to_pt [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 go_to_pt [as 别名]
def main(args):
    rospy.init_node("unfolding_click")

    while True and not rospy.is_shutdown():
        process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono)
        resp = process_mono("wide_stereo/left")
        pt = resp.pts3d[0]
        print pt
        GripUtils.go_to_pt(point=pt,roll=pi/2,pitch=pi/2,yaw=0,grip=True,arm="l",dur=5.0,link_name="l_tip_frame")
        print "waiting.."
        a = raw_input()

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import go_to_pt [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 go_to_pt [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 go_to_pt [as 别名]
def main(args):
    rospy.init_node("unfolding_click")

    StanceUtils.call_stance("look_down",3.0)
    StanceUtils.call_stance("arms_up",3.0)
    while True and not rospy.is_shutdown():
        process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono)
        resp = process_mono("wide_stereo/left")
        pt = resp.pts3d[0]
        z_offset = 0.06
        link_frame = "r_wrist_roll_link"
        GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame)
        z_offset = 0.00
        GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame)
        rospy.sleep(5.0)

# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import go_to_pt [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 go_to_pt [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 go_to_pt [as 别名]
def pickup_clump(arm):
    init_pt = initial_pickup("r")
    GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.08,dur=3.0)
    GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.01,y_offset=0.1,dur=3.0)
    GripUtils.go_to_pt(init_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",grip=True,z_offset = 0.01,y_offset=-0.1,dur=3.0)
    StanceUtils.call_stance("arms_up",5.0)
    seam_pt = grab_far_left("l")
    GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.08,dur=3.0)
    GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.01,y_offset=+0.1,dur=3.0)
    GripUtils.go_to_pt(seam_pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",grip=True,z_offset = 0.01,y_offset=-0.1,dur=3.0)
    StanceUtils.call_stance("arms_up",5.0)

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