def addTasks(self):
# some helpers
self.folder = None
def addTask(task, parent=None):
parent = parent or self.folder
self.taskTree.onAddTask(task, copy=False, parent=parent)
def addFunc(name, func, parent=None):
addTask(rt.CallbackTask(callback=func, name=name), parent=parent)
def addFolder(name, parent=None):
self.folder = self.taskTree.addGroup(name, parent=parent)
return self.folder
def addManipTask(name, planFunc, userPrompt=False):
prevFolder = self.folder
addFolder(name, prevFolder)
addFunc('plan', planFunc)
if not userPrompt:
addTask(rt.CheckPlanInfo(name='check manip plan info'))
else:
addTask(rt.UserPromptTask(name='approve manip plan', message='Please approve manipulation plan.'))
addFunc('execute manip plan', self.planner.commitManipPlan)
addTask(rt.WaitForManipulationPlanExecution(name='wait for manip execution'))
self.folder = prevFolder
self.taskTree.removeAllTasks()
##################
addFolder('reset')
addFunc('plan to home', self.planner.planToNominal)
addFunc('execute', self.planner.commitManipPlan)
addFunc('wait for execute', self.planner.waitForExecute)
addFunc('open gripper', self.planner.openGripper)
addTask(rt.PauseTask(name='pause'))
addFolder('fit support')
addTask(rt.UserPromptTask(name='user fit support', message='Please fit the support surface.'))
addFolder('grasp loop')
addFunc('fit object', iiwaplanning.fitObjectOnSupport)
addFunc('add grasp frames', iiwaplanning.addGraspFrames)
addFunc('plan pregrasp', self.planner.planPreGrasp)
addFunc('execute', self.planner.commitManipPlan)
addFunc('wait for execute', self.planner.waitForExecute)
addFunc('plan grasp', self.planner.planGrasp)
addFunc('execute', self.planner.commitManipPlan)
addFunc('wait for execute', self.planner.waitForExecute)
addFunc('close gripper', self.planner.closeGripper)
addTask(rt.DelayTask(name='wait', delayTime=0.25))
addFunc('plan to home', self.planner.planToNominal)
addFunc('execute', self.planner.commitManipPlan)
addFunc('wait for execute', self.planner.waitForExecute)
addFunc('open gripper', self.planner.openGripper)
def addManipulation(func, name, parent=None, confirm=False):
group = self.taskTree.addGroup(name, parent=parent)
addFunc(func, name='plan motion', parent=group)
addTask(rt.CheckPlanInfo(name='check manip plan info'), parent=group)
addFunc(v.commitManipPlan, name='execute manip plan', parent=group)
addTask(rt.WaitForManipulationPlanExecution(name='wait for manip execution'),
parent=group)
if confirm:
addTask(rt.UserPromptTask(name='Confirm execution has finished', message='Continue when plan finishes.'),
parent=group)
def addManipTask(name, planFunc, userPrompt=False):
prevFolder = self.folder
addFolder(name, prevFolder)
addFunc('plan', planFunc)
if not userPrompt:
addTask(rt.CheckPlanInfo(name='check manip plan info'))
else:
addTask(rt.UserPromptTask(name='approve manip plan', message='Please approve manipulation plan.'))
addFunc('execute manip plan', self.switchPlanner.commitManipPlan)
addTask(rt.WaitForManipulationPlanExecution(name='wait for manip execution'))
self.folder = prevFolder
def addTasks(self):
# some helpers
self.folder = None
def addTask(task, parent=None):
parent = parent or self.folder
self.taskTree.onAddTask(task, copy=False, parent=parent)
def addFunc(func, name, parent=None):
addTask(rt.CallbackTask(callback=func, name=name), parent=parent)
def addFolder(name, parent=None):
self.folder = self.taskTree.addGroup(name, parent=parent)
return self.folder
def addManipTask(name, planFunc, userPrompt=False, planner=None):
if planner is None:
planner = self.platformPlanner
prevFolder = self.folder
addFolder(name, prevFolder)
addFunc(planFunc, 'plan')
if not userPrompt:
addTask(rt.CheckPlanInfo(name='check manip plan info'))
else:
addTask(rt.UserPromptTask(name='approve manip plan', message='Please approve manipulation plan.'))
addFunc(planner.commitManipPlan, name='execute manip plan')
addTask(rt.WaitForManipulationPlanExecution(name='wait for manip execution'))
self.folder = prevFolder
pp = self.platformPlanner
ep = self.egressPlanner
stepPrep = addFolder('Prep')
self.folder = stepPrep
addTask(rt.UserPromptTask(name="Verify SE processes", message="Please confirm that all SE processes have started"))
addTask(rt.UserPromptTask(name="Run Init Nav", message='Please click "Init Nav"'))
addTask(rt.UserPromptTask(name="Stop April Tags", message='Please stop the "April Tags" process'))
addTask(rt.UserPromptTask(name="Confirm pressure", message='Set high pressure for egress'))
addTask(rt.UserPromptTask(name="Disable recovery and bracing", message="Please disable recovery and bracing"))
addTask(rt.SetNeckPitch(name='set neck position', angle=60))
stepOut = addFolder('Step out of car')
self.folder = stepOut
addManipTask('Get weight over feet', ep.planGetWeightOverFeet, userPrompt=True, planner=ep)
addManipTask('Stand up', ep.planStandUp, userPrompt=True, planner=ep)
addManipTask('Step out', ep.planFootEgress, userPrompt=True, planner=ep)
addManipTask('Move arms up for walking', ep.planArmsForward, userPrompt=True, planner=ep)
prep = addFolder('Step down prep')
addFunc(self.onStart, 'start')
addFunc(pp.switchToPolarisPlatformParameters, "Switch walking params to 'Polaris Platform")
addTask(rt.UserPromptTask(name="wait for lidar", message="Please wait for next lidar sweep"))
self.folder = prep
addFunc(pp.fitRunningBoardAtFeet, 'fit running board')
addTask(rt.FindAffordance(name='confirm running board affordance', affordanceName='running board'))
addFunc(pp.spawnGroundAffordance, 'spawn ground affordance')
addFunc(pp.requestRaycastTerrain, 'raycast terrain')
addTask(rt.UserPromptTask(name="wait for raycast terrain", message="wait for raycast terrain"))
folder = addFolder('Step Down')
addFunc(self.onPlanStepDown, 'plan step down')
addTask(rt.UserPromptTask(name="approve footsteps, set support contact group",
message="Please approve/modify footsteps."))
addFunc(self.robotSystem.footstepsDriver.onExecClicked, 'commit footstep plan')
addTask(rt.WaitForWalkExecution(name='wait for walking'))
folder = addFolder('Step Off')
# addTask(rt.UserPromptTask(name="wait for lidar sweep", message="wait for lidar sweep before spawning ground affordance"))
addFunc(pp.spawnFootplaneGroundAffordance, 'spawn footplane ground affordance')
addFunc(pp.requestRaycastTerrain, 'raycast terrain')
addTask(rt.UserPromptTask(name="wait for raycast terrain", message="wait for raycast terrain"))
addFunc(self.onPlanStepOff, 'plan step off')
addTask(rt.UserPromptTask(name="approve footsteps", message="Please approve footsteps, modify if necessary"))
addFunc(self.robotSystem.footstepsDriver.onExecClicked, 'commit footstep plan')
addTask(rt.WaitForWalkExecution(name='wait for walking'))
addManipTask('plan nominal', pp.planNominal, userPrompt=True)
addTask(rt.UserPromptTask(name="reset walking parameters", message="Please set walking parameters to drake nominal"))
addTask(rt.SetNeckPitch(name='set neck position', angle=20))
def addFunc(func, name, parent=None, confirm=False):
addTask(rt.CallbackTask(callback=func, name=name), parent=parent)
if confirm:
addTask(rt.UserPromptTask(name='Confirm execution has finished', message='Continue when plan finishes.'), parent=parent)
def addSwitchTasks(self):
# some helpers
self.folder = None
def addTask(task, parent=None):
parent = parent or self.folder
self.taskTree.onAddTask(task, copy=False, parent=parent)
def addFunc(name, func, parent=None):
addTask(rt.CallbackTask(callback=func, name=name), parent=parent)
def addFolder(name, parent=None):
self.folder = self.taskTree.addGroup(name, parent=parent)
return self.folder
def addManipTask(name, planFunc, userPrompt=False):
prevFolder = self.folder
addFolder(name, prevFolder)
addFunc('plan', planFunc)
if not userPrompt:
addTask(rt.CheckPlanInfo(name='check manip plan info'))
else:
addTask(
rt.UserPromptTask(
name='approve manip plan',
message='Please approve manipulation plan.'))
addFunc('execute manip plan', self.switchPlanner.commitManipPlan)
addTask(
rt.WaitForManipulationPlanExecution(
name='wait for manip execution'))
self.folder = prevFolder
self.taskTree.removeAllTasks()
side = self.getSide()
addFolder('Fit Box Affordance')
addFunc('fit switch box affordance', self.fitSwitchBox)
addTask(
rt.UserPromptTask(name='verify/adjust affordance',
message='verify/adjust affordance.'))
# walk to drill
addFolder('Walk')
addFunc('plan footstep frame', self.switchPlanner.spawnFootstepFrame)
addTask(
rt.RequestFootstepPlan(name='plan walk to drill',
stanceFrameName='switch box stance frame'))
addTask(
rt.UserPromptTask(name='approve footsteps',
message='Please approve footstep plan.'))
addTask(
rt.CommitFootstepPlan(
name='walk to switch box',
planName='switch box stance frame footstep plan'))
addTask(rt.WaitForWalkExecution(name='wait for walking'))
armsUp = addFolder('Arms Up')
addManipTask('Arms Up 1',
self.switchPlanner.planArmsPrep1,
userPrompt=True)
self.folder = armsUp
addManipTask('Arms Up 2',
self.switchPlanner.planArmsPrep2,
userPrompt=True)
addTask(
rt.CloseHand(side='Right', mode='Pinch', name='set finger pinch'))
reach = addFolder('Reach')
addFunc('set degrees per second 30', self.setParamsPreTeleop)
addFunc('update reach frame', self.switchPlanner.updateReachFrame)
addTask(
rt.UserPromptTask(name='adjust frame',
message='adjust reach frame if necessary'))
addManipTask('reach above box', self.onPlanPinchReach, userPrompt=True)
teleop = addFolder('Teleop')
addFunc('set degrees per second 10', self.setParamsTeleop)
addTask(
rt.UserPromptTask(name='wait for teleop',
message='continue when finished with task.'))
armsDown = addFolder('Arms Down')
addTask(
rt.UserPromptTask(
name='check left hand free',
message='check left hand free to close and move back'))
addTask(rt.CloseHand(name='close left hand', side='Right'))
addManipTask('Arms Down 1',
self.switchPlanner.planArmsPrep2,
userPrompt=True)
self.folder = armsDown
self.folder = armsDown
addManipTask('Arms Down 2',
self.switchPlanner.planArmsPrep1,
userPrompt=True)
self.folder = armsDown
addManipTask('plan nominal',
self.switchPlanner.planNominal,
userPrompt=True)
