def showPolyData(polyData, name, color=None, colorByName=None, colorByRange=None, alpha=1.0, visible=True, view=None, parent='segmentation', cls=None): view = view or app.getCurrentRenderView() assert view cls = cls or PolyDataItem item = cls(name, polyData, view) if isinstance(parent, str): parentObj = om.getOrCreateContainer(parent) else: parentObj = parent om.addToObjectModel(item, parentObj) item.setProperty('Visible', visible) item.setProperty('Alpha', alpha) if colorByName and colorByName not in item.getArrayNames(): print 'showPolyData(colorByName=%s): array not found' % colorByName colorByName = None if colorByName: item.setProperty('Color By', colorByName) item.colorBy(colorByName, colorByRange) else: color = [1.0, 1.0, 1.0] if color is None else color item.setProperty('Color', [float(c) for c in color]) item.colorBy(None) return item
def _onPropertyChanged(self, propertySet, propertyName): PolyDataItem._onPropertyChanged(self, propertySet, propertyName) if propertyName == 'Scale': scale = self.getProperty(propertyName) self.rep.SetWorldSize(scale) self._updateAxesGeometry() elif propertyName == 'Edit': view = app.getCurrentRenderView() if view not in self.views: view = self.views[0] self.widget.SetInteractor(view.renderWindow().GetInteractor()) self.widget.SetEnabled(self.getProperty(propertyName)) isEditing = self.getProperty(propertyName) if isEditing: frameupdater.registerFrame(self) elif propertyName == 'Trace': trace = self.getProperty(propertyName) if trace and not self.traceData: self.traceData = FrameTraceVisualizer(self) elif not trace and self.traceData: om.removeFromObjectModel(self.traceData.getTraceData()) self.traceData = None elif propertyName == 'Tube': self._updateAxesGeometry()
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 onNewWalkingGoal(self, walkingGoal=None): walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() if self.placer: self.placer.stop() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine( polyData, pos, pos + [0, 0, 1]) polyData = segmentation.thresholdPoints( polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax( vnp.getNumpyFromVtk(polyData, 'Points')[:, 2]) frameObj.transform.Translate( pos - np.array(frameObj.transform.GetPosition())) d = DebugData() d.addSphere((0, 0, 0), radius=0.03) handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1, 1, 0]) handle.actor.SetUserTransform(frameObj.transform) self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == 'Edit': if propertySet.getProperty(propertyName): self.placer.start() else: self.placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, 'Edit') frameObj.connectFrameModified(self.onWalkingGoalModified) self.onWalkingGoalModified(frameObj)
def newTerrainItem(self, tform, uid=None, region=None): if uid is None: uid = self.getNewUID() elif uid in self.regions: return self.regions[uid] view = app.getCurrentRenderView() item = TerrainRegionItem(uid, view, tform, self, region) parentObj = om.getOrCreateContainer('Safe terrain regions') om.addToObjectModel(item, parentObj) self.regions[uid] = item return item
def pickPoint(displayPoint, view, obj=None, pickType='points', tolerance=0.01, returnNormal=False): assert pickType in ('points', 'cells', 'render') view = view or app.getCurrentRenderView() assert view if isinstance(obj, str): obj = om.findObjectByName(obj) assert obj if pickType == 'render': picker = vtk.vtkPropPicker() else: picker = vtk.vtkPointPicker() if pickType == 'points' else vtk.vtkCellPicker() picker.SetTolerance(tolerance) if obj: if isinstance(obj, list): for o in obj: picker.AddPickList(o.actor) obj = None else: picker.AddPickList(obj.actor) picker.PickFromListOn() picker.Pick(displayPoint[0], displayPoint[1], 0, view.renderer()) pickedProp = picker.GetViewProp() pickedPoint = np.array(picker.GetPickPosition()) pickedDataset = pickedProp.GetMapper().GetInput() if isinstance(pickedProp, vtk.vtkActor) else None pickedNormal = np.zeros(3) if returnNormal: if pickType == 'cells': pickedNormal = np.array(picker.GetPickNormal()) elif pickType == 'points' and pickedDataset: pointId = picker.GetPointId() normals = pickedDataset.GetPointData().GetNormals() if normals: pickedNormal = np.array(normals.GetTuple3(pointId)) if obj: if returnNormal: return (pickedPoint, pickedNormal) if pickedProp else (None, None) else: return pickedPoint if pickedProp else None else: return (pickedPoint, pickedProp, pickedDataset, pickedNormal) if returnNormal else (pickedPoint, pickedProp, pickedDataset)
def showFrame(frame, name, view=None, parent='segmentation', scale=0.35, visible=True): view = view or app.getCurrentRenderView() assert view if isinstance(parent, str): parentObj = om.getOrCreateContainer(parent) else: parentObj = parent item = FrameItem(name, frame, view) om.addToObjectModel(item, parentObj) item.setProperty('Visible', visible) item.setProperty('Scale', scale) return item
def showText(text, name, fontSize=18, position=(10, 10), parent=None, view=None): view = view or app.getCurrentRenderView() assert view item = TextItem(name, text, view=view) item.setProperty('Font Size', fontSize) item.setProperty('Position', list(position)) if isinstance(parent, str): parentObj = om.getOrCreateContainer(parent) else: parentObj = parent om.addToObjectModel(item, parentObj) return item
def onNewWalkingGoal(self, walkingGoal=None): walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel) frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25) frameObj.setProperty('Edit', True) rep = frameObj.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frameObj.widget.HandleRotationEnabledOff() if self.placer: self.placer.stop() terrain = om.findObjectByName('HEIGHT_MAP_SCENE') if terrain: pos = np.array(frameObj.transform.GetPosition()) polyData = filterUtils.removeNonFinitePoints(terrain.polyData) if polyData.GetNumberOfPoints(): polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1]) polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1]) if polyData.GetNumberOfPoints(): pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2]) frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition())) d = DebugData() d.addSphere((0,0,0), radius=0.03) handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0]) handle.actor.SetUserTransform(frameObj.transform) self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain) def onFramePropertyModified(propertySet, propertyName): if propertyName == 'Edit': if propertySet.getProperty(propertyName): self.placer.start() else: self.placer.stop() frameObj.properties.connectPropertyChanged(onFramePropertyModified) onFramePropertyModified(frameObj, 'Edit') frameObj.connectFrameModified(self.onWalkingGoalModified) self.onWalkingGoalModified(frameObj)
def showCaptionWidget(position, text, view=None): view = view or app.getCurrentRenderView() assert view global captionWidget if not captionWidget: rep = vtk.vtkCaptionRepresentation() rep.SetPosition(0.2, 0.8) w = vtk.vtkCaptionWidget() w.SetInteractor(view.renderWindow().GetInteractor()) w.SetRepresentation(rep) w.On() captionWidget = w rep = captionWidget.GetRepresentation() rep.SetAnchorPosition(position) rep.GetCaptionActor2D().SetCaption(text) a = rep.GetCaptionActor2D() pr = a.GetTextActor().GetTextProperty() pr.SetJustificationToCentered() pr.SetVerticalJustificationToCentered() pr.SetItalic(0) pr.SetBold(0) pr.SetShadow(0) pr.SetFontFamilyToArial() c2 = rep.GetPosition2Coordinate() c2.SetCoordinateSystemToDisplay() c2.SetValue(12*len(text),30) # disable border #rep.SetShowBorder(0) a.SetThreeDimensionalLeader(0) a.SetLeaderGlyphSize(0.005) captionWidget.On() captionWidget.Render()
def _onPropertyChanged(self, propertySet, propertyName): om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName) if propertyName == 'Visible': self.actor.SetVisibility(self.getProperty(propertyName)) self._renderAllViews() elif propertyName == 'Text': view = app.getCurrentRenderView() self.actor.SetInput(self.getProperty(propertyName)) elif propertyName == 'Position': pos = self.getProperty(propertyName) self.actor.SetPosition(pos[0], pos[1]) elif propertyName == 'Font Size': self.actor.GetTextProperty().SetFontSize(self.getProperty(propertyName)) elif propertyName == 'Bold Size': self.actor.GetTextProperty().SetBold(self.getProperty(propertyName)) elif propertyName == 'Italic': self.actor.GetTextProperty().SetItalic(self.getProperty(propertyName)) if self.getProperty('Visible'): self._renderAllViews()
def init(self, view=None, globalsDict=None): view = view or applogic.getCurrentRenderView() useRobotState = True usePerception = True useFootsteps = True useHands = True usePlanning = True useAtlasDriver = True useAtlasConvexHull = False useWidgets = False directorConfig = self.getDirectorConfig() neckPitchJoint = 'neck_ay' if useAtlasDriver: atlasDriver = atlasdriver.init(None) if useRobotState: robotStateModel, robotStateJointController = roboturdf.loadRobotModel('robot state model', view, urdfFile=directorConfig['urdfConfig']['robotState'], parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True) robotStateJointController.setPose('EST_ROBOT_STATE', robotStateJointController.getPose('q_nom')) roboturdf.startModelPublisherListener([(robotStateModel, robotStateJointController)]) robotStateJointController.addLCMUpdater('EST_ROBOT_STATE') segmentationroutines.SegmentationContext.initWithRobot(robotStateModel) if usePerception: multisenseDriver, mapServerSource = perception.init(view) def getNeckPitch(): return robotStateJointController.q[robotstate.getDrakePoseJointNames().index( neckPitchJoint )] neckDriver = perception.NeckDriver(view, getNeckPitch) def getSpindleAngleFunction(): if (robotStateJointController.lastRobotStateMessage): if ('hokuyo_joint' in robotStateJointController.lastRobotStateMessage.joint_name): index = robotStateJointController.lastRobotStateMessage.joint_name.index('hokuyo_joint') return (float(robotStateJointController.lastRobotStateMessage.utime)/(1e6), robotStateJointController.lastRobotStateMessage.joint_position[index]) return (0, 0) spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction) robotStateModel.connectModelChanged(spindleMonitor.onRobotStateChanged) if useHands: rHandDriver = handdriver.RobotiqHandDriver(side='right') lHandDriver = handdriver.RobotiqHandDriver(side='left') if useFootsteps: footstepsDriver = footstepsdriver.FootstepsDriver(robotStateJointController) irisDriver = irisdriver.IRISDriver(robotStateJointController, footstepsDriver.params) raycastDriver = raycastdriver.RaycastDriver() if usePlanning: ikRobotModel, ikJointController = roboturdf.loadRobotModel('ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None) om.removeFromObjectModel(ikRobotModel) ikJointController.addPose('q_end', ikJointController.getPose('q_nom')) ikJointController.addPose('q_start', ikJointController.getPose('q_nom')) if 'leftFootLink' in directorConfig: ikServer = ik.AsyncIKCommunicator(directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], directorConfig['leftFootLink'], directorConfig['rightFootLink'], directorConfig['pelvisLink']) else: # assume that robot has no feet e.g. fixed base arm ikServer = ik.AsyncIKCommunicator(directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], '', '', '') def startIkServer(): ikServer.startServerAsync() ikServer.comm.writeCommandsToLogFile = True #startIkServer() playbackRobotModel, playbackJointController = roboturdf.loadRobotModel('playback model', view, urdfFile=directorConfig['urdfConfig']['playback'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False) teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False) if useAtlasConvexHull: chullRobotModel, chullJointController = roboturdf.loadRobotModel('convex hull atlas', view, urdfFile=urdfConfig['chull'], parent='planning', color=roboturdf.getRobotOrangeColor(), visible=False) playbackJointController.models.append(chullRobotModel) planPlayback = planplayback.PlanPlayback() if (roboturdf.numberOfHands == 1): handFactory = roboturdf.HandFactory(robotStateModel) handModels = [handFactory.getLoader(side) for side in ['left']] elif (roboturdf.numberOfHands == 2): handFactory = roboturdf.HandFactory(robotStateModel) handModels = [handFactory.getLoader(side) for side in ['left', 'right']] else: handFactory = None handModels = [] ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels) manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner) affordanceManager = affordancemanager.AffordanceObjectModelManager(view) affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() segmentation.affordanceManager = affordanceManager plannerPub = plannerPublisher.PlannerPublisher(ikPlanner,affordanceManager) ikPlanner.setPublisher(plannerPub) # This joint angle is mapped to the Multisense panel neckPitchJoint = ikPlanner.neckPitchJoint applogic.resetCamera(viewDirection=[-1,0,0], view=view) if useWidgets: playbackPanel = playbackpanel.PlaybackPanel(planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner) teleopPanel = teleoppanel.TeleopPanel(robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner, playbackPanel.setPlan, playbackPanel.hidePlan) footstepsDriver.walkingPlanCallback = playbackPanel.setPlan manipPlanner.connectPlanReceived(playbackPanel.setPlan) robotSystemArgs = dict(locals()) for arg in ['globalsDict', 'self']: del robotSystemArgs[arg] if globalsDict is not None: globalsDict.update(robotSystemArgs) robotSystem = FieldContainer(**robotSystemArgs) viewbehaviors.ViewBehaviors.addRobotBehaviors(robotSystem) return robotSystem
def startButton(): view = app.getCurrentRenderView() init(view) _kinectSource.start()
def onOpenUrdf(filename): model = roboturdf.openUrdf(filename, app.getCurrentRenderView()) if not model: app.showErrorMessage('Failed to read urdf file: %s' % filename, title='Read urdf error')
def onOpenOtdf(filename): model = otdfmodel.openOtdf(filename, app.getCurrentRenderView())
def startButton(): view = app.getCurrentRenderView() init(view) _pointcloudSource.start()
def init(self, view=None, globalsDict=None): view = view or applogic.getCurrentRenderView() useRobotState = True usePerception = True useFootsteps = True useHands = True usePlanning = True useAtlasDriver = True useAtlasConvexHull = False useWidgets = False directorConfig = self.getDirectorConfig() neckPitchJoint = 'neck_ay' if useAtlasDriver: atlasDriver = atlasdriver.init(None) if useRobotState: robotStateModel, robotStateJointController = roboturdf.loadRobotModel( 'robot state model', view, urdfFile=directorConfig['urdfConfig']['robotState'], parent='sensors', color=roboturdf.getRobotGrayColor(), visible=True) robotStateJointController.setPose( 'EST_ROBOT_STATE', robotStateJointController.getPose('q_nom')) roboturdf.startModelPublisherListener([ (robotStateModel, robotStateJointController) ]) robotStateJointController.addLCMUpdater('EST_ROBOT_STATE') segmentationroutines.SegmentationContext.initWithRobot( robotStateModel) if usePerception: multisenseDriver, mapServerSource = perception.init(view) def getNeckPitch(): return robotStateJointController.q[ robotstate.getDrakePoseJointNames().index(neckPitchJoint)] neckDriver = perception.NeckDriver(view, getNeckPitch) def getSpindleAngleFunction(): if (robotStateJointController.lastRobotStateMessage): if ('hokuyo_joint' in robotStateJointController. lastRobotStateMessage.joint_name): index = robotStateJointController.lastRobotStateMessage.joint_name.index( 'hokuyo_joint') return (float(robotStateJointController. lastRobotStateMessage.utime) / (1e6), robotStateJointController. lastRobotStateMessage.joint_position[index]) return (0, 0) spindleMonitor = perception.SpindleMonitor(getSpindleAngleFunction) robotStateModel.connectModelChanged( spindleMonitor.onRobotStateChanged) if useHands: rHandDriver = handdriver.RobotiqHandDriver(side='right') lHandDriver = handdriver.RobotiqHandDriver(side='left') if useFootsteps: footstepsDriver = footstepsdriver.FootstepsDriver( robotStateJointController) irisDriver = irisdriver.IRISDriver(robotStateJointController, footstepsDriver.params) raycastDriver = raycastdriver.RaycastDriver() if usePlanning: ikRobotModel, ikJointController = roboturdf.loadRobotModel( 'ik model', urdfFile=directorConfig['urdfConfig']['ik'], parent=None) om.removeFromObjectModel(ikRobotModel) ikJointController.addPose('q_end', ikJointController.getPose('q_nom')) ikJointController.addPose('q_start', ikJointController.getPose('q_nom')) if 'leftFootLink' in directorConfig: ikServer = ik.AsyncIKCommunicator( directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], directorConfig['leftFootLink'], directorConfig['rightFootLink'], directorConfig['pelvisLink']) else: # assume that robot has no feet e.g. fixed base arm ikServer = ik.AsyncIKCommunicator( directorConfig['urdfConfig']['ik'], directorConfig['fixedPointFile'], '', '', '') def startIkServer(): ikServer.startServerAsync() ikServer.comm.writeCommandsToLogFile = True #startIkServer() playbackRobotModel, playbackJointController = roboturdf.loadRobotModel( 'playback model', view, urdfFile=directorConfig['urdfConfig']['playback'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False) teleopRobotModel, teleopJointController = roboturdf.loadRobotModel( 'teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False) if useAtlasConvexHull: chullRobotModel, chullJointController = roboturdf.loadRobotModel( 'convex hull atlas', view, urdfFile=urdfConfig['chull'], parent='planning', color=roboturdf.getRobotOrangeColor(), visible=False) playbackJointController.models.append(chullRobotModel) planPlayback = planplayback.PlanPlayback() if (roboturdf.numberOfHands == 1): handFactory = roboturdf.HandFactory(robotStateModel) handModels = [handFactory.getLoader(side) for side in ['left']] elif (roboturdf.numberOfHands == 2): handFactory = roboturdf.HandFactory(robotStateModel) handModels = [ handFactory.getLoader(side) for side in ['left', 'right'] ] else: handFactory = None handModels = [] ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels) manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner) affordanceManager = affordancemanager.AffordanceObjectModelManager( view) affordanceitems.MeshAffordanceItem.getMeshManager( ).collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() segmentation.affordanceManager = affordanceManager plannerPub = plannerPublisher.PlannerPublisher( ikPlanner, affordanceManager) ikPlanner.setPublisher(plannerPub) # This joint angle is mapped to the Multisense panel neckPitchJoint = ikPlanner.neckPitchJoint applogic.resetCamera(viewDirection=[-1, 0, 0], view=view) if useWidgets: playbackPanel = playbackpanel.PlaybackPanel( planPlayback, playbackRobotModel, playbackJointController, robotStateModel, robotStateJointController, manipPlanner) teleopPanel = teleoppanel.TeleopPanel( robotStateModel, robotStateJointController, teleopRobotModel, teleopJointController, ikPlanner, manipPlanner, playbackPanel.setPlan, playbackPanel.hidePlan) footstepsDriver.walkingPlanCallback = playbackPanel.setPlan manipPlanner.connectPlanReceived(playbackPanel.setPlan) robotSystemArgs = dict(locals()) for arg in ['globalsDict', 'self']: del robotSystemArgs[arg] if globalsDict is not None: globalsDict.update(robotSystemArgs) robotSystem = FieldContainer(**robotSystemArgs) viewbehaviors.ViewBehaviors.addRobotBehaviors(robotSystem) return robotSystem