Python pr2_simple_arm_motions.GripUtils类代码示例

    def execute(self, userdata):

        height = 0.35
        lateral_amount = 0.65
        forward_amount = 0.3

        if self.arm == 'b':
            lateral_amount_r = lateral_amount * -1
            lateral_amount_l = lateral_amount

            if not GripUtils.go_to_multi(   x_l=forward_amount,         y_l=lateral_amount_l,   z_l=height,
                                            roll_l=0,                   pitch_l=0,              yaw_l=0, 
                                            frame_l="torso_lift_link",  grip_l=False,
                                            x_r=forward_amount,         y_r=lateral_amount_r,   z_r=height,
                                            roll_r=0,                   pitch_r=0,              yaw_r=0,
                                            frame_r="torso_lift_link",  grip_r=False,           dur=4.0):
                return FAILURE
            else:
                return SUCCESS
        else:
            if self.arm == 'r':
                lateral_amount = lateral_amount * -1
            if not GripUtils.go_to(     x=forward_amount, y=lateral_amount, z=height,
                                        roll=0, pitch=0, yaw=0, grip=False,
                                        frame="torso_lift_link", dur=4.0, arm=self.arm):
                return FAILURE
            else:
                return SUCCESS

    def executeFold_right(self,gripPt,endPt,color_current='blue',color_next='blue'):
        """
        execute fold with right arm
        """
        print "points",gripPt,endPt
        if (color_current == 'blue'):
            startpoint = self.convert_to_world_frame(gripPt)    
            if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='r',
                                  roll=pi/2,yaw=pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id):
                print "failure"
                return False
        midpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
        midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2
        midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2
        midpoint.point.z = (midpoint.point.z + 0.1)
        pitch = pi/4
        roll = pi/2    
        yaw=pi/2        
        grip=True
        frame= midpoint.header.frame_id        
        
        if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
            print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z
            #return False
        endpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt)
        frame=endpoint.header.frame_id
        if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5):
            print "failure"
            #return False

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

 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

def grab_far_right(arm):
    StanceUtils.call_stance("look_down",5.0)
    rospy.sleep(2.5)
    process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",ProcessMono)
    resp = process_mono("wide_stereo/left")
    pt = resp.pts3d[0]
    GripUtils.grab_point(pt,roll=-pi/2,yaw=pi/2,arm=arm,z_offset = 0.02,x_offset = -0.01)
    return pt

 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

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()

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)

    def execute(self, userdata):

        cloth_width     = userdata.cloth_width

        if cloth_width < 0.35:
            rospy.logwarn("Cannot shake at less than 0.35m apart; tried to do %s" % str(cloth_width))
            return FAILURE
        
        if cloth_width > 0.65:
            cloth_width = 0.65

        forward_amount  = 0.45
        height          = 0.625
        drop_amount     = 0.3
        lateral_amount  = 0.1

        for i in range(self.num_shakes):
            if i == 0:
                duration = 4.0
            if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
                                        roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
                                        x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
                                        roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
                                        frame_l="table_height",frame_r="table_height",dur=duration):
                return FAILURE

            duration = .45
            return_val = SUCCESS
            if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount,
                                        roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True,
                                        x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount,
                                        roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True,
                                        frame_l="table_height",frame_r="table_height",dur=duration):
                return_val = FAILURE
        if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height,
                                        roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True,
                                        x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height,
                                        roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True,
                                        frame_l="table_height",frame_r="table_height",dur=duration):
            return_val = FAILURE

        cloth_width = cloth_width * self.clothWidthScaleFactor
        if not GripUtils.go_to_multi(x_l=forward_amount+.2,y_l=cloth_width/2.0,z_l=height,
                                        roll_l=0,pitch_l=0,yaw_l=-pi/4,grip_l=True,
                                        x_r=forward_amount+.2,y_r=-cloth_width/2.0,z_r=height,
                                        roll_r=0,pitch_r=0,yaw_r=pi/4,grip_r=True,
                                        frame_l="table_height",frame_r="table_height",dur=4.0):
            return FAILURE

        return SUCCESS

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

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)

 def sendTarget(self, dur, target1, target2=False,grab=False):
     resp = False
     if grab:
         point = target1.point
         pitch = target1.pitch
         roll = target1.roll
         yaw = target1.yaw
         GripUtils.grab_point(point=point,pitch=pitch,roll=roll,yaw=yaw,arm=target1.arm,approach=False)
         return
     target1.point.header.stamp = rospy.Time.now()
     if not target2:
         try:
             srv = rospy.ServiceProxy("move_one_arm",MoveOneArm)
             resp = srv(MoveOneArmRequest(target=target1,dur=dur))
         except rospy.ServiceException,e:
             rospy.loginfo("Service Call Failed: %s"%e)

def main(args):

    rospy.init_node("lift_demo_node")
    listener = tf.TransformListener()
    GripUtils.go_to_relative(0, 0, 0.1, True, "l","base_footprint",listener)

    sm = OuterStateMachine(DEFAULT_OUTCOMES)
    START_STATE = 'Lift_Off'
    with sm:
         OuterStateMachine.add('Initialize',Initialize(),{SUCCESS:START_STATE,FAILURE:FAILURE})
         OuterStateMachine.add('Lift_Off',LiftOff(),{SUCCESS:SUCCESS,FAILURE:FAILURE})
         OuterStateMachine.add('Clump_To_Triangle',ClumpToTriangle(),{SUCCESS:'Triangle_To_Rectangle',FAILURE:'Initialize'})
         OuterStateMachine.add('Triangle_To_Rectangle',TriangleToRectangle(),{SUCCESS:'Fold_Towel',FAILURE:FAILURE})
         OuterStateMachine.add('Fold_Towel',FoldTowel(),{SUCCESS:SUCCESS,FAILURE:FAILURE})
    
    sis = smach_ros.IntrospectionServer('demo_smach_server', sm, '/SM_ROOT')
    sis.start()
    outcome = sm.execute()

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

    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

def smooth(pt,side):
    y_offset = -0.08
    x_offset = 0.02
    x = pt.point.x + x_offset
    y = pt.point.y + y_offset
    z = pt.point.z
    frame = "base_footprint"
    
    x_l = x
    x_r = x
    y_l = y
    y_r = y
    z_l = z_r = z
    roll_l = roll_r = 0
    yaw_l = -pi/2
    yaw_r = pi/2
    pitch_l = pitch_r=pi/6
    grip_l = grip_r = True
    GripUtils.go_to_multi   (x_l=x,y_l=y_l,z_l=z+0.04,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
                            ,x_r=x,y_r=y_r,z_r=z+0.04,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
                            ,dur=5.0)
    GripUtils.go_to_multi   (x_l=x,y_l=y_l,z_l=z,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
                            ,x_r=x,y_r=y_r,z_r=z,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
                            ,dur=3.0)
    y_l += 0.06
    y_r -= 0.06
    pitch_l = pi/4
    pitch_r = pi/4
    
    GripUtils.go_to_multi   (x_l=x,y_l=y_l,z_l=z+0.01,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame
                            ,x_r=x,y_r=y_r,z_r=z+0.01,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame
                            ,dur=5.0)