if useFootsteps:
footstepsPanel = footstepsdriverpanel.init(footstepsDriver, robotStateModel, robotStateJointController, irisDriver)
if useLCMGL:
lcmglManager = lcmgl.init(view)
app.MenuActionToggleHelper('Tools', 'Renderer - LCM GL', lcmglManager.isEnabled, lcmglManager.setEnabled)
if useDrakeVisualizer:
drakeVisualizer = drakevisualizer.DrakeVisualizer(view)
app.MenuActionToggleHelper('Tools', 'Renderer - Drake', drakeVisualizer.isEnabled, drakeVisualizer.setEnabled)
if useNavigationPanel:
navigationPanel = navigationpanel.init(robotStateJointController, footstepsDriver)
picker = PointPicker(view, callback=navigationPanel.pointPickerStoredFootsteps, numberOfPoints=2)
#picker.start()
if usePlanning:
def showPose(pose):
playbackRobotModel.setProperty('Visible', True)
playbackJointController.setPose('show_pose', pose)
def playPlan(plan):
playPlans([plan])
def playPlans(plans):
planPlayback.stopAnimation()
playbackRobotModel.setProperty('Visible', True)
def userFitBin(self):
self.binFrame = None
self.picker = PointPicker(self.view, numberOfPoints=2, drawLines=True, callback=self.onSegmentBin)
self.picker.start()
class TableDemo(object):
def __init__(self, robotStateModel, playbackRobotModel, ikPlanner, manipPlanner, footstepPlanner,
atlasDriver, lhandDriver, rhandDriver, multisenseDriver, view, sensorJointController,
planPlaybackFunction, teleopPanel):
self.planPlaybackFunction = planPlaybackFunction
self.robotStateModel = robotStateModel
self.playbackRobotModel = playbackRobotModel
self.ikPlanner = ikPlanner
self.manipPlanner = manipPlanner
self.footstepPlanner = footstepPlanner
self.atlasDriver = atlasDriver
self.lhandDriver = lhandDriver
self.rhandDriver = rhandDriver
self.multisenseDriver = multisenseDriver
self.sensorJointController = sensorJointController
self.view = view
self.teleopPanel = teleopPanel
# live operation flags:
self.useFootstepPlanner = True
self.visOnly = False
self.planFromCurrentRobotState = True
self.useDevelopment = False
if (self.useDevelopment):
self.visOnly = True
self.planFromCurrentRobotState = False
extraModels = [self.robotStateModel]
self.affordanceUpdater = affordanceupdater.AffordanceGraspUpdater(self.playbackRobotModel, self.ikPlanner, extraModels)
else:
extraModels = [self.playbackRobotModel]
self.affordanceUpdater = affordanceupdater.AffordanceGraspUpdater(self.robotStateModel, self.ikPlanner, extraModels)
self.optionalUserPromptEnabled = True
self.requiredUserPromptEnabled = True
self.plans = []
self.frameSyncs = {}
self.graspingHand = 'left' # left, right, both
self.tableData = None
self.binFrame = None
# top level switch between BDI or IHMC (locked base) and MIT (moving base and back)
self.lockBack = True
self.lockBase = True
self.constraintSet = []
self.reachDist = 0.07
# Switch indicating whether to use affordances as a collision environment
self.useCollisionEnvironment = True
# Switch between simulation/visualisation and real robot operation
def setMode(self, mode='visualization'):
'''
Switches between visualization and real robot operation.
mode='visualization'
mode='robot'
'''
if (mode == 'visualization'):
print "Setting mode to VISUALIZATION"
self.useDevelopment = True
self.visOnly = True
self.planFromCurrentRobotState = False
extraModels = [self.robotStateModel]
self.affordanceUpdater = affordanceupdater.AffordanceGraspUpdater(self.playbackRobotModel, self.ikPlanner, extraModels)
else:
print "Setting mode to ROBOT OPERATION"
self.useDevelopment = False
extraModels = [self.playbackRobotModel]
self.affordanceUpdater = affordanceupdater.AffordanceGraspUpdater(self.robotStateModel, self.ikPlanner, extraModels)
def addPlan(self, plan):
self.plans.append(plan)
### Table and Bin Focused Functions
def userFitTable(self):
self.tableData = None
self.picker = PointPicker(self.view, numberOfPoints=2, drawLines=True, callback=self.onSegmentTable)
self.picker.start()
def userFitBin(self):
self.binFrame = None
self.picker = PointPicker(self.view, numberOfPoints=2, drawLines=True, callback=self.onSegmentBin)
self.picker.start()
def waitForTableFit(self):
while not self.tableData:
yield
def waitForBinFit(self):
while not self.binFrame:
yield
def getInputPointCloud(self):
polyData = segmentation.getCurrentRevolutionData()
if polyData is None:
obj = om.findObjectByName('scene')
if obj:
polyData = obj.polyData
else: # fall back to map in case we used mapping rather than loading of a scene
obj = om.findObjectByName('map')
if obj:
polyData = obj.polyData
return polyData
def onSegmentTable(self, p1, p2):
print p1
print p2
self.picker.stop()
om.removeFromObjectModel(self.picker.annotationObj)
self.picker = None
om.removeFromObjectModel(om.findObjectByName('table demo'))
self.tableData = segmentation.segmentTableEdge(self.getInputPointCloud(), p1, p2)
self.tableObj = vis.showPolyData(self.tableData.mesh, 'table', parent='table demo', color=[0,1,0])
self.tableFrame = vis.showFrame(self.tableData.frame, 'table frame', parent=self.tableObj, scale=0.2)
self.tableBox = vis.showPolyData(self.tableData.box, 'table box', parent=self.tableObj, color=[0,1,0], visible=False)
self.tableObj.actor.SetUserTransform(self.tableFrame.transform)
self.tableBox.actor.SetUserTransform(self.tableFrame.transform)
if self.useCollisionEnvironment:
self.addCollisionObject(self.tableObj)
def onSegmentBin(self, p1, p2):
print p1
print p2
self.picker.stop()
om.removeFromObjectModel(self.picker.annotationObj)
self.picker = None
om.removeFromObjectModel(om.findObjectByName('bin frame'))
binEdge = p2 - p1
zaxis = [0.0, 0.0, 1.0]
xaxis = np.cross(binEdge, zaxis)
xaxis /= np.linalg.norm(xaxis)
yaxis = np.cross(zaxis, xaxis)
t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis)
t.PostMultiply()
t.Translate(p1)
self.binFrame = vis.showFrame(t, 'bin frame', parent=None, scale=0.2)
def sortClustersOnTable(self, clusters):
'''
returns list copy of clusters, sorted left to right using the
table coordinate system. (Table y axis points right to left)
'''
tableYAxis = self.tableData.axes[1]
tableOrigin = np.array(self.tableData.frame.GetPosition())
origins = [np.array(c.frame.GetPosition()) for c in clusters]
dists = [np.dot(origin-tableOrigin, -tableYAxis) for origin in origins]
return [clusters[i] for i in np.argsort(dists)]
def cleanupSegmentedObjects(self):
om.removeFromObjectModel(om.findObjectByName('segmentation'))
self.clusterObjects = None
self.segmentationData = None
def segmentTableObjects(self):
tableCentroid = segmentation.computeCentroid(self.tableData.box)
self.tableData.frame.TransformPoint(tableCentroid, tableCentroid)
data = segmentation.segmentTableScene(self.getInputPointCloud(), tableCentroid)
data.clusters = self.sortClustersOnTable(data.clusters)
self.clusterObjects = vis.showClusterObjects(data.clusters, parent='segmentation')
self.segmentationData = data
def graspTableObject(self, side):
#linkName = self.ikPlanner.getHandLink(side)
#t = self.ikPlanner.getLinkFrameAtPose(linkName, self.getPlanningStartPose())
#linkFrame = vis.updateFrame(t, '%s frame' % linkName, scale=0.2, visible=False, parent='planning')
obj, objFrame = self.getNextTableObject(side)
#frameSync = vis.FrameSync()
#frameSync.addFrame(linkFrame)
#frameSync.addFrame(objFrame)
#self.frameSyncs[linkName] = frameSync
#self.playbackRobotModel.connectModelChanged(self.onRobotModelChanged)
self.affordanceUpdater.graspAffordance( obj.getProperty('Name') , side)
def dropTableObject(self, side='left'):
obj, _ = self.getNextTableObject(side)
obj.setProperty('Visible', False)
for child in obj.children():
child.setProperty('Visible', False)
self.clusterObjects.remove(obj) # remove from clusterObjects
om.removeFromObjectModel(obj) # remove from objectModel
if self.useCollisionEnvironment:
objAffordance = om.findObjectByName(obj.getProperty('Name') + ' affordance')
objAffordance.setProperty('Collision Enabled', False)
objAffordance.setProperty('Visible', False)
self.affordanceUpdater.ungraspAffordance(obj.getProperty('Name'))
def getNextTableObject(self, side='left'):
assert len(self.clusterObjects)
obj = self.clusterObjects[0] if side == 'left' else self.clusterObjects[-1]
frameObj = obj.findChild(obj.getProperty('Name') + ' frame')
if self.useCollisionEnvironment:
self.prepCollisionEnvironment()
collisionObj = om.findObjectByName(obj.getProperty('Name') + ' affordance')
collisionObj.setProperty('Collision Enabled', False)
return obj, frameObj
def computeTableStanceFrame(self):
assert self.tableData
zGround = 0.0
tableHeight = self.tableData.frame.GetPosition()[2] - zGround
t = transformUtils.copyFrame(self.tableData.frame)
t.PreMultiply()
t1 = transformUtils.frameFromPositionAndRPY([-x/2 for x in self.tableData.dims],[0,0,0])
t.Concatenate(t1)
t2 = transformUtils.frameFromPositionAndRPY([-0.35, self.tableData.dims[1]*0.5, -tableHeight],[0,0,0])
t.Concatenate(t2)
self.tableStanceFrame = vis.showFrame(t, 'table stance frame', parent=self.tableObj, scale=0.2)
def computeBinStanceFrame(self):
assert self.binFrame
zGround = 0.0
binHeight = self.binFrame.transform.GetPosition()[2] - zGround
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(-0.45, 0.1, -binHeight)
t.Concatenate(self.binFrame.transform)
self.binStanceFrame = vis.showFrame(t, 'bin stance frame', parent=None, scale=0.2)
t = vtk.vtkTransform()
t.PostMultiply()
t.RotateZ(30)
t.Translate(-0.8, 0.4, -binHeight)
t.Concatenate(self.binFrame.transform)
self.startStanceFrame = vis.showFrame(t, 'start stance frame', parent=None, scale=0.2)
# TODO: deprecate this function: (to end of section):
def moveRobotToTableStanceFrame(self):
self.teleportRobotToStanceFrame(self.tableStanceFrame.transform)
def moveRobotToBinStanceFrame(self):
self.teleportRobotToStanceFrame(self.binStanceFrame.transform)
def moveRobotToStartStanceFrame(self):
self.teleportRobotToStanceFrame(self.startStanceFrame.transform)
def planFootstepsToTable(self):
self.planFootsteps(self.tableStanceFrame.transform)
def planFootstepsToBin(self):
self.planFootsteps(self.binStanceFrame.transform)
def planFootstepsToStart(self):
self.planFootsteps(self.startStanceFrame.transform)
### End Object Focused Functions ###############################################################
### Planning Functions ########################################################################
def planFootsteps(self, goalFrame):
startPose = self.getPlanningStartPose()
request = self.footstepPlanner.constructFootstepPlanRequest(startPose, goalFrame)
self.footstepPlan = self.footstepPlanner.sendFootstepPlanRequest(request, waitForResponse=True)
def planWalking(self):
startPose = self.getPlanningStartPose()
plan = self.footstepPlanner.sendWalkingPlanRequest(self.footstepPlan, startPose, waitForResponse=True)
self.addPlan(plan)
def planWalkToStance(self, stanceTransform):
if self.useFootstepPlanner:
self.planFootsteps(stanceTransform)
self.planWalking()
else:
self.teleportRobotToStanceFrame(stanceTransform)
def planPostureFromDatabase(self, groupName, postureName, side='left'):
startPose = self.getPlanningStartPose()
endPose = self.ikPlanner.getMergedPostureFromDatabase(startPose, groupName, postureName, side=side)
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
self.addPlan(newPlan)
# TODO: integrate this function with the ones below
def getRaisedArmPose(self, startPose, side):
return self.ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'arm up pregrasp', side)
def getPreDropHighPose(self, startPose, side):
return self.ikPlanner.getMergedPostureFromDatabase(startPose, 'table clearing', 'pre drop 1', side)
def getPreDropLowPose(self, startPose, side):
return self.ikPlanner.getMergedPostureFromDatabase(startPose, 'table clearing', 'pre drop 2', side)
def getLoweredArmPose(self, startPose, side):
return self.ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'handdown', side)
def planPreGrasp(self, side='left'):
startPose = self.getPlanningStartPose()
endPose = self.ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'arm up pregrasp', side=side)
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
self.addPlan(newPlan)
def planLowerArm(self, side):
startPose = self.getPlanningStartPose()
endPose = self.ikPlanner.getMergedPostureFromDatabase(startPose, 'General', 'handdown', side=side)
newPlan = self.ikPlanner.computePostureGoal(startPose, endPose)
self.addPlan(newPlan)
def planDropPostureRaise(self, side):
startPose = self.getPlanningStartPose()
poseA = self.getRaisedArmPose(startPose, side)
poseB = self.getPreDropHighPose(startPose, side)
poseC = self.getPreDropLowPose(startPose, side)
plan = self.ikPlanner.computeMultiPostureGoal([startPose, poseA, poseB, poseC])
self.addPlan(plan)
def planDropPostureLower(self, side):
startPose = self.getPlanningStartPose()
poseA = self.getPreDropHighPose(startPose, side)
poseB = self.getRaisedArmPose(startPose, side)
poseC = self.getLoweredArmPose(startPose, side)
plan = self.ikPlanner.computeMultiPostureGoal([startPose, poseA, poseB, poseC])
self.addPlan(plan)
def planDropPostureSwap(self, lowerSide, raiseSide):
startPose = self.getPlanningStartPose()
poseA = self.getRaisedArmPose(startPose, raiseSide)
poseA = self.getPreDropHighPose(poseA, lowerSide)
poseB = self.getPreDropHighPose(poseA, raiseSide)
poseB = self.getRaisedArmPose(poseB, lowerSide)
poseC = self.getPreDropLowPose(poseB, raiseSide)
poseC = self.getLoweredArmPose(poseC, lowerSide)
plan = self.ikPlanner.computeMultiPostureGoal([startPose, poseA, poseB, poseC])
self.addPlan(plan)
def planLowerArmAndStand(self, side):
startPose = self.getPlanningStartPose()
endPose = self.getLoweredArmPose(startPose, side)
endPose, info = self.ikPlanner.computeStandPose(endPose)
plan = self.ikPlanner.computePostureGoal(startPose, endPose)
self.addPlan(plan)
def planReachToTableObject(self, side='left'):
obj, frame = self.getNextTableObject(side)
startPose = self.getPlanningStartPose()
if self.ikPlanner.fixedBaseArm: # includes reachDist hack instead of in ikPlanner (TODO!)
f = transformUtils.frameFromPositionAndRPY( np.array(frame.transform.GetPosition())-np.array([self.reachDist,0,0]), [0,0,-90] )
f.PreMultiply()
f.RotateY(90)
f.Update()
self.constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, side, f, lockBase=False, lockBack=True)
#newFrame = vis.FrameItem('reach_item', f, self.view)
#self.constraintSet = self.ikPlanner.planGraspOrbitReachPlan(startPose, side, newFrame, constraints=None, dist=self.reachDist, lockBase=self.lockBase, lockBack=self.lockBack, lockArm=False)
else:
self.constraintSet = self.ikPlanner.planGraspOrbitReachPlan(startPose, side, frame, constraints=None, dist=self.reachDist, lockBase=self.lockBase, lockBack=self.lockBack, lockArm=False)
loweringSide = 'left' if side == 'right' else 'right'
armPose = self.getLoweredArmPose(startPose, loweringSide)
armPoseName = 'lowered_arm_pose'
self.ikPlanner.ikServer.sendPoseToServer(armPose, armPoseName)
loweringSideJoints = []
if (loweringSide == 'left'):
loweringSideJoints += self.ikPlanner.leftArmJoints
else:
loweringSideJoints += self.ikPlanner.rightArmJoints
reachingSideJoints = []
if (side == 'left'):
reachingSideJoints += self.ikPlanner.leftArmJoints
else:
reachingSideJoints += self.ikPlanner.rightArmJoints
armPostureConstraint = self.ikPlanner.createPostureConstraint(armPoseName, loweringSideJoints)
armPostureConstraint.tspan = np.array([1.0, 1.0])
self.constraintSet.constraints.append(armPostureConstraint)
self.constraintSet.runIk()
#armPose = self.getRaisedArmPose(startPose, side)
#armPoseName = 'raised_arm_pose'
#self.ikPlanner.ikServer.sendPoseToServer(armPose, armPoseName)
#armPostureConstraint = self.ikPlanner.createPostureConstraint(armPoseName, reachingSideJoints)
#armPostureConstraint.tspan = np.array([0.5, 0.5])
#self.constraintSet.constraints.append(armPostureConstraint)
print 'planning reach to'
plan = self.constraintSet.runIkTraj()
self.addPlan(plan)
def planReachToTableObjectCollisionFree(self, side ='left'):
# Hard-coded demonstration of collision reaching to object on table
# Using RRT Connect
goalFrame = transformUtils.frameFromPositionAndRPY([1.05,0.4,1],[0,90,-90])
vis.showFrame(goalFrame,'goal frame')
frameObj = om.findObjectByName( 'goal frame')
startPose = self.getPlanningStartPose()
self.constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, side, frameObj.transform, lockBase=self.lockBase, lockBack=self.lockBack)
self.constraintSet.runIk()
print 'planning reach to planReachToTableObjectCollisionFree'
self.constraintSet.ikParameters.usePointwise = False
self.constraintSet.ikParameters.useCollision = True
self.teleopPanel.endEffectorTeleop.updateCollisionEnvironment()
plan = self.constraintSet.runIkTraj()
self.addPlan(plan)
def planTouchTableObject(self, side='left'):
obj, frame = self.getNextTableObject(side)
startPose = self.getPlanningStartPose()
if self.ikPlanner.fixedBaseArm: # includes distance hack and currently uses reachDist instead of touchDist (TODO!)
f = transformUtils.frameFromPositionAndRPY( np.array(frame.transform.GetPosition())-np.array([self.reachDist,0,0]), [0,0,-90] )
f.PreMultiply()
f.RotateY(90)
f.Update()
item = vis.FrameItem('reach_item', f, self.view)
self.constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, side, f, lockBase=False, lockBack=True)
else:
self.constraintSet = self.ikPlanner.planGraspOrbitReachPlan(startPose, side, frame, dist=0.05, lockBase=self.lockBase, lockBack=self.lockBack)
self.constraintSet.constraints[-1].tspan = [-np.inf, np.inf]
self.constraintSet.constraints[-2].tspan = [-np.inf, np.inf]
self.constraintSet.runIk()
print 'planning touch'
plan = self.constraintSet.runIkTraj()
self.addPlan(plan)
def planLiftTableObject(self, side):
startPose = self.getPlanningStartPose()
self.constraintSet = self.ikPlanner.planEndEffectorDelta(startPose, side, [0.0, 0.0, 0.15])
if not self.ikPlanner.fixedBaseArm:
self.constraintSet.constraints[-1].tspan[1] = 1.0
endPose, info = self.constraintSet.runIk()
if not self.ikPlanner.fixedBaseArm:
endPose = self.getRaisedArmPose(endPose, side)
reachingSideJoints = []
if (side == 'left'):
reachingSideJoints += self.ikPlanner.leftArmJoints
else:
reachingSideJoints += self.ikPlanner.rightArmJoints
endPoseName = 'raised_arm_end_pose'
self.ikPlanner.ikServer.sendPoseToServer(endPose, endPoseName)
postureConstraint = self.ikPlanner.createPostureConstraint(endPoseName, reachingSideJoints)
postureConstraint.tspan = np.array([2.0, 2.0])
self.constraintSet.constraints.append(postureConstraint)
#postureConstraint = self.ikPlanner.createPostureConstraint('q_nom', robotstate.matchJoints('.*_leg_kny'))
#postureConstraint.tspan = np.array([2.0, 2.0])
#self.constraintSet.constraints.append(postureConstraint)
#postureConstraint = self.ikPlanner.createPostureConstraint('q_nom', robotstate.matchJoints('back'))
#postureConstraint.tspan = np.array([2.0, 2.0])
#self.constraintSet.constraints.append(postureConstraint)
print 'planning lift'
plan = self.constraintSet.runIkTraj()
self.addPlan(plan)
### End Planning Functions ####################################################################
########## Glue Functions #####################################################################
def teleportRobotToStanceFrame(self, frame):
self.sensorJointController.setPose('q_nom')
stancePosition = frame.GetPosition()
stanceOrientation = frame.GetOrientation()
q = self.sensorJointController.q.copy()
q[:2] = [stancePosition[0], stancePosition[1]]
q[5] = math.radians(stanceOrientation[2])
self.sensorJointController.setPose('EST_ROBOT_STATE', q)
def getHandDriver(self, side):
assert side in ('left', 'right')
return self.lhandDriver if side == 'left' else self.rhandDriver
def openHand(self, side):
#self.getHandDriver(side).sendOpen()
self.getHandDriver(side).sendCustom(0.0, 100.0, 100.0, 0)
def closeHand(self, side):
self.getHandDriver(side).sendCustom(100.0, 100.0, 100.0, 0)
def sendNeckPitchLookDown(self):
self.multisenseDriver.setNeckPitch(40)
def sendNeckPitchLookForward(self):
self.multisenseDriver.setNeckPitch(15)
def waitForAtlasBehaviorAsync(self, behaviorName):
assert behaviorName in self.atlasDriver.getBehaviorMap().values()
while self.atlasDriver.getCurrentBehaviorName() != behaviorName:
yield
def printAsync(self, s):
yield
print s
def optionalUserPrompt(self, message):
if not self.optionalUserPromptEnabled:
return
yield
result = raw_input(message)
if result != 'y':
raise Exception('user abort.')
def requiredUserPrompt(self, message):
if not self.requiredUserPromptEnabled:
return
yield
result = raw_input(message)
if result != 'y':
raise Exception('user abort.')
def delay(self, delayTimeInSeconds):
yield
t = SimpleTimer()
while t.elapsed() < delayTimeInSeconds:
yield
def waitForCleanLidarSweepAsync(self):
currentRevolution = self.multisenseDriver.displayedRevolution
desiredRevolution = currentRevolution + 2
while self.multisenseDriver.displayedRevolution < desiredRevolution:
yield
def getEstimatedRobotStatePose(self):
return self.sensorJointController.getPose('EST_ROBOT_STATE')
def getPlanningStartPose(self):
if self.planFromCurrentRobotState:
return self.getEstimatedRobotStatePose()
else:
if self.plans:
return robotstate.convertStateMessageToDrakePose(self.plans[-1].plan[-1])
else:
return self.getEstimatedRobotStatePose()
def cleanupFootstepPlans(self):
om.removeFromObjectModel(om.findObjectByName('walking goal'))
om.removeFromObjectModel(om.findObjectByName('footstep plan'))
self.footstepPlan = None
def playSequenceNominal(self):
assert None not in self.plans
self.planPlaybackFunction(self.plans)
def commitManipPlan(self):
self.manipPlanner.commitManipPlan(self.plans[-1])
def commitFootstepPlan(self):
self.footstepPlanner.commitFootstepPlan(self.footstepPlan)
def waitForPlanExecution(self, plan):
planElapsedTime = planplayback.PlanPlayback.getPlanElapsedTime(plan)
return self.delay(planElapsedTime + 1.0)
def animateLastPlan(self):
plan = self.plans[-1]
if not self.visOnly:
self.commitManipPlan()
return self.waitForPlanExecution(plan)
def onRobotModelChanged(self, model):
for linkName in self.frameSyncs.keys():
t = self.playbackRobotModel.getLinkFrame(linkName)
vis.updateFrame(t, '%s frame' % linkName, scale=0.2, visible=False, parent='planning')
def createCollisionPlanningScene(self, scene=1, moveRobot=False, loadPerception=False):
if (loadPerception):
#filename = os.path.expanduser('~/drc-testing-data/collision_scene/collision_scene.vtp')
#polyData = ioUtils.readPolyData( filename )
pd = io.readPolyData('/home/mfallon/Desktop/rrt_scene/all.vtp')
vis.showPolyData(pd,'scene')
if (scene == 0):
pose = (array([ 1.20, 0. , 0.8]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-tabletop', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,1,0.06])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.20, 0.5 , 0.4]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-leg1', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,0.05,0.8])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.20, -0.5 , 0.4]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-leg2', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,0.05,0.8])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.05, 0.3 , 0.98]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-object1', uuid=newUUID(), pose=pose, Color=[0.9, 0.9, 0.1], Dimensions=[0.08,0.08,0.24])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.25, 0.1 , 0.98]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene0-object2', uuid=newUUID(), pose=pose, Color=[0.0, 0.9, 0.0], Dimensions=[0.07,0.07,0.25])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.25, -0.1 , 0.95]), array([ 1., 0., 0., 0.]))
desc = dict(classname='CylinderAffordanceItem', Name='scene0-object3', uuid=newUUID(), pose=pose, Color=[0.0, 0.9, 0.0], Radius=0.035, Length = 0.22)
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ 1.05, -0.2 , 0.95]), array([ 1., 0., 0., 0.]))
desc = dict(classname='CylinderAffordanceItem', Name='scene0-object4', uuid=newUUID(), pose=pose, Color=[0.9, 0.1, 0.1], Radius=0.045, Length = 0.22)
obj = affordancepanel.panel.affordanceFromDescription(desc)
if (moveRobot):
self.sensorJointController.q[0] = 0.67
self.sensorJointController.push()
elif (scene == 1):
pose = (array([-0.69, -1.50, 0.92]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-tabletop', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,1,0.06])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-1.05, -1.10, 0.95]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-edge1', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.1,0.3,0.05])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-0.35, -1.10, 0.95]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-edge2', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.1,0.3,0.05])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-0.6803156 , -1.1826616 , 1.31299839]), array([-0.707106781, 0. , 0. , 0.707106781 ]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-edge3', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.14,1.0,0.07])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([ -0.7, -1.5 , 1.03]), array([ 1., 0., 0., 0.]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-object1', uuid=newUUID(), pose=pose, Color=[0.9, 0.9, 0.1], Dimensions=[0.05,0.05,0.14])
obj = affordancepanel.panel.affordanceFromDescription(desc)
if (moveRobot):
self.sensorJointController.q[5] = -1.571
self.sensorJointController.q[0] = -0.75
self.sensorJointController.q[1] = -0.85
self.sensorJointController.push()
elif (scene == 2):
pose = (array([-0.98873106, 1.50393395, 0.91420001]), array([ 0.49752312, 0. , 0. , 0.86745072]))
desc = dict(classname='BoxAffordanceItem', Name='scene2-tabletop', uuid=newUUID(), pose=pose, Color=[0.66, 0.66, 0.66], Dimensions=[0.5,1,0.06])
obj = affordancepanel.panel.affordanceFromDescription(desc)
pose = (array([-0.98873106, 1.50393395, 0.57]), array([ 0.49752312, 0. , 0. , 0.86745072]))
desc = dict(classname='BoxAffordanceItem', Name='scene1-object1', uuid=newUUID(), pose=pose, Color=[0.005, 0.005, 0.3], Dimensions=[0.05,0.05,0.14])
obj = affordancepanel.panel.affordanceFromDescription(desc)
if (moveRobot):
self.sensorJointController.q[0] = -0.6
self.sensorJointController.q[1] = 1.1
self.sensorJointController.q[5] = 2.1
self.sensorJointController.push()
######### Setup collision environment ####################
def prepCollisionEnvironment(self):
assert len(self.clusterObjects)
for obj in self.clusterObjects:
self.addCollisionObject(obj)
def addCollisionObject(self, obj):
if om.getOrCreateContainer('affordances').findChild(obj.getProperty('Name') + ' affordance'):
return # Affordance has been created previously
frame = obj.findChild(obj.getProperty('Name') + ' frame')
(origin, quat) = transformUtils.poseFromTransform(frame.transform)
(xaxis, yaxis, zaxis) = transformUtils.getAxesFromTransform(frame.transform)
# TODO: move this into transformUtils as getAxisDimensions or so
box = obj.findChild(obj.getProperty('Name') + ' box')
box_np = vtkNumpy.getNumpyFromVtk(box.polyData, 'Points')
box_min = np.amin(box_np, 0)
box_max = np.amax(box_np, 0)
xwidth = np.linalg.norm(box_max[0]-box_min[0])
ywidth = np.linalg.norm(box_max[1]-box_min[1])
zwidth = np.linalg.norm(box_max[2]-box_min[2])
name = obj.getProperty('Name') + ' affordance'
boxAffordance = segmentation.createBlockAffordance(origin, xaxis, yaxis, zaxis, xwidth, ywidth, zwidth, name, parent='affordances')
boxAffordance.setSolidColor(obj.getProperty('Color'))
boxAffordance.setProperty('Alpha', 0.3)
######### Nominal Plans and Execution #################################################################
def prepKukaTestDemoSequence(self, inputFile='~/drc-testing-data/tabletop/kinect_collision_environment.vtp'):
filename = os.path.expanduser(inputFile)
scene = ioUtils.readPolyData(filename)
vis.showPolyData(scene,"scene")
self.prepKukaLabScene()
def prepKukaLabScene(self):
self.userFitTable()
self.onSegmentTable( np.array([ 0.91544128, 0.06092263, 0.14906664]), np.array([ 0.73494804, -0.21896157, 0.13435645]) )
self.userFitBin() # TODO: actually fit bin, put bin in picture.
self.onSegmentBin( np.array([-0.02, 2.43, 0.61 ]), np.array([-0.40, 2.79, 0.61964661]) ) # TODO: fix bin location
self.segmentTableObjects()
# Plan sequence
self.plans = []
def prepTestDemoSequence(self):
'''
Running this function should launch a full planning sequence
to pick to objects, walk and drop.
Requires footstep footstepPlanner
'''
filename = os.path.expanduser('~/drc-testing-data/tabletop/table-and-bin-scene.vtp')
scene = ioUtils.readPolyData(filename)
vis.showPolyData(scene,"scene")
#stanceFrame = transformUtils.frameFromPositionAndRPY([0, 0, 0], [0, 0, 123.0])
#self.teleportRobotToStanceFrame(stanceFrame)
self.userFitTable()
self.onSegmentTable( np.array([-1.72105646, 2.73210716, 0.79449952]), np.array([-1.67336452, 2.63351011, 0.78698605]) )
self.userFitBin()
self.onSegmentBin( np.array([-0.02, 2.43, 0.61 ]), np.array([-0.40, 2.79, 0.61964661]) )
self.computeTableStanceFrame()
self.computeBinStanceFrame()
# Actually plan the sequence:
#self.demoSequence()
def prepIhmcDemoSequenceFromFile(self):
filename = os.path.expanduser('~/drc-testing-data/ihmc_table/ihmc_table.vtp')
polyData = ioUtils.readPolyData( filename )
vis.showPolyData( polyData,'scene')
self.prepIhmcDemoSequence()
def prepIhmcDemoSequence(self):
self.userFitBin()
self.onSegmentBin( np.array([ 0.62, -1.33, 0.80]), np.array([ 0.89, -0.87, 0.57]) )
self.userFitTable()
self.onSegmentTable( np.array([ 1.11, 0.11, 0.85]), np.array([ 0.97, 0.044, 0.84]) )
self.segmentTableObjects()
self.computeBinStanceFrame()
self.computeTableStanceFrame()
def planSequence(self):
self.useFootstepPlanner = True
self.cleanupFootstepPlans()
self.planFromCurrentRobotState = False
self.segmentTableObjects()
self.plans = []
# Go home
self.planWalkToStance(self.startStanceFrame.transform)
# Pick Objects from table:
self.planWalkToStance(self.tableStanceFrame.transform)
if (self.graspingHand == 'both'):
self.planSequenceTablePick('left')
self.planSequenceTablePick('right')
else:
self.planSequenceTablePick(self.graspingHand)
# Go home
self.planWalkToStance(self.startStanceFrame.transform)
# Go to Bin
self.planWalkToStance(self.binStanceFrame.transform)
# Drop into the Bin:
if (self.graspingHand == 'both'):
self.planDropPostureRaise('left')
self.dropTableObject('left')
self.planDropPostureLower('left')
self.planDropPostureRaise('right')
self.dropTableObject('right')
self.planDropPostureLower('right')
else:
self.planDropPostureRaise(self.graspingHand)
self.dropTableObject(self.graspingHand)
self.planDropPostureLower(self.graspingHand)
# Go home
self.planWalkToStance(self.startStanceFrame.transform)
def planSequenceTablePick(self, side):
self.planPreGrasp(side)
if self.ikPlanner.fixedBaseArm:
self.planLowerArm(side)
self.planReachToTableObject(side)
if not self.ikPlanner.fixedBaseArm:
self.planTouchTableObject(side) # TODO: distance is handled by reach, hence ignore
self.graspTableObject(side)
self.planLiftTableObject(side)
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
def addTasksToQueueInit(self, taskQueue):
taskQueue.addTask(self.printAsync('user fit table'))
taskQueue.addTask(self.userFitTable)
taskQueue.addTask(self.waitForTableFit)
taskQueue.addTask(self.printAsync('user fit bin'))
taskQueue.addTask(self.userFitBin)
taskQueue.addTask(self.waitForBinFit)
if not self.ikPlanner.fixedBaseArm:
taskQueue.addTask(self.computeTableStanceFrame)
taskQueue.addTask(self.computeBinStanceFrame)
def addTasksToQueueTablePick(self, taskQueue, side):
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
taskQueue.addTask(functools.partial(self.planPreGrasp, side))
taskQueue.addTask(self.animateLastPlan)
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
taskQueue.addTask(functools.partial(self.planReachToTableObject, side))
taskQueue.addTask(self.animateLastPlan)
if not self.ikPlanner.fixedBaseArm: # TODO: distance is handled by reach, hence ignore
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
taskQueue.addTask(functools.partial(self.planTouchTableObject, side))
taskQueue.addTask(self.animateLastPlan)
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
taskQueue.addTask(functools.partial(self.closeHand, side))
taskQueue.addTask(functools.partial(self.graspTableObject, side))
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
taskQueue.addTask(functools.partial(self.planLiftTableObject, side))
taskQueue.addTask(self.animateLastPlan)
def addTasksToQueueDropIntoBin(self, taskQueue, side):
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
taskQueue.addTask(functools.partial(self.planDropPostureRaise, side))
taskQueue.addTask(self.animateLastPlan)
taskQueue.addTask(functools.partial(self.openHand, side))
taskQueue.addTask(functools.partial(self.dropTableObject, side))
taskQueue.addTask(self.requiredUserPrompt('continue? y/n: '))
if not self.ikPlanner.fixedBaseArm:
taskQueue.addTask(functools.partial(self.planDropPostureLower, side))
else:
taskQueue.addTask(functools.partial(self.planPreGrasp, side))
taskQueue.addTask(self.animateLastPlan)
def addTasksToQueueWalking(self, taskQueue, stanceTransform, message):
taskQueue.addTask(self.printAsync(message))
taskQueue.addTask( functools.partial(self.planWalkToStance, stanceTransform ))
taskQueue.addTask(self.optionalUserPrompt('Send footstep plan. continue? y/n: '))
taskQueue.addTask(self.commitFootstepPlan)
#taskQueue.addTask(self.animateLastPlan) # ought to wait until arrival, currently doesnt wait the right amount of time
taskQueue.addTask(self.requiredUserPrompt('Have you arrived? y/n: '))
def userFitTable(self):
self.tableData = None
self.picker = PointPicker(self.view, numberOfPoints=2, drawLines=True, callback=self.onSegmentTable)
self.picker.start()