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 addManipulation(func, name, parent=None):
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)
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 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)
def addTasks(self):
# some helpers
def addTask(task, parent=None):
self.taskTree.onAddTask(task, copy=False, parent=parent)
def addFunc(func, name, parent=None):
addTask(rt.CallbackTask(callback=func, name=name), parent=parent)
def addManipulation(func, name, parent=None):
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)
addTask(rt.UserPromptTask(name='Confirm execution has finished', message='Continue when plan finishes.'),
parent=group)
v = self.tableDemo
self.taskTree.removeAllTasks()
# graspingHand is 'left', side is 'Left'
side = self.params.getPropertyEnumValue('Hand')
###############
# add the tasks
# prep
prep = self.taskTree.addGroup('Preparation')
addTask(rt.CloseHand(name='close grasp hand', side=side), parent=prep)
addTask(rt.CloseHand(name='close left hand', side='Left'), parent=prep)
addTask(rt.CloseHand(name='close right hand', side='Right'), parent=prep)
addFunc(v.prepIhmcDemoSequenceFromFile, 'prep from file', parent=prep)
# walk
walk = self.taskTree.addGroup('Approach Table')
addTask(rt.RequestFootstepPlan(name='plan walk to table', stanceFrameName='table stance frame'), parent=walk)
addTask(rt.UserPromptTask(name='approve footsteps',
message='Please approve footstep plan.'), parent=walk)
addTask(rt.CommitFootstepPlan(name='walk to table',
planName='table grasp stance footstep plan'), parent=walk)
addTask(rt.SetNeckPitch(name='set neck position', angle=35), parent=walk)
addTask(rt.WaitForWalkExecution(name='wait for walking'), parent=walk)
# lift object
# Not Collision Free:
addManipulation(functools.partial(v.planPreGrasp, v.graspingHand ), name='raise arm') # seems to ignore arm side?
addManipulation(functools.partial(v.planReachToTableObject, v.graspingHand), name='reach')
# Collision Free:
#addManipulation(functools.partial(v.planReachToTableObjectCollisionFree, v.graspingHand), name='reach')
addFunc(functools.partial(v.graspTableObject, side=v.graspingHand), 'grasp', parent='reach')
addManipulation(functools.partial(v.planLiftTableObject, v.graspingHand), name='lift object')
# walk to start
walkToStart = self.taskTree.addGroup('Walk to Start')
addTask(rt.RequestFootstepPlan(name='plan walk to start', stanceFrameName='start stance frame'), parent=walkToStart)
addTask(rt.UserPromptTask(name='approve footsteps',
message='Please approve footstep plan.'), parent=walkToStart)
addTask(rt.CommitFootstepPlan(name='walk to start',
planName='start stance footstep plan'), parent=walkToStart)
addTask(rt.WaitForWalkExecution(name='wait for walking'), parent=walkToStart)
# walk to bin
walkToBin = self.taskTree.addGroup('Walk to Bin')
addTask(rt.RequestFootstepPlan(name='plan walk to bin', stanceFrameName='bin stance frame'), parent=walkToBin)
addTask(rt.UserPromptTask(name='approve footsteps',
message='Please approve footstep plan.'), parent=walkToBin)
addTask(rt.CommitFootstepPlan(name='walk to start',
planName='bin stance footstep plan'), parent=walkToBin)
addTask(rt.WaitForWalkExecution(name='wait for walking'), parent=walkToBin)
# drop in bin
addManipulation(functools.partial(v.planDropPostureRaise, v.graspingHand), name='drop: raise arm') # seems to ignore arm side?
addFunc(functools.partial(v.dropTableObject, side=v.graspingHand), 'drop', parent='drop: release')
addManipulation(functools.partial(v.planDropPostureLower, v.graspingHand), name='drop: lower arm')
def addTasks(self):
# some helpers
def addTask(task, parent=None):
self.taskTree.onAddTask(task, copy=False, parent=parent)
def addFunc(func, name, parent=None):
addTask(rt.CallbackTask(callback=func, name=name), parent=parent)
def addManipulation(func, name, parent=None):
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)
addTask(rt.UserPromptTask(name='Confirm execution has finished',
message='Continue when plan finishes.'),
parent=group)
def addLargeValveTurn(parent=None):
group = self.taskTree.addGroup('Valve Turn', parent=parent)
# valve manip actions
addManipulation(functools.partial(
v.planReach, wristAngleCW=self.initialWristAngleCW),
name='Reach to valve',
parent=group)
addManipulation(functools.partial(
v.planTouch, wristAngleCW=self.initialWristAngleCW),
name='Insert hand',
parent=group)
addManipulation(functools.partial(
v.planTurn, wristAngleCW=self.finalWristAngleCW),
name='Turn valve',
parent=group)
addManipulation(v.planRetract, name='Retract hand', parent=group)
def addSmallValveTurn(parent=None):
group = self.taskTree.addGroup('Valve Turn', parent=parent)
side = 'Right' if v.graspingHand == 'right' else 'Left'
addManipulation(functools.partial(
v.planReach, wristAngleCW=self.initialWristAngleCW),
name='Reach to valve',
parent=group)
addManipulation(functools.partial(
v.planTouch, wristAngleCW=self.initialWristAngleCW),
name='Insert hand',
parent=group)
addTask(rt.CloseHand(name='grasp valve',
side=side,
mode='Basic',
amount=self.valveDemo.closedAmount),
parent=group)
addManipulation(functools.partial(
v.planTurn, wristAngleCW=self.finalWristAngleCW),
name='plan turn valve',
parent=group)
addTask(rt.CloseHand(name='release valve',
side=side,
mode='Basic',
amount=self.valveDemo.openAmount),
parent=group)
addManipulation(v.planRetract, name='plan retract', parent=group)
v = self.valveDemo
self.taskTree.removeAllTasks()
side = self.params.getPropertyEnumValue('Hand')
###############
# add the tasks
# prep
prep = self.taskTree.addGroup('Preparation')
addTask(rt.CloseHand(name='close left hand', side='Left'), parent=prep)
addTask(rt.CloseHand(name='close right hand', side='Right'),
parent=prep)
# fit
fit = self.taskTree.addGroup('Fitting')
addTask(rt.UserPromptTask(
name='fit valve',
message='Please fit and approve valve affordance.'),
parent=fit)
addTask(rt.FindAffordance(name='check valve affordance',
affordanceName='valve'),
parent=fit)
# walk
walk = self.taskTree.addGroup('Approach')
addFunc(v.planFootstepsToStance, 'plan walk to valve', parent=walk)
addTask(rt.UserPromptTask(name='approve footsteps',
message='Please approve footstep plan.'),
parent=walk)
addTask(rt.CommitFootstepPlan(
name='walk to valve', planName='valve grasp stance footstep plan'),
parent=walk)
addTask(rt.SetNeckPitch(name='set neck position', angle=35),
parent=walk)
addTask(rt.WaitForWalkExecution(name='wait for walking'), parent=walk)
# refit
refit = self.taskTree.addGroup('Re-fitting')
addTask(rt.UserPromptTask(
name='fit valve',
message='Please fit and approve valve affordance.'),
parent=refit)
# set fingers
addTask(rt.CloseHand(name='set finger positions',
side=side,
mode='Basic',
amount=self.valveDemo.openAmount),
parent=refit)
# add valve turns
if v.smallValve:
for i in range(0, 2):
addSmallValveTurn()
else:
for i in range(0, 2):
addLargeValveTurn()
# go to finishing posture
prep = self.taskTree.addGroup('Prep for walking')
addTask(rt.CloseHand(name='close left hand', side='Left'), parent=prep)
addTask(rt.CloseHand(name='close right hand', side='Right'),
parent=prep)
addTask(rt.PlanPostureGoal(name='plan walk posture',
postureGroup='General',
postureName='safe nominal',
side='Default'),
parent=prep)
addTask(rt.CommitManipulationPlan(
name='execute manip plan', planName='safe nominal posture plan'),
parent=prep)
addTask(rt.WaitForManipulationPlanExecution(
name='wait for manip execution'),
parent=prep)
