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