def updateIntersection(frame): origin = np.array(frame.transform.GetPosition()) rayLength = 5 for i in range(0,numRays): ray = rays[:,i] rayTransformed = np.array(frame.transform.TransformNormal(ray)) intersection = computeIntersection(locator, origin, origin + rayTransformed*rayLength) name = 'ray intersection ' + str(i) if intersection is not None: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, intersection) color = [1,0,0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color) else: om.removeFromObjectModel(om.findObjectByName(name)) d = DebugData() d.addLine(origin, origin+rayTransformed*rayLength) color = [0,1,0] # d.addSphere(intersection, radius=0.04) vis.updatePolyData(d.getPolyData(), name, color=color)
def __init__(self, robotSystem, view): self.robotStateModel = robotSystem.robotStateModel self.robotStateJointController = robotSystem.robotStateJointController self.robotSystem = robotSystem self.lFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.leftFootLink) self.rFootFtFrameId = self.robotStateModel.model.findLinkID( self.robotSystem.ikPlanner.rightFootLink) self.leftInContact = 0 self.rightInContact = 0 self.view = view self.ddDrakeWrapper = PythonQt.dd.ddDrakeWrapper() self.warningButton = QtGui.QLabel('COP Warning') self.warningButton.setStyleSheet("background-color:white") self.dialogVisible = False d = DebugData() vis.updatePolyData(d.getPolyData(), 'measured cop', view=self.view, parent='robot state model') om.findObjectByName('measured cop').setProperty('Visible', False) self.robotStateModel.connectModelChanged(self.update)
def projectDrillDemoInCamera(): q = om.findObjectByName('camera overlay') om.removeFromObjectModel(q) imageView = cameraview.views['CAMERA_LEFT'] imageView.imageActor.SetOpacity(.2) drawFrameInCamera(drillDemo.drill.frame.transform, 'drill frame',visible=False) tf = transformUtils.copyFrame( drillDemo.drill.frame.transform ) tf.PreMultiply() tf.Concatenate( drillDemo.drill.drillToButtonTransform ) drawFrameInCamera(tf, 'drill button') tf2 = transformUtils.copyFrame( tf ) tf2.PreMultiply() tf2.Concatenate( transformUtils.frameFromPositionAndRPY( [0,0,0] , [180,0,0] ) ) drawFrameInCamera(tf2, 'drill button flip') drawObjectInCamera('drill',visible=False) drawObjectInCamera('sensed pointer tip') obj = om.findObjectByName('sensed pointer tip frame') if (obj is not None): drawFrameInCamera(obj.transform, 'sensed pointer tip frame',visible=False) #drawObjectInCamera('left robotiq',visible=False) #drawObjectInCamera('right pointer',visible=False) v = imageView.view v.render()
def moveDrill(self,Pos=[0,0,0],RPY=[0,0,0],Style='Local'): linkMap = { 'left' : 'l_hand_face', 'right': 'r_hand_face'} linkName = linkMap[self.graspingHand] affordance = om.findObjectByName('drill') affordanceReach = om.findObjectByName('reach frame') frame = om.findObjectByName('drill frame') drillTransform = affordance.actor.GetUserTransform() reach = transformUtils.copyFrame(affordanceReach.actor.GetUserTransform()) drillTransformCopy = transformUtils.copyFrame(affordance.actor.GetUserTransform()) drillToReach = vtkTransform() drillToReach.Identity() drillToReach.PostMultiply() drillToReach.Concatenate(drillTransformCopy) drillToReach.Concatenate(reach.GetLinearInverse()) handfaceToWorld = self.ikPlanner.getLinkFrameAtPose(linkName, self.getPlanningStartPose()) # find a transform that move forward wrt hand palm delta = transformUtils.frameFromPositionAndRPY(Pos, RPY) drillTransform.Identity() drillTransform.PostMultiply() drillTransform.Concatenate(drillToReach) drillTransform.Concatenate(delta) drillTransform.Concatenate(handfaceToWorld)
def toggleFootstepWidget(displayPoint, view, useHorizontalWidget=False): obj, _ = vis.findPickedObject(displayPoint, view) if not obj: return False name = obj.getProperty('Name') if name in ('footstep widget', 'footstep widget frame'): om.removeFromObjectModel(om.findObjectByName('footstep widget')) return True match = re.match('^step (\d+)$', name) if not match: return False stepIndex = int(match.group(1)) existingWidget = om.findObjectByName('footstep widget') if existingWidget: previousStep = existingWidget.stepIndex print 'have existing widget for step:', stepIndex om.removeFromObjectModel(existingWidget) if previousStep == stepIndex: print 'returning because widget was for selected step' return True footMesh = shallowCopy(obj.polyData) footFrame = transformUtils.copyFrame(obj.getChildFrame().transform) if useHorizontalWidget: rpy = [0.0, 0.0, transformUtils.rollPitchYawFromTransform(footFrame)[2]] footFrame = transformUtils.frameFromPositionAndRPY(footFrame.GetPosition(), np.degrees(rpy)) footObj = vis.showPolyData(footMesh, 'footstep widget', parent='planning', alpha=0.2) footObj.stepIndex = stepIndex frameObj = vis.showFrame(footFrame, 'footstep widget frame', parent=footObj, scale=0.2) footObj.actor.SetUserTransform(frameObj.transform) footObj.setProperty('Color', obj.getProperty('Color')) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() walkGoal = om.findObjectByName('walking goal') if walkGoal: walkGoal.setProperty('Edit', False) def onFootWidgetChanged(frame): footstepsDriver.onStepModified(stepIndex - 1, frame) frameObj.connectFrameModified(onFootWidgetChanged) return True
def onExecClicked(self): self.commitFootstepPlan(self.lastFootstepPlan) om.removeFromObjectModel(om.findObjectByName('footstep widget')) walkGoal = om.findObjectByName('walking goal') if walkGoal: walkGoal.setProperty('Edit', False) self.execButton.setEnabled(False)
def onClearClicked(self): om.removeFromObjectModel(om.findObjectByName('walking goal')) om.removeFromObjectModel(om.findObjectByName('footstep widget')) om.removeFromObjectModel(om.findObjectByName('LCM GL')) self.clearFootstepPlan() if self.toolbarWidget: self.execButton.setEnabled(False)
def colorizeMaps(enabled): if enabled: om.findObjectByName('Map Server').source.colorizeCallback = colorizeMapCallback for name in _colorizeMapNames: colorizeMapCallback(om.findObjectByName(name)) else: om.findObjectByName('Map Server').source.colorizeCallback = None
def onExecClicked(self): self.commitFootstepPlan(self.lastFootstepPlan) om.removeFromObjectModel(om.findObjectByName('footstep widget')) walkGoal = om.findObjectByName('walking goal') if walkGoal: walkGoal.setProperty('Edit', False) self.execButton.setEnabled(False)
def onClearClicked(self): om.removeFromObjectModel(om.findObjectByName('walking goal')) om.removeFromObjectModel(om.findObjectByName('footstep widget')) om.removeFromObjectModel(om.findObjectByName('LCM GL')) self.clearFootstepPlan() if self.toolbarWidget: self.execButton.setEnabled(False)
def ungraspAffordance(self, affordanceName): try: del self.frameSyncs[affordanceName] except KeyError: pass if not self.frameSyncs: om.removeFromObjectModel(om.findObjectByName('l_hand frame')) om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
def ungraspAffordance(self, affordanceName): try: del self.frameSyncs[affordanceName] except KeyError: pass if not self.frameSyncs: om.removeFromObjectModel(om.findObjectByName('l_hand frame')) om.removeFromObjectModel(om.findObjectByName('r_hand frame'))
def apply3DFit(self): if om.findObjectByName('drill') is None: self.log('No 3D fit of drill. Click Spawn Drill button to provide a fit.') msg = lcmdrc.pfgrasp_command_t() msg.command = lcmdrc.pfgrasp_command_t.RUN_ONE_ITER_W_3D_PRIOR affordanceReach = om.findObjectByName('grasp frame') affordanceReach.actor.GetUserTransform().GetPosition(msg.pos) lcmUtils.publish('PFGRASP_CMD', msg)
def colorizeMaps(enabled): if enabled: om.findObjectByName( 'Map Server').source.colorizeCallback = colorizeMapCallback for name in _colorizeMapNames: colorizeMapCallback(om.findObjectByName(name)) else: om.findObjectByName('Map Server').source.colorizeCallback = None
def spawnDrillAffordance(self): if om.findObjectByName('drill') is None: self.drillDemo.spawnDrillAffordance() if self.graspingHand == 'left': self.moveDrill() else: self.moveDrill(RPY=[0,180,0]) om.findObjectByName('drill frame').connectFrameModified(self.onModifiedDrillFrame)
def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = [ 'defaultRandom', 'learnedRandom', 'learned', 'default' ] self.setDefaultOptions() self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'], numRays=self.options['Sensor']['numRays']) self.Controller = ControllerObj(self.Sensor) self.Car = CarPlant(controller=self.Controller, velocity=self.options['Car']['velocity']) self.Reward = Reward( self.Sensor, collisionThreshold=self.collisionThreshold, actionCost=self.options['Reward']['actionCost'], collisionPenalty=self.options['Reward']['collisionPenalty'], raycastCost=self.options['Reward']['raycastCost']) self.setSARSA() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildCircleWorld( percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World'] ['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.supervisedTrainingTime = self.options['runTime'][ 'supervisedTrainingTime'] self.learningRandomTime = self.options['runTime']['learningRandomTime'] self.learningEvalTime = self.options['runTime']['learningEvalTime'] self.defaultControllerTime = self.options['runTime'][ 'defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization"
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 initialize(self): self.dt = self.options["dt"] self.controllerTypeOrder = ["defaultRandom", "learnedRandom", "learned", "default"] self.setDefaultOptions() self.Sensor = SensorObj( rayLength=self.options["Sensor"]["rayLength"], numRays=self.options["Sensor"]["numRays"] ) self.Controller = ControllerObj(self.Sensor) self.Car = CarPlant(controller=self.Controller, velocity=self.options["Car"]["velocity"]) self.Reward = Reward( self.Sensor, collisionThreshold=self.collisionThreshold, actionCost=self.options["Reward"]["actionCost"], collisionPenalty=self.options["Reward"]["collisionPenalty"], raycastCost=self.options["Reward"]["raycastCost"], ) self.setSARSA() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName("world")) self.world = World.buildCircleWorld( percentObsDensity=self.options["World"]["percentObsDensity"], circleRadius=self.options["World"]["circleRadius"], nonRandom=self.options["World"]["nonRandomWorld"], scale=self.options["World"]["scale"], randomSeed=self.options["World"]["randomSeed"], obstaclesInnerFraction=self.options["World"]["obstaclesInnerFraction"], ) om.removeFromObjectModel(om.findObjectByName("robot")) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty("Scale", 3) self.frame.setProperty("Edit", True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.supervisedTrainingTime = self.options["runTime"]["supervisedTrainingTime"] self.learningRandomTime = self.options["runTime"]["learningRandomTime"] self.learningEvalTime = self.options["runTime"]["learningEvalTime"] self.defaultControllerTime = self.options["runTime"]["defaultControllerTime"] self.Car.setFrame(self.frame) print "Finished initialization"
def onMergeIntoPointCloud(): allPointClouds = om.findObjectByName('point clouds') if allPointClouds: allPointClouds = [ i.getProperty('Name') for i in allPointClouds.children() ] sel = QtGui.QInputDialog.getItem(None, "Point Cloud Merging", "Pick point cloud to merge into:", allPointClouds, current=0, editable=False) sel = om.findObjectByName(sel) # Make a copy of each in same frame polyDataInto = vtk.vtkPolyData() polyDataInto.ShallowCopy(sel.polyData) if sel.getChildFrame(): polyDataInto = segmentation.transformPolyData( polyDataInto, sel.getChildFrame().transform) polyDataFrom = vtk.vtkPolyData() polyDataFrom.DeepCopy(pointCloudObj.polyData) if pointCloudObj.getChildFrame(): polyDataFrom = segmentation.transformPolyData( polyDataFrom, pointCloudObj.getChildFrame().transform) # Actual merge append = filterUtils.appendPolyData([polyDataFrom, polyDataInto]) if sel.getChildFrame(): polyDataInto = segmentation.transformPolyData( polyDataInto, sel.getChildFrame().transform.GetInverse()) # resample append = segmentationroutines.applyVoxelGrid(append, 0.01) append = segmentation.addCoordArraysToPolyData(append) # Recenter the frame sel.setPolyData(append) t = vtk.vtkTransform() t.PostMultiply() t.Translate(filterUtils.computeCentroid(append)) segmentation.makeMovable(sel, t) # Hide the old one if pointCloudObj.getProperty('Name') in allPointClouds: pointCloudObj.setProperty('Visible', False)
def planReach(self): ikPlanner = self.robotSystem.ikPlanner startPose = self.getPlanningStartPose() startPoseName = 'q_reach_start' endPoseName = 'q_reach_end' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) side = 'right' movingReachConstraint = ikPlanner.createMovingReachConstraints(startPoseName, lockBase=True, lockBack=True, lockArm=True, side=side) palmToHand = ikPlanner.getPalmToHandLink(side=side) targetFrame = om.findObjectByName('reach frame').transform poseConstraints = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame, graspToHandLinkFrame=palmToHand, angleToleranceInDegrees=5.0) constraints = [] constraints.extend(movingReachConstraint) constraints.extend(poseConstraints) constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30) seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right') seedPoseName = 'q_above_switch' self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName) constraintSet.seedPoseName = seedPoseName constraintSet.nominalPoseName = seedPoseName endPose, info = constraintSet.runIk() plan = constraintSet.planEndPoseGoal() self.addPlan(plan)
def __init__(self, view): self.view = view loader = QtUiTools.QUiLoader() uifile = QtCore.QFile(':/ui/ddFrameVisualization.ui') assert uifile.open(uifile.ReadOnly) self.widget = loader.load(uifile) self.ui = WidgetDict(self.widget.children()) self.botFrameUpdater = BotFrameUpdater(self.ui.botFramesListWidget) robotModel = om.findObjectByName('robot state model') self.linkFrameUpdater = LinkFrameUpdater(robotModel, self.ui.linkFramesListWidget) self.eventFilter = PythonQt.dd.ddPythonEventFilter() self.ui.scrollArea.installEventFilter(self.eventFilter) self.eventFilter.addFilteredEventType(QtCore.QEvent.Resize) self.eventFilter.connect('handleEvent(QObject*, QEvent*)', self.onEvent) PythonQt.dd.ddGroupBoxHider(self.ui.botFramesGroup) PythonQt.dd.ddGroupBoxHider(self.ui.linkFramesGroup) self.updateTimer = TimerCallback(targetFps=60) self.updateTimer.callback = self.updateFrames self.updateTimer.start()
def computeRewardFromFrameLocation(self, u=0.0): carFrame = om.findObjectByName('robot frame') raycastDistance = self.sensorObj.raycastAll(carFrame) carState = 0.0 # just a placeholder for now S = (carState, raycastDistance) return self.computeReward(S, u)
def planGraspLineMotion(self): self.turnPointwiseOffSlow() startPose = self.getPlanningStartPose() graspFrame = vtk.vtkTransform() graspFrame.Identity() graspFrame.PostMultiply() if self.graspingHand == 'right': graspFrame.Concatenate(transformUtils.frameFromPositionAndRPY([0,0,0], [0,180,0])) graspFrame.Concatenate(transformUtils.copyFrame(om.findObjectByName('grasp frame').actor.GetUserTransform())) constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, graspFrame, \ lockBase=False, lockBack=True) # constraint orientation p,q = self.ikPlanner.createPositionOrientationGraspConstraints(self.graspingHand, graspFrame) q.tspan=[0.5,1] constraintSet.constraints.append(q) # constraint line axis positionConstraint, orientationConstraint, axisConstraint = self.ikPlanner.createMoveOnLineConstraints(startPose, graspFrame) ## broken robot arm has a new joint limit if self.graspingHand == 'left': constraintSet.constraints.append(self.createBrokenArmConstraint()) constraintSet.constraints.append(axisConstraint) constraintSet.constraints[-1].tspan = [0.5,np.inf] endPose, info = constraintSet.runIk() #print endPose if info > 10: self.log("in Target received: Bad movement") return graspPlan = constraintSet.runIkTraj()
def planPinchReach(self, maxDegreesPerSecond=None): if maxDegreesPerSecond is None: maxDegreesPerSecond = 10 ikPlanner = self.ikPlanner targetFrame = om.findObjectByName('pinch reach frame').transform pinchToHand = self.getPinchToHandFrame() startPose = self.getPlanningStartPose() constraintSet = self.computeGraspPose(startPose, targetFrame, graspToHand=pinchToHand) constraintSet.ikParameters = IkParameters( maxDegreesPerSecond=maxDegreesPerSecond) seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right') seedPoseName = 'q_above_switch' self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName) constraintSet.seedPoseName = seedPoseName constraintSet.nominalPoseName = seedPoseName endPose, info = constraintSet.runIk() plan = constraintSet.planEndPoseGoal() self.addPlan(plan)
def getBDIAdjustedFootstepsFolder(): obj = om.findObjectByName('BDI adj footstep plan') if obj is None: obj = om.getOrCreateContainer('BDI adj footstep plan') obj.setIcon(om.Icons.Feet) om.collapse(obj) return obj
def getTerrainSlicesFolder(): obj = om.findObjectByName('terrain slices') if obj is None: obj = om.getOrCreateContainer('terrain slices', parentObj=getFootstepsFolder()) obj.setProperty('Visible', False) om.collapse(obj) return obj
def updateReachFrame(self): graspFrame = transformUtils.copyFrame(self.pinchToBox) boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform graspFrame.PostMultiply() graspFrame.Concatenate(boxFrame) vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
def getDebugFolder(): obj = om.findObjectByName('debug') if obj is None: obj = om.getOrCreateContainer('debug', om.getOrCreateContainer('segmentation')) om.collapse(obj) return obj
def planReach(self): ikPlanner = self.robotSystem.ikPlanner startPose = self.getPlanningStartPose() startPoseName = 'q_reach_start' endPoseName = 'q_reach_end' self.robotSystem.ikPlanner.addPose(startPose, startPoseName) side = 'right' movingReachConstraint = ikPlanner.createMovingReachConstraints(startPoseName, lockBase=True, lockBack=True, lockArm=True, side=side) palmToHand = ikPlanner.getPalmToHandLink(side=side) targetFrame = om.findObjectByName('reach frame').transform poseConstraints = ikPlanner.createPositionOrientationGraspConstraints(side, targetFrame, graspToHandLinkFrame=palmToHand, angleToleranceInDegrees=5.0) constraints = [] constraints.extend(movingReachConstraint) constraints.extend(poseConstraints) constraintSet = ikplanner.ConstraintSet(ikPlanner, constraints, endPoseName, startPoseName) constraintSet.ikParameters = IkParameters(maxDegreesPerSecond=30) seedPose = ikPlanner.getMergedPostureFromDatabase(startPose, 'surprise:switch', 'above_switch', side='right') seedPoseName = 'q_above_switch' self.robotSystem.ikPlanner.addPose(seedPose, seedPoseName) constraintSet.seedPoseName = seedPoseName constraintSet.nominalPoseName = seedPoseName endPose, info = constraintSet.runIk() plan = constraintSet.planEndPoseGoal() self.addPlan(plan)
def updateReachFrame(self): graspFrame = transformUtils.copyFrame(self.pinchToBox) boxFrame = om.findObjectByName('Switch Box').getChildFrame().transform graspFrame.PostMultiply() graspFrame.Concatenate(boxFrame) vis.updateFrame(graspFrame, 'pinch reach frame', scale=0.2)
def getBDIAdjustedFootstepsFolder(): obj = om.findObjectByName('BDI adj footstep plan') if obj is None: obj = om.getOrCreateContainer('BDI adj footstep plan') obj.setIcon(om.Icons.Feet) om.collapse(obj) return obj
def colorizeMapsOff(): obj = om.findObjectByName('Map Server') obj.source.colorizeCallback = None alpha = 0.7 pointSize = 1.0 obj.setProperty('Alpha', alpha) obj.setProperty('Point Size', pointSize)
def run(self): polyData = self.getPointCloud() annotation = self.getAnnotationInput() annotationPoint = annotation.annotationPoints[0] planePoints, normal = segmentation.applyLocalPlaneFit( polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2) viewDirection = segmentation.SegmentationContext.getGlobalInstance( ).getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal xaxis = normal zaxis = [0, 0, 1] yaxis = np.cross(zaxis, xaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis /= np.linalg.norm(yaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() t.Translate(annotationPoint) polyData = annotation.polyData polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse()) annotation.setProperty('Visible', False) om.removeFromObjectModel(om.findObjectByName('wall')) obj = vis.showPolyData(polyData, 'wall') obj.actor.SetUserTransform(t) vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def _addPlanItem(plan, name, itemClass): assert plan is not None item = itemClass(name) item.plan = plan om.removeFromObjectModel(om.findObjectByName(name)) om.addToObjectModel(item, om.getOrCreateContainer('segmentation')) return item
def getGroundFrame(self): frame = om.findObjectByName( self.properties.getProperty('Ground frame name')) if not frame: self.fail('could not find ground frame') return frame
def _addPlanItem(plan, name, itemClass): assert plan is not None item = itemClass(name) item.plan = plan om.removeFromObjectModel(om.findObjectByName(name)) om.addToObjectModel(item, om.getOrCreateContainer('segmentation')) return item
def colorizeMapsOff(): obj = om.findObjectByName('Map Server') obj.source.colorizeCallback = None alpha = 0.7 pointSize = 1.0 obj.setProperty('Alpha', alpha) obj.setProperty('Point Size', pointSize)
def run(self): polyData = self.getPointCloud() om.removeFromObjectModel(om.findObjectByName('pointcloud snapshot')) vis.showPolyData(polyData, 'pointcloud snapshot', parent='segmentation', visible=False)
def showFusedMaps(self): om.removeFromObjectModel(om.findObjectByName('stereo')) om.getOrCreateContainer('stereo') q = imageManager.queue cameraToLocalNow = vtk.vtkTransform() utime = q.getCurrentImageTime('CAMERA_TSDF') q.getTransform('CAMERA_LEFT','local', utime,cameraToLocalNow) cameraToLocalFusedNow = vtk.vtkTransform() q.getTransform('CAMERA_LEFT_ALT','local', utime,cameraToLocalFusedNow) for i in range(len(self.pointClouds)): fusedNowToLocalNow = vtk.vtkTransform() fusedNowToLocalNow.PreMultiply() fusedNowToLocalNow.Concatenate( cameraToLocalNow) fusedNowToLocalNow.Concatenate( cameraToLocalFusedNow.GetLinearInverse() ) fusedTransform = vtk.vtkTransform() fusedTransform.PreMultiply() fusedTransform.Concatenate( fusedNowToLocalNow) fusedTransform.Concatenate( self.cameraToLocalFusedTransforms[i] ) pd = filterUtils.transformPolyData(self.pointClouds[i], fusedTransform) vis.showFrame(fusedTransform, ('cloud frame ' + str(i)), visible=True, scale=0.2, parent='stereo') vis.showPolyData(pd, ('stereo ' + str(i)), parent='stereo', colorByName='rgb_colors')
def getFootstepRelativeTransform(self): self.footstepsToPlan = [] for n in xrange(1,self.numFootsteps + 1): stepFrameName = 'step ' + str(n) + ' frame' fToWorld = transformUtils.copyFrame(om.findObjectByName(stepFrameName).transform) fToPlan = self.getTransformToPlanningFrame(fToWorld) self.footstepsToPlan.append(fToPlan)
def getFootstepsFolder(): obj = om.findObjectByName('footstep plan') if obj is None: obj = om.getOrCreateContainer('footstep plan', parentObj=om.getOrCreateContainer('planning')) obj.setIcon(om.Icons.Feet) om.collapse(obj) return obj
def getTerrainSlicesFolder(): obj = om.findObjectByName('terrain slices') if obj is None: obj = om.getOrCreateContainer('terrain slices', parentObj=getFootstepsFolder()) obj.setProperty('Visible', False) om.collapse(obj) return obj
def showHandCloud(hand='left', view=None): view = view or app.getCurrentRenderView() if view is None: return assert hand in ('left', 'right') maps = om.findObjectByName('Map Server') assert maps is not None viewId = 52 if hand == 'left' else 53 reader = maps.source.reader def getCurrentViewId(): return reader.GetCurrentMapId(viewId) p = vtk.vtkPolyData() obj = showPolyData(p, '%s hand cloud' % hand, view=view, parent='sensors') obj.currentViewId = -1 def updateCloud(): currentViewId = getCurrentViewId() #print 'updateCloud: current view id:', currentViewId if currentViewId != obj.currentViewId: reader.GetDataForMapId(viewId, currentViewId, p) #print 'updated poly data. %d points.' % p.GetNumberOfPoints() obj._renderAllViews() t = TimerCallback() t.targetFps = 1 t.callback = updateCloud t.start() obj.updater = t return obj
def testFindWalls(self): raycastDistance = self.sensor.raycastAllFromCurrentFrameLocation() wallsFound = self.findWalls(raycastDistance, addNoise=True) carTransform = om.findObjectByName('robot frame').transform d = DebugData() for wallData in wallsFound: intercept = wallData['ransacFit'][0] slope = wallData['ransacFit'][1] wallDirectionInCarFrame = np.array([1.0, slope, 0.0]) wallPointInCarFrame = np.array([0.0, intercept, 0.0]) # now need to transform these to world frame in order to plot them. wallPointWorldFrame = np.array( carTransform.TransformPoint(wallPointInCarFrame)) wallDirectionWorldFrame = np.array( carTransform.TransformVector(wallDirectionInCarFrame)) wallDirectionWorldFrame = 1 / np.linalg.norm( wallDirectionWorldFrame) * wallDirectionWorldFrame lineLength = 15.0 lineOrigin = wallPointWorldFrame - lineLength * wallDirectionWorldFrame lineEnd = wallPointWorldFrame + lineLength * wallDirectionWorldFrame d.addLine(lineOrigin, lineEnd, radius=0.3, color=[0, 0, 1]) vis.updatePolyData(d.getPolyData(), 'line estimate', colorByName='RGB255') return wallsFound
def getFootstepsFolder(): obj = om.findObjectByName('footstep plan') if obj is None: obj = om.getOrCreateContainer('footstep plan', parentObj=om.getOrCreateContainer('planning')) obj.setIcon(om.Icons.Feet) om.collapse(obj) return obj
def run(self): polyData = self.getPointCloud() annotation = self.getAnnotationInput() annotationPoint = annotation.annotationPoints[0] planePoints, normal = segmentation.applyLocalPlaneFit(polyData, annotationPoint, searchRadius=0.1, searchRadiusEnd=0.2) viewDirection = segmentation.SegmentationContext.getGlobalInstance().getViewDirection() if np.dot(normal, viewDirection) < 0: normal = -normal xaxis = normal zaxis = [0, 0, 1] yaxis = np.cross(zaxis, xaxis) xaxis = np.cross(yaxis, zaxis) xaxis /= np.linalg.norm(xaxis) yaxis /= np.linalg.norm(yaxis) t = transformUtils.getTransformFromAxes(xaxis, yaxis, zaxis) t.PostMultiply() t.Translate(annotationPoint) polyData = annotation.polyData polyData = segmentation.transformPolyData(polyData, t.GetLinearInverse()) annotation.setProperty('Visible', False) om.removeFromObjectModel(om.findObjectByName('wall')) obj = vis.showPolyData(polyData, 'wall') obj.actor.SetUserTransform(t) vis.showFrame(t, 'wall frame', scale=0.2, parent=obj)
def initialize(self): self.dt = self.options['dt'] self.controllerTypeOrder = ['defaultRandom', 'learnedRandom', 'learned', 'default'] self.setDefaultOptions() self.Sensor = SensorObj(rayLength=self.options['Sensor']['rayLength'], numRays=self.options['Sensor']['numRays']) self.Controller = ControllerObj(self.Sensor) self.Car = CarPlant( controller=self.Controller, velocity=self.options['Car']['velocity']) self.Reward = Reward(self.Sensor, collisionThreshold=self.collisionThreshold, actionCost=self.options['Reward']['actionCost'], collisionPenalty=self.options['Reward']['collisionPenalty'], raycastCost=self.options['Reward']['raycastCost']) self.setSARSA() # create the things needed for simulation om.removeFromObjectModel(om.findObjectByName('world')) self.world = World.buildCircleWorld(percentObsDensity=self.options['World']['percentObsDensity'], circleRadius=self.options['World']['circleRadius'], nonRandom=self.options['World']['nonRandomWorld'], scale=self.options['World']['scale'], randomSeed=self.options['World']['randomSeed'], obstaclesInnerFraction=self.options['World']['obstaclesInnerFraction']) om.removeFromObjectModel(om.findObjectByName('robot')) self.robot, self.frame = World.buildRobot() self.locator = World.buildCellLocator(self.world.visObj.polyData) self.Sensor.setLocator(self.locator) self.frame = self.robot.getChildFrame() self.frame.setProperty('Scale', 3) self.frame.setProperty('Edit', True) self.frame.widget.HandleRotationEnabledOff() rep = self.frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) self.supervisedTrainingTime = self.options['runTime']['supervisedTrainingTime'] self.learningRandomTime = self.options['runTime']['learningRandomTime'] self.learningEvalTime = self.options['runTime']['learningEvalTime'] self.defaultControllerTime = self.options['runTime']['defaultControllerTime'] self.Car.setFrame(self.frame) print "Finished initialization"
def update(self, robotModel): name = 'camera frustum %s' % self.robotModel.getProperty('Name') obj = om.findObjectByName(name) if obj and not obj.getProperty('Visible'): return vis.updatePolyData(self.getCameraFrustumGeometry(self.rayLength), name, parent=self.robotModel, visible=False)
def colorizeMultisense(enabled): obj = om.findObjectByName('Multisense') if not obj: return setVisProperties(obj, enabled) colorBy = 'Camera RGB' if enabled else 'Solid Color' obj.setProperty('Color By', colorBy)
def __init__(self): om.init() self.view = PythonQt.dd.ddQVTKWidgetView() # init grid self.gridObj = vis.showGrid(self.view, parent='scene') self.gridObj.setProperty('Surface Mode', 'Surface with edges') self.gridObj.setProperty('Color', [0,0,0]) self.gridObj.setProperty('Alpha', 0.1) # init view options self.viewOptions = vis.ViewOptionsItem(self.view) om.addToObjectModel(self.viewOptions, parentObj=om.findObjectByName('scene')) self.viewOptions.setProperty('Background color', [0.3, 0.3, 0.35]) self.viewOptions.setProperty('Background color 2', [0.95,0.95,1]) # setup camera applogic.setCameraTerrainModeEnabled(self.view, True) applogic.resetCamera(viewDirection=[-1, 0, -0.3], view=self.view) # add view behaviors viewBehaviors = viewbehaviors.ViewBehaviors(self.view) applogic._defaultRenderView = self.view self.mainWindow = QtGui.QMainWindow() self.mainWindow.setCentralWidget(self.view) self.mainWindow.resize(768 * (16/9.0), 768) self.mainWindow.setWindowTitle('Drake Visualizer') self.mainWindow.setWindowIcon(QtGui.QIcon(':/images/drake_logo.png')) self.mainWindow.show() self.drakeVisualizer = DrakeVisualizer(self.view) self.lcmglManager = lcmgl.LCMGLManager(self.view) if lcmgl.LCMGL_AVAILABLE else None self.screenGrabberPanel = ScreenGrabberPanel(self.view) self.screenGrabberDock = self.addWidgetToDock(self.screenGrabberPanel.widget, QtCore.Qt.RightDockWidgetArea) self.screenGrabberDock.setVisible(False) self.cameraBookmarksPanel = camerabookmarks.CameraBookmarkWidget(self.view) self.cameraBookmarksDock = self.addWidgetToDock(self.cameraBookmarksPanel.widget, QtCore.Qt.RightDockWidgetArea) self.cameraBookmarksDock.setVisible(False) model = om.getDefaultObjectModel() model.getTreeWidget().setWindowTitle('Scene Browser') model.getPropertiesPanel().setWindowTitle('Properties Panel') model.setActiveObject(self.viewOptions) self.sceneBrowserDock = self.addWidgetToDock(model.getTreeWidget(), QtCore.Qt.LeftDockWidgetArea) self.propertiesDock = self.addWidgetToDock(self.wrapScrollArea(model.getPropertiesPanel()), QtCore.Qt.LeftDockWidgetArea) self.sceneBrowserDock.setVisible(False) self.propertiesDock.setVisible(False) applogic.addShortcut(self.mainWindow, 'Ctrl+Q', self.applicationInstance().quit) applogic.addShortcut(self.mainWindow, 'F1', self._toggleObjectModel) applogic.addShortcut(self.mainWindow, 'F2', self._toggleScreenGrabber) applogic.addShortcut(self.mainWindow, 'F3', self._toggleCameraBookmarks) applogic.addShortcut(self.mainWindow, 'F8', applogic.showPythonConsole)