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
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
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()
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
def pauseQueue(self):
raise AsyncTaskQueue.PauseException()
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
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
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
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()
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