Exemple #1
0
    def autonomousExecute(self):
        '''
        Use global variable self.useDevelopment to switch between simulation and real robot execution
        '''
        #self.ikPlanner.ikServer.usePointwise = True
        #self.ikPlanner.ikServer.maxDegreesPerSecond = 20

        taskQueue = AsyncTaskQueue()
        #self.addTasksToQueueInit(taskQueue)

        # Go home
        if not self.ikPlanner.fixedBaseArm:
            self.addTasksToQueueWalking(taskQueue, self.startStanceFrame.transform, 'Walk to Start')

        for _ in self.clusterObjects:
            # Pick Objects from table:
            if not self.ikPlanner.fixedBaseArm:
                self.addTasksToQueueWalking(taskQueue, self.tableStanceFrame.transform, 'Walk to Table')
            taskQueue.addTask(self.printAsync('Pick with Left Arm'))
            self.addTasksToQueueTablePick(taskQueue, 'left')
            #taskQueue.addTask(self.printAsync('Pick with Right Arm'))
            #self.addTasksToQueueTablePick(taskQueue, 'right')

            # Go home
            if not self.ikPlanner.fixedBaseArm:
                self.addTasksToQueueWalking(taskQueue, self.startStanceFrame.transform, 'Walk to Start')

            # Go to Bin
            if not self.ikPlanner.fixedBaseArm:
                self.addTasksToQueueWalking(taskQueue, self.binStanceFrame.transform, 'Walk to Bin')

            # Drop into the Bin:
            taskQueue.addTask(self.printAsync('Drop from Left Arm'))
            self.addTasksToQueueDropIntoBin(taskQueue, 'left')
            #taskQueue.addTask(self.printAsync('Drop from Right Arm'))
            #self.addTasksToQueueDropIntoBin(taskQueue, 'right')

            # Go home
            if not self.ikPlanner.fixedBaseArm:
                self.addTasksToQueueWalking(taskQueue, self.startStanceFrame.transform, 'Walk to Start')
        
        taskQueue.addTask(self.printAsync('done!'))

        return taskQueue
Exemple #2
0
    def autonomousExecuteRoomMap(self):
        self.graspingHand = 'left'
        self.targetSweepType = 'orientation'
        self.graspToHandLinkFrame = self.ikPlanner.newGraspToHandFrame(self.graspingHand)
        self.planFromCurrentRobotState = True
        self.visOnly = False
        self.ikPlanner.ikServer.maxDegreesPerSecond = 3#5
        self.currentYawDegrees = 60
        self.fromTop = True
        self.mapFolder=om.getOrCreateContainer('room mapping')

        taskQueue = AsyncTaskQueue()
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        self.addTasksToQueueSweep(taskQueue)
        taskQueue.addTask(self.printAsync('done!'))
        taskQueue.addTask(self.doneIndicator)
        return taskQueue
Exemple #3
0
    def autonomousExecute(self):

        self.planFromCurrentRobotState = True
        self.visOnly = False

        taskQueue = AsyncTaskQueue()

        taskQueue.addTask(self.printAsync('Plan and execute walking'))
        taskQueue.addTask( functools.partial(self.planFootsteps, self.goalTransform1) )
        taskQueue.addTask(self.optionalUserPrompt('Send footstep plan. continue? y/n: '))
        taskQueue.addTask(self.commitFootstepPlan)
        taskQueue.addTask(self.waitForWalkExecution)

        taskQueue.addTask(self.printAsync('Plan and execute walking'))
        taskQueue.addTask( functools.partial(self.planFootsteps, self.goalTransform2) )
        taskQueue.addTask(self.optionalUserPrompt('Send footstep plan. continue? y/n: '))
        taskQueue.addTask(self.commitFootstepPlan)
        taskQueue.addTask(self.waitForWalkExecution)

        taskQueue.addTask(self.sendAutonmousTestDone)

        return taskQueue
Exemple #4
0
    def autonomousExecute(self):


        taskQueue = AsyncTaskQueue()

        # require affordance at start
        taskQueue.addTask(self.printAsync('get affordance'))
        taskQueue.addTask(self.ikPlanner.getAffordanceFrame)

        # stand, open hand, manip
        taskQueue.addTask(self.printAsync('send behavior start commands'))
        taskQueue.addTask(self.userPrompt('stand and open hand. continue? y/n: '))
        taskQueue.addTask(self.atlasDriver.sendManipCommand)
        taskQueue.addTask(self.openHand)
        taskQueue.addTask(self.delay(3.0))
        taskQueue.addTask(self.sendHeightMode)
        taskQueue.addTask(self.atlasDriver.sendManipCommand)

        # user prompt
        taskQueue.addTask(self.userPrompt('sending neck pitch down. continue? y/n: '))

        # set neck pitch
        taskQueue.addTask(self.printAsync('neck pitch down'))
        taskQueue.addTask(self.sendNeckPitchLookDown)
        taskQueue.addTask(self.delay(1.0))

        # user prompt
        taskQueue.addTask(self.userPrompt('perception and fitting. continue? y/n: '))

        # perception & fitting
        #taskQueue.addTask(self.printAsync('waiting for clean lidar sweep'))
        #taskQueue.addTask(self.waitForCleanLidarSweepAsync)
        #taskQueue.addTask(self.printAsync('user fit affordance'))
        #taskQueue.addTask(self.pauseQueue)

        # compute grasp & stance
        taskQueue.addTask(self.printAsync('grasp search'))
        taskQueue.addTask(self.search)

        # user select end pose
        taskQueue.addTask(self.printAsync('user select end pose'))
        #taskQueue.addTask(self.pauseQueue)

        # compute pre grasp plan
        taskQueue.addTask(self.preGrasp)

        # user prompt
        taskQueue.addTask(self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit pre grasp plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))


        # perception & fitting
        taskQueue.addTask(self.printAsync('user fit affordance'))
        taskQueue.addTask(self.toggleAffordanceEdit)
        taskQueue.addTask(self.pauseQueue)

        #taskQueue.addTask(self.printAsync('waiting for clean lidar sweep'))
        #taskQueue.addTask(self.waitForCleanLidarSweepAsync)
        #taskQueue.addTask(self.printAsync('refit affordance'))
        #taskQueue.addTask(self.refitFunction)


        # compute pre grasp plan
        taskQueue.addTask(self.grasp)

        # user prompt
        taskQueue.addTask(self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit grasp plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))

        # user prompt
        taskQueue.addTask(self.userPrompt('closing hand. continue? y/n: '))

        # close hand
        taskQueue.addTask(self.printAsync('close hand'))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(2.0))
        taskQueue.addTask(self.closeHand)

        # compute retract plan
        taskQueue.addTask(self.retract)

        # user prompt
        taskQueue.addTask(self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit retract plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(0.1))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.1))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(1.0))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(1.0))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(1.0))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(6.0))


        # compute extend arm plan
        taskQueue.addTask(self.extendArm)

        # user prompt
        taskQueue.addTask(self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit extend arm plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))

        # user prompt
        taskQueue.addTask(self.userPrompt('opening hand. continue? y/n: '))

        # open hand
        taskQueue.addTask(self.printAsync('open hand'))
        taskQueue.addTask(self.openHand)
        taskQueue.addTask(self.delay(2.0))

        # compute nominal pose plan
        taskQueue.addTask(self.drop)

        # user prompt
        taskQueue.addTask(self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit nominal pose plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))


        taskQueue.addTask(self.printAsync('done!'))

        return taskQueue
Exemple #5
0
    def startServerAsync(self):

        self.comm = matlab.MatlabCommunicator(self._createMatlabClient())
        self.comm.echoToStdOut = False
        self.comm.outputConsole = self.outputConsole

        taskQueue = AsyncTaskQueue()
        taskQueue.addTask(functools.partial(self.comm.sendCommandsAsync, ['\n']))
        taskQueue.addTask(self._checkServerRestarted)
        taskQueue.addTask(self._sendStartupCommands)
        taskQueue.addTask(self._checkServerStartup)
        taskQueue.addTask(self._notifyStartupCompleted)
        taskQueue.addTask(functools.partial(setattr, self.comm, 'echoToStdOut', True))

        self.taskQueue = taskQueue
        self.taskQueue.start()
Exemple #6
0
    def autonomousRoomMapNew(self, side='left'):
        taskQueue = AsyncTaskQueue()
        lowSpeed = 5
        highSpeed = 30
        delayTime = 3 # TODO: for potential self.delay to wait for pointclouds to be registered

        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'General', 'arm up pregrasp'))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, highSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p1_up'))
        taskQueue.addTask(self.animateLastPlan)
        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, lowSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p1_down', side=side))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, highSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p2_down', side=side))
        taskQueue.addTask(self.animateLastPlan)
        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, lowSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p2_up', side=side))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, highSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p3_up', side=side))
        taskQueue.addTask(self.animateLastPlan)
        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, lowSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p3_down', side=side))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, highSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p4_down', side=side))
        taskQueue.addTask(self.animateLastPlan)
        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, lowSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p4_up', side=side))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, highSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p5_up', side=side))
        taskQueue.addTask(self.animateLastPlan)
        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, lowSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'roomMapping', 'p5_down', side=side))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(functools.partial(self.setMaxDegreesPerSecond, highSpeed))
        taskQueue.addTask(functools.partial(self.planPostureFromDatabase, 'General', 'arm up pregrasp', side=side))
        taskQueue.addTask(self.animateLastPlan)

        taskQueue.addTask(self.doneIndicator)
        return taskQueue
Exemple #7
0
    def autonomousExecute(self):

        taskQueue = AsyncTaskQueue()

        # require affordance at start
        taskQueue.addTask(self.printAsync('get affordance'))
        taskQueue.addTask(self.ikPlanner.getAffordanceFrame)

        # stand, open hand, manip
        taskQueue.addTask(self.printAsync('send behavior start commands'))
        taskQueue.addTask(
            self.userPrompt('stand and open hand. continue? y/n: '))
        taskQueue.addTask(self.atlasDriver.sendManipCommand)
        taskQueue.addTask(self.openHand)
        taskQueue.addTask(self.delay(3.0))
        taskQueue.addTask(self.sendHeightMode)
        taskQueue.addTask(self.atlasDriver.sendManipCommand)

        # user prompt
        taskQueue.addTask(
            self.userPrompt('sending neck pitch down. continue? y/n: '))

        # set neck pitch
        taskQueue.addTask(self.printAsync('neck pitch down'))
        taskQueue.addTask(self.sendNeckPitchLookDown)
        taskQueue.addTask(self.delay(1.0))

        # user prompt
        taskQueue.addTask(
            self.userPrompt('perception and fitting. continue? y/n: '))

        # perception & fitting
        #taskQueue.addTask(self.printAsync('waiting for clean lidar sweep'))
        #taskQueue.addTask(self.waitForCleanLidarSweepAsync)
        #taskQueue.addTask(self.printAsync('user fit affordance'))
        #taskQueue.addTask(self.pauseQueue)

        # compute grasp & stance
        taskQueue.addTask(self.printAsync('grasp search'))
        taskQueue.addTask(self.search)

        # user select end pose
        taskQueue.addTask(self.printAsync('user select end pose'))
        #taskQueue.addTask(self.pauseQueue)

        # compute pre grasp plan
        taskQueue.addTask(self.preGrasp)

        # user prompt
        taskQueue.addTask(
            self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit pre grasp plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))

        # perception & fitting
        taskQueue.addTask(self.printAsync('user fit affordance'))
        taskQueue.addTask(self.toggleAffordanceEdit)
        taskQueue.addTask(self.pauseQueue)

        #taskQueue.addTask(self.printAsync('waiting for clean lidar sweep'))
        #taskQueue.addTask(self.waitForCleanLidarSweepAsync)
        #taskQueue.addTask(self.printAsync('refit affordance'))
        #taskQueue.addTask(self.refitFunction)

        # compute pre grasp plan
        taskQueue.addTask(self.grasp)

        # user prompt
        taskQueue.addTask(
            self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit grasp plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))

        # user prompt
        taskQueue.addTask(self.userPrompt('closing hand. continue? y/n: '))

        # close hand
        taskQueue.addTask(self.printAsync('close hand'))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(2.0))
        taskQueue.addTask(self.closeHand)

        # compute retract plan
        taskQueue.addTask(self.retract)

        # user prompt
        taskQueue.addTask(
            self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit retract plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(0.1))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.1))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(0.2))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(1.0))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(1.0))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(1.0))
        taskQueue.addTask(self.closeHand)
        taskQueue.addTask(self.delay(6.0))

        # compute extend arm plan
        taskQueue.addTask(self.extendArm)

        # user prompt
        taskQueue.addTask(
            self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit extend arm plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))

        # user prompt
        taskQueue.addTask(self.userPrompt('opening hand. continue? y/n: '))

        # open hand
        taskQueue.addTask(self.printAsync('open hand'))
        taskQueue.addTask(self.openHand)
        taskQueue.addTask(self.delay(2.0))

        # compute nominal pose plan
        taskQueue.addTask(self.drop)

        # user prompt
        taskQueue.addTask(
            self.userPrompt('commit manip plan. continue? y/n: '))

        # commit pre grasp plan
        taskQueue.addTask(self.printAsync('commit nominal pose plan'))
        taskQueue.addTask(self.commit)
        taskQueue.addTask(self.delay(10.0))

        taskQueue.addTask(self.printAsync('done!'))

        return taskQueue
Exemple #8
0
 def pauseQueue(self):
     raise AsyncTaskQueue.PauseException()
Exemple #9
0
    def autonomousExecute(self):

        self.planFromCurrentRobotState = True
        self.visOnly = False

        taskQueue = AsyncTaskQueue()

        taskQueue.addTask(self.printAsync('Plan and execute walking'))
        taskQueue.addTask(
            functools.partial(self.planFootsteps, self.goalTransform1))
        taskQueue.addTask(
            self.optionalUserPrompt('Send footstep plan. continue? y/n: '))
        taskQueue.addTask(self.commitFootstepPlan)
        taskQueue.addTask(self.waitForWalkExecution)

        taskQueue.addTask(self.printAsync('Plan and execute walking'))
        taskQueue.addTask(
            functools.partial(self.planFootsteps, self.goalTransform2))
        taskQueue.addTask(
            self.optionalUserPrompt('Send footstep plan. continue? y/n: '))
        taskQueue.addTask(self.commitFootstepPlan)
        taskQueue.addTask(self.waitForWalkExecution)

        taskQueue.addTask(self.sendAutonmousTestDone)

        return taskQueue
Exemple #10
0
    def startServerAsync(self):

        self.comm = matlab.MatlabCommunicator(self._createMatlabClient())
        self.comm.echoToStdOut = False
        self.comm.outputConsole = self.outputConsole

        taskQueue = AsyncTaskQueue()
        taskQueue.addTask(
            functools.partial(self.comm.sendCommandsAsync, ['\n']))
        taskQueue.addTask(self._checkServerRestarted)
        taskQueue.addTask(self._sendStartupCommands)
        taskQueue.addTask(self._checkServerStartup)
        taskQueue.addTask(self._notifyStartupCompleted)
        taskQueue.addTask(
            functools.partial(setattr, self.comm, 'echoToStdOut', True))

        self.taskQueue = taskQueue
        self.taskQueue.start()