Exemple #1
0
def percept(obj_pose=None,
            binNum=0,
            obj_id=None,
            bin_contents=None,
            isExecute=True,
            withPause=True):
    capsen.capsen.init()
    print '[Percept] percept heuristic'

    goToHome.goToHome(robotConfig=None,
                      homePos=[1, 0, 1.2],
                      isExecute=isExecute,
                      withPause=withPause)
    kinect_obj_pose = None

    if useKinect:
        print '[Percept] kinect percept'
        kinect_obj_pose, kinect_score = capsen.capsen.detectOneObject(
            obj_id, bin_contents, binNum, mode=0, withScore=True)
        if kinect_obj_pose is None:
            print '[Percept] kinect percept failed.'
        else:
            kinect_pose_type = find_object_pose_type(obj_id, kinect_obj_pose)
            print '[Percept] kinect percept success.'

        if not (objectUseHandVisionList[obj_id]
                or binNum in binUseHandVisionList or kinect_obj_pose is None):
            return kinect_obj_pose, kinect_pose_type

    # turn off kinect
    kinect_stop()

    print '[Percept] Perception heuristic wants to do percept_hand.'
    hand_obj_pose, hand_pose_type, hand_score = percept_hand(
        obj_pose, binNum, obj_id, bin_contents, isExecute, withPause)

    # turn on kinect
    kinect_start()

    if hand_obj_pose and kinect_obj_pose:
        if hand_score >= kinect_score:
            return hand_obj_pose, hand_pose_type
        else:
            return kinect_obj_pose, kinect_pose_type
    elif hand_obj_pose:
        return hand_obj_pose, hand_pose_type
    elif kinect_obj_pose:
        return kinect_obj_pose, kinect_pose_type
    else:
        return None, None
def percept(obj_pose = None, binNum = 0, obj_id = None, bin_contents = None, isExecute = True, withPause = True):
    capsen.capsen.init()  
    print '[Percept] percept heuristic'
    
    goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
    kinect_obj_pose = None
    
    if useKinect:
        print '[Percept] kinect percept'
        kinect_obj_pose, kinect_score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode = 0, withScore = True)
        if kinect_obj_pose is None:
            print '[Percept] kinect percept failed.' 
        else:
            kinect_pose_type = find_object_pose_type(obj_id, kinect_obj_pose)
            print '[Percept] kinect percept success.'
        
        if not (objectUseHandVisionList[obj_id] or binNum in binUseHandVisionList or kinect_obj_pose is None):
            return kinect_obj_pose, kinect_pose_type
    
    # turn off kinect
    kinect_stop()
    
    print '[Percept] Perception heuristic wants to do percept_hand.' 
    hand_obj_pose, hand_pose_type, hand_score = percept_hand(obj_pose, binNum, obj_id, bin_contents, isExecute, withPause)
    
    # turn on kinect
    kinect_start()
    
    if hand_obj_pose and kinect_obj_pose: 
        if hand_score >= kinect_score:
            return hand_obj_pose, hand_pose_type
        else:
            return kinect_obj_pose, kinect_pose_type
    elif hand_obj_pose:
        return hand_obj_pose, hand_pose_type
    elif kinect_obj_pose:
        return kinect_obj_pose, kinect_pose_type
    else:
        return None, None
Exemple #3
0
def manual_cal_2(binNum=0, endBinNum=11):

    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    joint_topic = '/joint_states'

    # set tcp
    l1 = 0.06345
    l2 = 0.400
    tip_hand_transform = [
        l1, 0, l2, 0, 0, 0
    ]  # to be updated when we have a hand design finalized

    moveDistHorizontal = 0.09  #we could do 1 more cm (without the lip we could move about 4cm more)
    moveDistVertical = 0.08  #we could do 1 more cm

    #set Speed
    ik.ik.setSpeedByName('faster')

    # broadcast frame attached to tcp
    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)

    # get position of the tcp in world frame
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]
    tcpPosHome = tcpPos
    orientation = [0.0, 0.7071, 0.0,
                   0.7071]  # set topple orientation (rotate wrist)

    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)

    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6',
                                          'map', listener)
    tcpPos = [
        pose_world.pose.position.x, pose_world.pose.position.y,
        pose_world.pose.position.z
    ]

    print 'TCP is at:', tcpPos

    print 'planning to go home'

    # home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
    # planner = IKJoint(target_joint_pos=home_joint_pose)
    # plan = planner.plan()
    # plan.visualize()
    raw_input('execute?')
    # plan.execute()
    goToHome.goToHome()
    print 'done going home'

    calibBins = []
    for x in range(binNum, endBinNum + 1):
        calibPts = []

        if x == 1 or x == 4 or x == 7 or x == 10:
            calibBins.append([])
            continue

        distFromShelf = 0.05
        mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(
            distFromShelf, x)
        mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map',
                                            listener)
        mouthBin = [
            mouthBin.pose.position.x, mouthBin.pose.position.y,
            mouthBin.pose.position.z
        ]

        epsilon = 0.016
        distFromShelf = distFromShelf - epsilon

        # go to mouth of bin
        print 'planning to go to mouth but a little out'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = mouthBin
        planner = IK(q0=None,
                     target_tip_pos=targetPt,
                     target_tip_ori=Orientation,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mouth'

        # go to the mouth of the bin but actually inside
        print 'planning to go to start position'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0] + distFromShelf, mouthBin[1], mouthBin[2]]
        planner = IK(q0=None,
                     target_tip_pos=targetPt,
                     target_tip_ori=Orientation,
                     tip_hand_transform=tip_hand_transform,
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            plan.visualize()
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to start position'

        OrientationList = []
        targetPtList = []
        captionList = []
        ntouch = 5

        # go down
        captionList.append('go down')
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1],
            mouthBin[2] - moveDistVertical
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go left
        captionList.append('go left')
        Orientation = [0.5, 0.5, 0.5, 0.5]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1] + moveDistHorizontal,
            mouthBin[2]
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go right
        captionList.append('go right')
        Orientation = [0.5, -0.5, 0.5, -0.5]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1] - moveDistHorizontal,
            mouthBin[2]
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go up
        captionList.append('go up')
        Orientation = [0.7071, 0, 0.7071, 0]
        targetPt = [
            mouthBin[0] + distFromShelf, mouthBin[1],
            mouthBin[2] + moveDistVertical
        ]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        # go to mid back of the bin
        captionList.append('go mid back')
        Orientation = [0, 0.7071 + 0.11 / 4, 0, 0.7071 - 0.11 / 4]
        targetPt = [mouthBin[0] + 0.30, mouthBin[1], mouthBin[2] - 0.08]
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)

        for touchInd in range(ntouch):
            print captionList[touchInd]
            targetPt = targetPtList[touchInd]
            Orientation = OrientationList[touchInd]

            planner = IK(q0=None,
                         target_tip_pos=targetPt,
                         target_tip_ori=Orientation,
                         tip_hand_transform=tip_hand_transform,
                         joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            if s:
                plan.visualize()
                raw_input('execute?')
                ik.ik.setSpeedByName('fast')
                plan.execute()
            if not s:
                print 'could not find plan'
            print 'done going down, TELEOP TIME:'
            raw_input('when done with teleop hit enter: ')
            #~ rospy.sleep(0.1)
            #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
            #~ rospy.sleep(0.1)
            t = listener.getLatestCommonTime('calib', 'map')
            (calibPoseTrans,
             calibPoseRot) = listener.lookupTransform('map', 'calib', t)
            calibPose = list(calibPoseTrans) + list(calibPoseRot)
            #~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
            #~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
            calibPts.append(calibPose)
            raw_input('done recording position, hit enter to go back')

            if s:
                plan.executeBackward()
            else:
                print 'Please teleop back'
                raw_input('done?')

        calibBins.append(calibPts)

        filename = os.environ[
            'APC_BASE'] + '/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
        with open(filename, 'w') as outfile:
            json.dump(calibBins, outfile)

        print 'planning to go home'
        home_joint_pose = [
            -0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715,
            0.1061231983, -0.1045031463
        ]
        planner = IKJoint(target_joint_pos=home_joint_pose)
        plan = planner.plan()
        plan.visualize()
        raw_input('execute?')
        ik.ik.setSpeedByName('faster')
        plan.execute()
        print 'done going home'

    print 'dumped to file ', filename
    
if __name__=='__main__':
    #scoop()
    rospy.init_node('listener', anonymous=True)
    
    suction.stop()
    
    objPoses = []
    objPoses.append([1.599, 0.450, 1.106, -0.0030, 0.6979, -0.7161, 0.0023]) # upper left corner
    objPoses.append([1.607, -0.494, 1.130, -0.0030, 0.6979, -0.7161, 0.0023]) # uppper right corner
    objPoses.append([1.598, 0.467, 0.412, -0.0030, 0.6911, -0.7227, 0.0023]) # lower left corner
    objPoses.append([1.660, -0.500, 0.379, -0.0384, 0.6970, -0.7150, 0.0368]) #lower right corner
    
    binNums = [0,2,9,11]
    
    goToHome.goToHome()
    for x in range(0,4):
        suction_side(
        objPose = objPoses[x],
        binNum = binNums[x],
        objId='expo_dry_erase_board_eraser' ,
        robotConfig=None, 
        shelfPosition = [1.9019,0.00030975,-0.503], 
        isExecute = True,
        withPause = False)
        goToHome.goToHome()
        
    
    # posesrv = rospy.ServiceProxy('/pose_service', GetPose)   # should move service name out
    # rospy.sleep(0.5)
    # data = posesrv('','')
def jsonTest(heuristic,
             strategies,
             jsonFilename,
             useVision=False,
             withPause=False,
             isExecute=True,
             toSort=False,
             forceSucc=False):
    globalSpeedSet = 'yolo'
    ik.ik.setSpeedByName(globalSpeedSet)
    objectiveBinPos = [1.15, 0, 0.2]

    import json
    from pprint import pprint
    with open(jsonFilename) as data_file:
        data = json.load(data_file)

    if useVision:
        capsen.capsen.init()

    bin_contents_all = data['bin_contents']
    work_order = data['work_order']
    if toSort:
        work_order = sortOrder(work_order, bin_contents_all)
    displayOrder(work_order, bin_contents_all)

    goToHome.goToHome(robotConfig=None,
                      homePos=[1, 0, 1.2],
                      isExecute=isExecute,
                      withPause=withPause)

    count_suck = 0
    count_grasp = 0
    for orderInd, order in enumerate(work_order):
        displayOrder(work_order, bin_contents_all, orderInd)
        ik.ik.setSpeedByName(globalSpeedSet)
        count_try = 0
        try_max = 10
        isDirty = False
        bin_id = order['bin']  # e.g. bin_A
        binNum = bin_id2num(bin_id)  # e.g. 0
        obj_id = order['item']  # object's id in string

        print '\n%s\n[Heuristic] [Order %d] bin_id:' % (
            '-' * 70,
            orderInd), bin_id, binNum, '; obj_id:', obj_id, '\n%s' % ('-' * 70)

        if useVision:
            try:
                obj_pose, pose_type = percept.percept(
                    obj_id=obj_id,
                    binNum=binNum,
                    bin_contents=bin_contents_all[bin_id],
                    isExecute=isExecute,
                    withPause=withPause)

                if obj_pose is None:
                    print '[Heuristic] vision failed, continue to next order.'
                    continue
            except:
                print '[Heuristic] vision failed, continue to next order.', 'encounters errors:', traceback.format_exc(
                )
                continue
        else:
            obj_pose, pose_type = percept.perceptFromManualFit(obj_id)

        print '[Heuristic] pose_type:', pose_type, '; pose:', map(
            shortfloat, obj_pose)

        resetStrat = True
        tryFlag = True
        visionSucc = True

        while resetStrat and tryFlag and visionSucc:
            resetStrat = False

            strategy_list = heuristic[obj_id][pose_type]
            for i in range(len(strategy_list)):
                strategy_name = strategy_list[i]
                print '[Heuristic] Attempt', i, 'of', try_max, ':', strategy_name

                count_try += 1
                if count_try == try_max:
                    print '[Heuristic] reached maximum trial number'
                    tryFlag = False
                    break

                if strategy_name == 'percept' or strategy_name == 'percept-hand':
                    if isDirty:
                        print '[Heuristic] perception has been called'
                        if useVision:
                            ik.ik.setSpeedByName(globalSpeedSet)
                            obj_pose, pose_type_new = percept.percept(
                                obj_pose=obj_pose,
                                binNum=binNum,
                                obj_id=obj_id,
                                bin_contents=bin_contents_all[bin_id],
                                isExecute=isExecute,
                                withPause=withPause)
                        else:
                            obj_pose, pose_type_new = percept.perceptFromManualFit(
                                obj_id)

                        if obj_pose is None:
                            visionSucc = False
                            break
                        else:
                            isDirty = False

                        print '[Heuristic] pose_type:', pose_type_new, '; pose:', obj_pose

                        if pose_type_new == pose_type:
                            pose_type = pose_type_new
                            continue
                        else:
                            pose_type = pose_type_new
                            resetStrat = True
                            break
                else:
                    try:
                        if strategy_name in strategies:
                            goToMouth.goToMouth(robotConfig=None,
                                                binNum=binNum,
                                                isExecute=True,
                                                withPause=withPause)

                            (isDirty, succ) = strategies[strategy_name](
                                obj_pose,
                                binNum,
                                obj_id,
                                bin_contents_all[bin_id],
                                isExecute=isExecute,
                                withPause=withPause)  #run it

                            if forceSucc and (i == len(strategy_list) - 1
                                              or count_try == try_max - 1
                                              ):  ###### for testing in virtual
                                succ = True
                        else:
                            print '[Heuristic] Strategy', strategy_name, 'not implemented yet. Skip it.'
                            continue

                        if succ:
                            print '[Heuristic] Strategy', strategy_name, 'success'
                            ik.ik.setSpeedByName('faster')
                            print '[Heuristic] Object HAS been picked up with the %s primitive' % strategy_name
                            if strategy_name == 'suck-down' or strategy_name == 'suck-side':
                                count_suck = count_suck + 1
                                attemptGoToBin = goToBin.goToBin(
                                    robotConfig=None,
                                    objectiveBinPos=objectiveBinPos,
                                    isExecute=isExecute,
                                    withPause=withPause,
                                    withSuction=True,
                                    counter=count_suck)

                            else:
                                count_grasp = count_grasp + 1
                                attemptGoToBin = goToBin.goToBin(
                                    binNum=binNum,
                                    robotConfig=None,
                                    objectiveBinPos=objectiveBinPos,
                                    isExecute=isExecute,
                                    withPause=withPause,
                                    withSuction=False,
                                    counter=count_grasp)
                                if not attemptGoToBin:
                                    goToHome.goToHome(robotConfig=None,
                                                      homePos=[1, 0, 1.2],
                                                      isExecute=isExecute,
                                                      withPause=withPause)
                                    goToBin.goToBin(
                                        binNum=binNum,
                                        robotConfig=None,
                                        objectiveBinPos=objectiveBinPos,
                                        isExecute=isExecute,
                                        withPause=withPause,
                                        withSuction=False,
                                        counter=count_grasp)
                            break
                    except:
                        print '[Heuristic] Strategy', strategy_name, 'encounters errors:', traceback.format_exc(
                        )

                    if withPause:
                        raw_input(
                            '[Heuristic] Press any key to continue next strategy.'
                        )

            ik.ik.setSpeedByName(globalSpeedSet)
            goToHome.goToHome(robotConfig=None,
                              homePos=[1, 0, 1.2],
                              isExecute=isExecute,
                              withPause=withPause)

        if withPause:
            raw_input('[Heuristic] Press any key to continue next object.')

    print '\n[Heuristic] We are done!'
def jsonTest(heuristic, strategies, jsonFilename, useVision = False, withPause = False, isExecute = True, toSort = False, forceSucc = False):
    globalSpeedSet = 'yolo'
    ik.ik.setSpeedByName(globalSpeedSet)
    objectiveBinPos = [1.15, 0 , 0.2]
    
    import json
    from pprint import pprint
    with open(jsonFilename) as data_file:   
        data = json.load(data_file)
    
    if useVision:
        capsen.capsen.init()
    
    bin_contents_all = data['bin_contents']
    work_order = data['work_order']
    if toSort:
        work_order = sortOrder(work_order, bin_contents_all)
    displayOrder(work_order, bin_contents_all)
        
    goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
    
    count_suck  = 0
    count_grasp = 0
    for orderInd, order in enumerate(work_order):
        displayOrder(work_order, bin_contents_all, orderInd)
        ik.ik.setSpeedByName(globalSpeedSet)
        count_try = 0
        try_max   = 10
        isDirty = False
        bin_id = order['bin']        # e.g. bin_A
        binNum = bin_id2num(bin_id) # e.g. 0
        obj_id = order['item']       # object's id in string
        
        print '\n%s\n[Heuristic] [Order %d] bin_id:' % ('-'*70, orderInd), bin_id, binNum, '; obj_id:', obj_id , '\n%s' % ('-'*70)
        
        if useVision:
            try:
                obj_pose, pose_type = percept.percept(obj_id=obj_id, binNum=binNum, bin_contents = bin_contents_all[bin_id], isExecute = isExecute, withPause = withPause)

                if obj_pose is None:
                    print '[Heuristic] vision failed, continue to next order.'
                    continue
            except:
                print '[Heuristic] vision failed, continue to next order.', 'encounters errors:', traceback.format_exc()
                continue
        else:
            obj_pose, pose_type = percept.perceptFromManualFit(obj_id)
            
        
        print '[Heuristic] pose_type:', pose_type, '; pose:', map(shortfloat, obj_pose)
        
        
        resetStrat = True
        tryFlag    = True
        visionSucc = True
        
        while resetStrat and tryFlag and visionSucc:
            resetStrat = False
            
            strategy_list = heuristic[obj_id][pose_type]
            for i in range(len(strategy_list)):
                strategy_name = strategy_list[i]
                print '[Heuristic] Attempt', i, 'of', try_max, ':', strategy_name
                
                count_try += 1
                if count_try == try_max:
                    print '[Heuristic] reached maximum trial number'
                    tryFlag = False
                    break
                
                if strategy_name == 'percept' or strategy_name == 'percept-hand':
                    if isDirty:
                        print '[Heuristic] perception has been called'
                        if useVision:
                            ik.ik.setSpeedByName(globalSpeedSet)
                            obj_pose, pose_type_new = percept.percept(obj_pose = obj_pose, binNum = binNum, obj_id = obj_id, bin_contents = bin_contents_all[bin_id], 
                                                            isExecute = isExecute, withPause = withPause)
                        else:
                            obj_pose, pose_type_new = percept.perceptFromManualFit(obj_id)

                        if obj_pose is None:
                            visionSucc = False
                            break
                        else:
                            isDirty = False
                        
                        print '[Heuristic] pose_type:', pose_type_new, '; pose:', obj_pose
                            
                        if pose_type_new == pose_type:
                            pose_type = pose_type_new
                            continue
                        else:
                            pose_type = pose_type_new
                            resetStrat = True
                            break
                else:
                    try:
                        if strategy_name in strategies:
                            goToMouth.goToMouth(robotConfig=None, binNum = binNum, isExecute = True, withPause = withPause)
                            
                            (isDirty,succ) = strategies[ strategy_name ](obj_pose, binNum, obj_id, bin_contents_all[bin_id],
                                                  isExecute = isExecute, withPause = withPause)   #run it
                                                  
                            if forceSucc and (i == len(strategy_list)-1 or count_try == try_max-1): ###### for testing in virtual
                                succ = True
                        else:
                            print '[Heuristic] Strategy', strategy_name , 'not implemented yet. Skip it.'
                            continue
                            
                        if succ: 
                            print '[Heuristic] Strategy', strategy_name , 'success'
                            ik.ik.setSpeedByName('faster')
                            print '[Heuristic] Object HAS been picked up with the %s primitive' % strategy_name
                            if strategy_name == 'suck-down' or strategy_name == 'suck-side':
                                count_suck = count_suck + 1
                                attemptGoToBin = goToBin.goToBin(  robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute, 
                                                    withPause = withPause, withSuction = True, counter = count_suck)
                                                    
                            else:
                                count_grasp = count_grasp + 1
                                attemptGoToBin = goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute, 
                                                    withPause = withPause, withSuction = False, counter = count_grasp)
                                if not attemptGoToBin:
                                    goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
                                    goToBin.goToBin( binNum=binNum, robotConfig=None, objectiveBinPos = objectiveBinPos, isExecute = isExecute, 
                                                withPause = withPause, withSuction = False, counter = count_grasp)
                            break
                    except:
                        print '[Heuristic] Strategy', strategy_name, 'encounters errors:', traceback.format_exc()
                
                    if withPause:  raw_input('[Heuristic] Press any key to continue next strategy.')
                
            ik.ik.setSpeedByName(globalSpeedSet)
            goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
            
        if withPause:  raw_input('[Heuristic] Press any key to continue next object.')
    
    print '\n[Heuristic] We are done!'
    suction.stop()

    objPoses = []
    objPoses.append([1.599, 0.450, 1.106, -0.0030, 0.6979, -0.7161,
                     0.0023])  # upper left corner
    objPoses.append([1.607, -0.494, 1.130, -0.0030, 0.6979, -0.7161,
                     0.0023])  # uppper right corner
    objPoses.append([1.598, 0.467, 0.412, -0.0030, 0.6911, -0.7227,
                     0.0023])  # lower left corner
    objPoses.append([1.660, -0.500, 0.379, -0.0384, 0.6970, -0.7150,
                     0.0368])  #lower right corner

    binNums = [0, 2, 9, 11]

    goToHome.goToHome()
    for x in range(0, 4):
        suction_down(objPose=objPoses[x],
                     binNum=binNums[x],
                     objId='expo_dry_erase_board_eraser',
                     robotConfig=None,
                     shelfPosition=[1.9019, 0.00030975, -0.503],
                     isExecute=True,
                     withPause=True)
        goToHome.goToHome()

    # posesrv = rospy.ServiceProxy('/pose_service', GetPose)   # should move service name out
    # rospy.sleep(0.5)
    # data = posesrv('','')
    # pos_x= data.pa.object_list[0].pose.position.x
    # pos_y= data.pa.object_list[0].pose.position.y
Exemple #8
0
import goToHome

temp_bias = 0.04  #TO-DO: Remove this bias, calculate object height correctly
topple.topple(objPose = [1.55620419979, 0.281148612499, 1.14214038849-temp_bias,0,0,0,0],
        objId = 0, 
        binNum=0, 
        robotConfig=None,
        shelfPosition = [1.9019,0.00030975,-0.503],
        forceThreshold = 1,
        effect = 'topple',
        binDepthReach = 0.37,
        isExecute = True,
        withPause = False)
scoop.scoop(
    objPose = [1.55620419979, 0.281148612499, 1.14214038849,0,0,0,0],
    binNum=0, 
    robotConfig=None, 
    shelfPosition = [1.9019,0.00030975,-0.503], 
    isExecute = True,
    withPause = False)

goToBin.goToBin(robotConfig=None,
        objectiveBinPos = [0.9,0,0], 
        isExecute = True,
        withPause = False)

goToHome.goToHome(robotConfig = None,
            homePos = [1,0,1.2], 
            isExecute = True,
            withPause = False)
def manual_cal_2(binNum = 0, endBinNum = 11):
    
    ## initialize listener rospy
    listener = tf.TransformListener()
    rospy.sleep(0.1)
    br = tf.TransformBroadcaster()
    rospy.sleep(0.1)
    joint_topic = '/joint_states'
    
    # set tcp
    l1 = 0.06345
    l2 = 0.400  
    tip_hand_transform = [l1, 0, l2, 0,0,0] # to be updated when we have a hand design finalized
    
    moveDistHorizontal = 0.09 #we could do 1 more cm (without the lip we could move about 4cm more)
    moveDistVertical = 0.08 #we could do 1 more cm
    
    #set Speed
    ik.ik.setSpeedByName('faster')
    
    # broadcast frame attached to tcp
    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)
    
    # get position of the tcp in world frame
    pose_world  = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos      =[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    tcpPosHome  = tcpPos
    orientation = [0.0, 0.7071, 0.0, 0.7071] # set topple orientation (rotate wrist)
    
    #~ rospy.sleep(0.1)
    #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
    #~ rospy.sleep(0.1)
    
    pose_world = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
    tcpPos=[pose_world.pose.position.x, pose_world.pose.position.y, pose_world.pose.position.z]
    
    print 'TCP is at:', tcpPos
    
    print 'planning to go home'
    
    # home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
    # planner = IKJoint(target_joint_pos=home_joint_pose)
    # plan = planner.plan()
    # plan.visualize()
    raw_input('execute?')
    # plan.execute()
    goToHome.goToHome()
    print 'done going home'
    
    calibBins = []
    for x in range(binNum, endBinNum+1):
        calibPts = []
        
        if x == 1 or x == 4 or x == 7 or x == 10:
            calibBins.append([])
            continue
        
        distFromShelf = 0.05
        mouthBinShelf, binBaseHeightShelf = getBinMouthAndFloor(distFromShelf, x)
        mouthBin = coordinateFrameTransform(mouthBinShelf, 'shelf', 'map', listener)
        mouthBin = [mouthBin.pose.position.x, mouthBin.pose.position.y, mouthBin.pose.position.z]
        
        epsilon = 0.016
        distFromShelf = distFromShelf - epsilon
               
        # go to mouth of bin
        print 'planning to go to mouth but a little out'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = mouthBin
        planner = IK(q0 = None, target_tip_pos = targetPt, 
                     target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                     joint_topic=joint_topic)
        plan = planner.plan()
        plan.visualize()
        s = plan.success()
        if s:
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to mouth'
        
        # go to the mouth of the bin but actually inside
        print 'planning to go to start position'
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]] 
        planner = IK(q0 = None, target_tip_pos = targetPt, 
                     target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                     joint_topic=joint_topic)
        plan = planner.plan()
        s = plan.success()
        if s:
            plan.visualize()
            raw_input('execute?')
            ik.ik.setSpeedByName('fast')
            plan.execute()
        if not s:
            print 'could not find plan'
        print 'done going to start position'
        
        
        
        OrientationList = []
        targetPtList = []
        captionList = []
        ntouch = 5
        
        # go down
        captionList.append('go down')
        Orientation = [0.0, 0.7071, 0.0, 0.7071]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]-moveDistVertical] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go left
        captionList.append('go left')
        Orientation = [0.5, 0.5, 0.5, 0.5]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]+moveDistHorizontal, mouthBin[2]] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go right
        captionList.append('go right')
        Orientation = [0.5, -0.5, 0.5, -0.5]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1]-moveDistHorizontal, mouthBin[2]] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go up
        captionList.append('go up')
        Orientation = [0.7071, 0, 0.7071, 0]
        targetPt = [mouthBin[0]+distFromShelf, mouthBin[1], mouthBin[2]+moveDistVertical] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        # go to mid back of the bin
        captionList.append('go mid back')
        Orientation = [0, 0.7071+0.11/4, 0, 0.7071-0.11/4]
        targetPt = [mouthBin[0]+0.30, mouthBin[1], mouthBin[2]-0.08] 
        OrientationList.append(Orientation)
        targetPtList.append(targetPt)
        
        for touchInd in range(ntouch):
            print captionList[touchInd]
            targetPt = targetPtList[touchInd]
            Orientation = OrientationList[touchInd]
            
            planner = IK(q0 = None, target_tip_pos = targetPt, 
                         target_tip_ori = Orientation, tip_hand_transform=tip_hand_transform, 
                         joint_topic=joint_topic)
            plan = planner.plan()
            s = plan.success()
            if s:
                plan.visualize()
                raw_input('execute?')
                ik.ik.setSpeedByName('fast')
                plan.execute()
            if not s:
                print 'could not find plan'
            print 'done going down, TELEOP TIME:'
            raw_input('when done with teleop hit enter: ')
            #~ rospy.sleep(0.1)
            #~ br.sendTransform(tuple(tip_hand_transform[0:3]), tfm.quaternion_from_euler(*tip_hand_transform[3:6]), rospy.Time.now(), 'calib', "link_6")
            #~ rospy.sleep(0.1)
            t = listener.getLatestCommonTime('calib', 'map')
            (calibPoseTrans, calibPoseRot) = listener.lookupTransform('map','calib',t)
            calibPose = list(calibPoseTrans) + list(calibPoseRot)
            #~ calibPose = coordinateFrameTransform(tip_hand_transform[0:3], 'link_6', 'map', listener)
            #~ calibPtsTemp = [calibPose.pose.position.x,calibPose.pose.position.y,calibPose.pose.position.z]
            calibPts.append(calibPose)
            raw_input('done recording position, hit enter to go back')
            
            if s:
                plan.executeBackward()
            else:
                print 'Please teleop back'
                raw_input('done?')
                
                
        
        calibBins.append(calibPts)
        
        filename = os.environ['APC_BASE']+'/catkin_ws/src/apc_config/shelf_calib_data/shelf_calib_all_Final.json'
        with open(filename, 'w') as outfile:
            json.dump(calibBins, outfile)
        
        print 'planning to go home'
        home_joint_pose = [-0.005744439031, -0.6879946105, 0.5861570764, 0.09693312715, 0.1061231983, -0.1045031463]
        planner = IKJoint(target_joint_pos=home_joint_pose)
        plan = planner.plan()
        plan.visualize()
        raw_input('execute?')
        ik.ik.setSpeedByName('faster')
        plan.execute()
        print 'done going home'
    
        
    print 'dumped to file ', filename
def percept_hand(obj_pose = None, binNum = 0, obj_id = None, bin_contents = None, isExecute = True, withPause = True):
    capsen.capsen.init()
    home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
    planner = IKJoint(target_joint_pos=home_joint_pose); 
    #plan = Plan()
    #plan.q_traj = [home_joint_pose]; 
    plan = planner.plan()
    plan.visualize(); 
    plan.setSpeedByName('yolo'); 
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()
    
    bin_cnstr = get_bin_inner_cnstr()
    
    # 1. init target poses
    target_poses = []
    refind = 7
    for i in range(12):
        target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(math.pi, 1.4, math.pi).tolist() # for bin 7 vertical
        target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(2.2, 0, math.pi/2).tolist()
        target_pose_7_invhori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(-math.pi/2, 0, -math.pi/2).tolist()
        
        if i < 3:
            target_pose = copy.deepcopy(target_pose_7_invhori)
            if i == 0:
                target_pose[1] -= 0.06
            target_pose[2] -= 0.08  # z
        elif i<6:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[0] += 0.03  # x
            target_pose[2] -= 0.04  # z
        elif i<9:
            target_pose = copy.deepcopy(target_pose_7_hori)
        else:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[2] += 0.1  # z

        target_pose[1] += (bin_cnstr[i][0]+bin_cnstr[i][1])/2 - (bin_cnstr[refind][0]+bin_cnstr[refind][1])/2  # y
        target_pose[2] += (bin_cnstr[i][4]+bin_cnstr[i][5])/2 - (bin_cnstr[refind][4]+bin_cnstr[refind][5])/2  # z
        target_poses.append(target_pose)
    
    y_deltas = [0.08, -0.08]
    caption = ['left', 'right']
    max_score = -100
    max_obj_pose = None
    for i in range(len(y_deltas)):
        # 2. plan to target
        tip_hand_transform = xyzrpy_from_xyzquat([-0.067, 0.027, -0.012, -0.007, 0.704, 0.710, -0.002])  # need to be updated
        
        target_pose = copy.deepcopy(target_poses[binNum])
        target_pose[1] += y_deltas[i]
        plan = None
        planner = IK(target_tip_pos = target_pose[0:3], target_tip_ori = target_pose[3:7], 
                     tip_hand_transform=tip_hand_transform, 
                     target_link='link_5', target_joint_bb=[[6, 1.2, 1.2]])
        for j in range(15):
            planner.ori_tol = 0.5*j/10.0
            planner.pos_tol = 0.01*j/10.0
            
            newplan = planner.plan()
            if newplan.success():
                plan = newplan
                print '[Percept] hand planning success at the trial %d' % j
                break
                
        if plan:
            plan.setSpeedByName('fastest')
            plan.visualize()

            if withPause:
                print '[Percept] Going to percept_hand %s ' % caption[i]
                pause()
            plan.execute()
        else:
            print '[Percept] hand planning failed'
            continue
        
        # 3. percept
        rospy.sleep(0.8)  # wait for new pointcloud
        obj_pose, score = capsen.capsen.detectOneObject(obj_id, bin_contents, binNum, mode = 1, withScore = True)
        if score > max_score:
            max_obj_pose = obj_pose
            max_score = score

    # 4. move back to percept home
    planner = IKJoint(target_joint_pos=home_joint_pose)
    plan = planner.plan()
    plan.visualize(); 
    plan.setSpeedByName('yolo'); 
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()
    
    goToHome.goToHome(robotConfig=None, homePos = [1,0,1.2], isExecute = isExecute, withPause = withPause)
    
    if max_obj_pose is None:
        return None, None, None
    
    pose_type = find_object_pose_type(obj_id, max_obj_pose)
    
    return max_obj_pose, pose_type, max_score
Exemple #11
0
def percept_hand(obj_pose=None,
                 binNum=0,
                 obj_id=None,
                 bin_contents=None,
                 isExecute=True,
                 withPause=True):
    capsen.capsen.init()
    home_joint_pose = [0, -0.2, 0.2, 0.01, 1, 1.4]
    planner = IKJoint(target_joint_pos=home_joint_pose)
    #plan = Plan()
    #plan.q_traj = [home_joint_pose];
    plan = planner.plan()
    plan.visualize()
    plan.setSpeedByName('yolo')
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()

    bin_cnstr = get_bin_inner_cnstr()

    # 1. init target poses
    target_poses = []
    refind = 7
    for i in range(12):
        target_pose_7_vert = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
            math.pi, 1.4, math.pi).tolist()  # for bin 7 vertical
        target_pose_7_hori = [1.1756, 0, 0.79966] + tfm.quaternion_from_euler(
            2.2, 0, math.pi / 2).tolist()
        target_pose_7_invhori = [1.1756, 0, 0.79966
                                 ] + tfm.quaternion_from_euler(
                                     -math.pi / 2, 0, -math.pi / 2).tolist()

        if i < 3:
            target_pose = copy.deepcopy(target_pose_7_invhori)
            if i == 0:
                target_pose[1] -= 0.06
            target_pose[2] -= 0.08  # z
        elif i < 6:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[0] += 0.03  # x
            target_pose[2] -= 0.04  # z
        elif i < 9:
            target_pose = copy.deepcopy(target_pose_7_hori)
        else:
            target_pose = copy.deepcopy(target_pose_7_hori)
            target_pose[2] += 0.1  # z

        target_pose[1] += (bin_cnstr[i][0] + bin_cnstr[i][1]) / 2 - (
            bin_cnstr[refind][0] + bin_cnstr[refind][1]) / 2  # y
        target_pose[2] += (bin_cnstr[i][4] + bin_cnstr[i][5]) / 2 - (
            bin_cnstr[refind][4] + bin_cnstr[refind][5]) / 2  # z
        target_poses.append(target_pose)

    y_deltas = [0.08, -0.08]
    caption = ['left', 'right']
    max_score = -100
    max_obj_pose = None
    for i in range(len(y_deltas)):
        # 2. plan to target
        tip_hand_transform = xyzrpy_from_xyzquat(
            [-0.067, 0.027, -0.012, -0.007, 0.704, 0.710,
             -0.002])  # need to be updated

        target_pose = copy.deepcopy(target_poses[binNum])
        target_pose[1] += y_deltas[i]
        plan = None
        planner = IK(target_tip_pos=target_pose[0:3],
                     target_tip_ori=target_pose[3:7],
                     tip_hand_transform=tip_hand_transform,
                     target_link='link_5',
                     target_joint_bb=[[6, 1.2, 1.2]])
        for j in range(15):
            planner.ori_tol = 0.5 * j / 10.0
            planner.pos_tol = 0.01 * j / 10.0

            newplan = planner.plan()
            if newplan.success():
                plan = newplan
                print '[Percept] hand planning success at the trial %d' % j
                break

        if plan:
            plan.setSpeedByName('fastest')
            plan.visualize()

            if withPause:
                print '[Percept] Going to percept_hand %s ' % caption[i]
                pause()
            plan.execute()
        else:
            print '[Percept] hand planning failed'
            continue

        # 3. percept
        rospy.sleep(0.8)  # wait for new pointcloud
        obj_pose, score = capsen.capsen.detectOneObject(obj_id,
                                                        bin_contents,
                                                        binNum,
                                                        mode=1,
                                                        withScore=True)
        if score > max_score:
            max_obj_pose = obj_pose
            max_score = score

    # 4. move back to percept home
    planner = IKJoint(target_joint_pos=home_joint_pose)
    plan = planner.plan()
    plan.visualize()
    plan.setSpeedByName('yolo')
    if withPause:
        print '[Percept] Going back to percept_hand home'
        pause()
    plan.execute()

    goToHome.goToHome(robotConfig=None,
                      homePos=[1, 0, 1.2],
                      isExecute=isExecute,
                      withPause=withPause)

    if max_obj_pose is None:
        return None, None, None

    pose_type = find_object_pose_type(obj_id, max_obj_pose)

    return max_obj_pose, pose_type, max_score