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
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)