def disableFrustum(): cameraObj.setProperty('Visible', False) depthScanner.pointCloudObj.actor.SetUserTransform(None) depthScanner._updateFunc = None applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False) applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])
def initGrid(self, fields): gridObj = vis.showGrid(fields.view, parent='scene') gridObj.setProperty('Surface Mode', 'Surface with edges') gridObj.setProperty('Color', [0,0,0]) gridObj.setProperty('Alpha', 0.1) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view) return FieldContainer(gridObj=gridObj)
def initGrid(self, fields): gridObj = vis.showGrid(fields.view, parent='scene') gridObj.setProperty('Surface Mode', 'Surface with edges') gridObj.setProperty('Color', [0, 0, 0]) gridObj.setProperty('Alpha', 0.1) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view) return FieldContainer(gridObj=gridObj)
def initGrid(self, fields): gridObj = vis.showGrid(fields.view, parent="scene") gridObj.setProperty("Surface Mode", "Surface with edges") gridObj.setProperty("Color", [0, 0, 0]) gridObj.setProperty("Alpha", 0.1) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=fields.view) return FieldContainer(gridObj=gridObj)
def initViewBehaviors(self, robotSystem): from director import applogic from director import robotviewbehaviors viewBehaviors = robotviewbehaviors.RobotViewBehaviors(robotSystem.view, robotSystem) applogic.resetCamera(viewDirection=[-1,0,0], view=robotSystem.view) return FieldContainer(viewBehaviors=viewBehaviors)
def initViewBehaviors(self, robotSystem): from director import applogic from director import robotviewbehaviors viewBehaviors = robotviewbehaviors.RobotViewBehaviors(robotSystem.view, robotSystem) applogic.resetCamera(viewDirection=[-1,0,0], view=robotSystem.view) return FieldContainer(viewBehaviors=viewBehaviors)
def enableFrustum(): updateCameraMesh() cameraObj.setProperty('Visible', True) onCameraModified() depthScanner._updateFunc = onCameraModified applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True) applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])
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)
def disableFrustum(): cameraObj.setProperty('Visible', False) depthScanner.pointCloudObj.actor.SetUserTransform(None) depthScanner._updateFunc = None applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, False) applogic.resetCamera(viewDirection=[0, 0, -1], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 1, 0])
def enableFrustum(): updateCameraMesh() cameraObj.setProperty('Visible', True) onCameraModified() depthScanner._updateFunc = onCameraModified applogic.setCameraTerrainModeEnabled(depthScanner.pointCloudView, True) applogic.resetCamera(viewDirection=[1, 1, -0.4], view=depthScanner.pointCloudView) depthScanner.pointCloudView.camera().SetViewUp([0, 0, 1])
def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.frame.connectFrameModified(self.updateDrawPolyApprox) self.updateDrawIntersection(self.frame) self.updateDrawPolyApprox(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start()
def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0 / self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2, 0, -1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter / elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start()
def __init__(self, robotSystem, cameraView): self.meshPoints = None self.imagePoints = None self.cameraView = cameraView self.robotMesh = vtk.vtkPolyData() robotSystem.robotStateModel.model.getModelMesh(self.robotMesh) self.robotBaseFrame = robotSystem.robotStateModel.getLinkFrame('base') self.view = PythonQt.dd.ddQVTKWidgetView() vis.showPolyData(self.robotMesh, 'robot mesh', view=self.view) self.imageFitter = ImageFitter(self) vis.showPolyData(self.imageFitter.getPointCloud(), 'pointcloud', view=self.view, colorByName='rgb_colors', visible=False) self.picker = pointpicker.PointPicker(self.view) self.picker.pickType = 'cells' self.picker.numberOfPoints = 3 self.picker.annotationName = 'mesh annotation' self.picker.annotationFunc = self.onPickPoints self.picker.start() self.widget = QtGui.QWidget() layout = QtGui.QHBoxLayout(self.widget) layout.addWidget(self.cameraView.view) layout.addWidget(self.view) self.widget.resize(800, 400) self.widget.setWindowTitle('Camera Alignment Tool') self.widget.show() self.viewBehaviors = viewbehaviors.ViewBehaviors(self.view) applogic.resetCamera(viewDirection=[0, 1, 0], view=self.view) applogic.setCameraTerrainModeEnabled(self.view, True)
def run(self, display): """Launches and displays the simulator. Args: display: Displays the simulator or not. """ if display: widget = QtGui.QWidget() layout = QtGui.QVBoxLayout(widget) layout.addWidget(self._view) widget.showMaximized() # Set camera. applogic.resetCamera(viewDirection=[0.2, 0, -1]) # Set timer. self._tick_count = 0 self._timer = TimerCallback(targetFps=120) self._timer.callback = self.tick self._timer.start() self._app.start()
def run(self, display): """Launches and displays the simulator. Args: display: Displays the simulator or not. """ if display: widget = QtGui.QWidget() layout = QtGui.QVBoxLayout(widget) layout.addWidget(self._view) widget.showMaximized() # Set camera. applogic.resetCamera(viewDirection=[0.2, 0, -1]) # Set timer. self._tick_count = 0 self._timer = TimerCallback(targetFps=120) self._timer.callback = self.tick self._timer.start() self._app.start()
def createView(self, useGrid=True): view = PythonQt.dd.ddQVTKWidgetView() view.resize(600, 400) applogic.setCameraTerrainModeEnabled(view, True) if useGrid: self.gridObj = vis.showGrid(view, parent='scene') self.viewOptions = vis.ViewOptionsItem(view) om.addToObjectModel(self.viewOptions, parentObj=om.findObjectByName('scene')) applogic.resetCamera(viewDirection=[-1,-1,-0.3], view=view) self.viewBehaviors = viewbehaviors.ViewBehaviors(view) applogic._defaultRenderView = view applogic.addShortcut(view, 'Ctrl+Q', self.quit) applogic.addShortcut(view, 'F8', self.showPythonConsole) applogic.addShortcut(view, 'F1', self.showObjectModel) view.setWindowIcon(om.Icons.getIcon(om.Icons.Robot)) view.setWindowTitle('View') return view
def createView(self, useGrid=True): view = PythonQt.dd.ddQVTKWidgetView() view.resize(600, 400) applogic.setCameraTerrainModeEnabled(view, True) if useGrid: self.gridObj = vis.showGrid(view, parent='scene') self.viewOptions = vis.ViewOptionsItem(view) om.addToObjectModel(self.viewOptions, parentObj=om.findObjectByName('scene')) applogic.resetCamera(viewDirection=[-1,-1,-0.3], view=view) self.viewBehaviors = viewbehaviors.ViewBehaviors(view) applogic._defaultRenderView = view applogic.addShortcut(view, 'Ctrl+Q', self.quit) applogic.addShortcut(view, 'F8', self.showPythonConsole) applogic.addShortcut(view, 'F1', self.showObjectModel) view.setWindowIcon(om.Icons.getIcon(om.Icons.Robot)) view.setWindowTitle('View') return view
def __init__(self): self.applicationInstance().setOrganizationName("RobotLocomotionGroup") self.applicationInstance().setApplicationName("drake-visualizer") 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) # This setting improves the near plane clipping resolution. # Drake often draws a very large ground plane which is detrimental to # the near clipping for up close objects. The trade-off is Z buffer # resolution but in practice things look good with this setting. self.view.renderer().SetNearClippingPlaneTolerance(0.0005) # add view behaviors self.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.settings = QtCore.QSettings() self.fileMenu = self.mainWindow.menuBar().addMenu("&File") self.viewMenu = self.mainWindow.menuBar().addMenu("&View") self.viewMenuManager = PythonQt.dd.ddViewMenu(self.viewMenu) self.drakeVisualizer = DrakeVisualizer(self.view) self.lcmglManager = lcmgl.LCMGLManager(self.view) if lcmgl.LCMGL_AVAILABLE else None model = om.getDefaultObjectModel() model.getTreeWidget().setWindowTitle("Scene Browser") model.getPropertiesPanel().setWindowTitle("Properties Panel") self.sceneBrowserDock = self.addWidgetToDock(model.getTreeWidget(), QtCore.Qt.LeftDockWidgetArea, visible=False) self.propertiesDock = self.addWidgetToDock( self.wrapScrollArea(model.getPropertiesPanel()), QtCore.Qt.LeftDockWidgetArea, visible=False ) self.addViewMenuSeparator() self.screenGrabberPanel = ScreenGrabberPanel(self.view) self.screenGrabberDock = self.addWidgetToDock( self.screenGrabberPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False ) self.cameraBookmarksPanel = camerabookmarks.CameraBookmarkWidget(self.view) self.cameraBookmarksDock = self.addWidgetToDock( self.cameraBookmarksPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False ) self.cameraControlPanel = cameracontrolpanel.CameraControlPanel(self.view) self.cameraControlDock = self.addWidgetToDock( self.cameraControlPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False ) act = self.fileMenu.addAction("&Quit") act.setShortcut(QtGui.QKeySequence("Ctrl+Q")) act.connect("triggered()", self.applicationInstance().quit) self.fileMenu.addSeparator() act = self.fileMenu.addAction("&Open Data...") act.setShortcut(QtGui.QKeySequence("Ctrl+O")) act.connect("triggered()", self._onOpenDataFile) applogic.addShortcut(self.mainWindow, "F1", self._toggleObjectModel) applogic.addShortcut(self.mainWindow, "F8", applogic.showPythonConsole) self.applicationInstance().connect("aboutToQuit()", self._onAboutToQuit) for obj in om.getObjects(): obj.setProperty("Deletable", False) self.mainWindow.show() self._saveWindowState("MainWindowDefault") self._restoreWindowState("MainWindowCustom")
def __init__(self, cameraView, modelPolyData=None, pointCloud=None, resultsDict=None, visualize=True, callback=None): self.modelPolyData = modelPolyData self.pointCloud = pointCloud self.resultsDict = resultsDict self.visualize = visualize self.callback = callback self.parent = om.getOrCreateContainer('object alignment') self.meshPoints = None self.imagePoints = None self.cameraView = cameraView # viewer for the object self.view = PythonQt.dd.ddQVTKWidgetView() # add some special logic to show colors if they exist colorByName = 'RGB255' if not self.modelPolyData.GetPointData().GetArray(colorByName): colorByName = None vis.showPolyData(self.modelPolyData, 'object poly data', view=self.view, parent=self.parent, colorByName=colorByName) self.imageFitter = ImageFitter(self, pointCloud) self.picker = pointpicker.PointPicker(self.view) self.picker.pickType = 'cells' # might need to change this self.picker.numberOfPoints = 3 self.picker.annotationName = 'mesh annotation' self.picker.annotationFunc = self.onPickPoints self.picker.start() # viewer for the pointcloud self.sceneView = PythonQt.dd.ddQVTKWidgetView() vis.showPolyData(self.pointCloud, 'pointcloud', view=self.sceneView, colorByName='RGB', parent=self.parent) self.scenePicker = pointpicker.PointPicker(self.sceneView) self.scenePicker.pickType = 'points' # might need to change this self.scenePicker.numberOfPoints = 3 self.scenePicker.annotationName = 'pointcloud annotation' self.scenePicker.annotationFunc = self.onScenePickPoints self.scenePicker.start() # workaround bug in PointPicker implementation for name in [self.picker.annotationName, self.scenePicker.annotationName]: om.removeFromObjectModel(om.findObjectByName(name)) self.widget = QtGui.QWidget() layout = QtGui.QHBoxLayout(self.widget) # layout.addWidget(self.cameraView.view) layout.addWidget(self.sceneView) layout.addWidget(self.view) # self.widget.resize(800, 400) self.widget.showMaximized() self.widget.setWindowTitle('Camera Alignment Tool') self.widget.show() self.viewBehaviors = viewbehaviors.ViewBehaviors(self.view) applogic.resetCamera(viewDirection=[0,1,0], view=self.view) applogic.setCameraTerrainModeEnabled(self.view, True) self.sceneViewBehaviors = viewbehaviors.ViewBehaviors(self.sceneView) applogic.resetCamera(viewDirection=[0, 1, 0], view=self.sceneView) applogic.setCameraTerrainModeEnabled(self.sceneView, True)
d = DebugData() d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0]) show(d, (0, 4, 0)) d = DebugData() d.addCube(dimensions=[0.8, 0.5, 0.3], center=[0, 0, 0], color=[0, 1, 1]) show(d, (2, 4, 0)) d = DebugData() d.addPlane(origin=[0, 0, 0], normal=[0, 0, 1], width=0.8, height=0.7, resolution=10, color=[0, 1, 0]) show(d, (4, 4, 0)).setProperty('Surface Mode', 'Surface with edges') d = DebugData() d.addCapsule(center=[0, 0, 0], axis=[1, 0, 0], length=1.0, radius=0.1, color=[0.5, 0.5, 1]) show(d, (6, 4, 0)) d = DebugData() polyData = vnp.numpyToPolyData(np.random.random((1000, 3))) vnp.addNumpyToVtk(polyData, np.arange(polyData.GetNumberOfPoints()), 'point_ids') d.addPolyData(polyData) show(d, (2.5, 5, 0)).setProperty('Color By', 'point_ids') applogic.resetCamera(viewDirection=[0, 0.1, -1]) app.start()
chull.setProperty('Surface Mode', 'Surface with edges') chull.actor.GetProperty().SetLineWidth(3) center = segmentation.computeCentroid(chull.polyData) chullPoints = vnp.getNumpyFromVtk(chull.polyData, 'Points') d.addLine(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.005 * np.array(plane.GetNormal()), radius=0.0001, color=[0,0,0]) #d.addArrow(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.01 * np.array(plane.GetNormal()), headRadius=0.001, tubeRadius=0.0002) #d.addSphere(chullPoints[0], radius=0.001, color=[1,0,0]) #d.addSphere(chullPoints[1], radius=0.001, color=[0,1,0]) vis.showPolyData(d.getPolyData(), 'plane normals', colorByName='RGB255') #saveConvexHulls(chulls, name) #vis.showPolyData(ioUtils.readPolyData(os.path.join(name, 'merged_planes.ply')), 'merged_planes') applogic.resetCamera([1,1,0]) applogic.setBackgroundColor([1,1,1]) view.orientationMarkerWidget().Off() app.gridObj.setProperty('Color', [0,0,0]) app.gridObj.setProperty('Surface Mode', 'Surface with edges') orbit = cameracontrol.OrbitController(view) screenGrabberPanel = screengrabberpanel.ScreenGrabberPanel(view) if app.getTestingInteractiveEnabled(): view.show() view.resize(1280, 1024) app.start()
msg.utime = 1 lcmUtils.publish('ENABLE_ENCODERS', msg) def disableArmEncoders(): msg = lcmdrc.utime_t() msg.utime = -1 lcmUtils.publish('ENABLE_ENCODERS', msg) def sendDesiredPumpPsi(desiredPsi): atlasDriver.sendDesiredPumpPsi(desiredPsi) app.setCameraTerrainModeEnabled(view, True) app.resetCamera(viewDirection=[-1,0,0], view=view) # Drill Demo Functions for in-image rendering: useDrillDemo = False if useDrillDemo: def spawnHandAtCurrentLocation(side='left'): if (side is 'left'): tf = transformUtils.copyFrame( getLinkFrame( 'l_hand_face') ) handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'left') else: tf = transformUtils.copyFrame( getLinkFrame( 'right_pointer_tip') ) handFactory.placeHandModelWithTransform( tf , app.getCurrentView(), 'right') def drawFrameInCamera(t, frameName='new frame',visible=True):
center = segmentation.computeCentroid(chull.polyData) chullPoints = vnp.getNumpyFromVtk(chull.polyData, 'Points') d.addLine(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.005 * np.array(plane.GetNormal()), radius=0.0001, color=[0, 0, 0]) #d.addArrow(plane.GetOrigin(), np.array(plane.GetOrigin()) + 0.01 * np.array(plane.GetNormal()), headRadius=0.001, tubeRadius=0.0002) #d.addSphere(chullPoints[0], radius=0.001, color=[1,0,0]) #d.addSphere(chullPoints[1], radius=0.001, color=[0,1,0]) vis.showPolyData(d.getPolyData(), 'plane normals', colorByName='RGB255') #saveConvexHulls(chulls, name) #vis.showPolyData(ioUtils.readPolyData(os.path.join(name, 'merged_planes.ply')), 'merged_planes') applogic.resetCamera([1, 1, 0]) applogic.setBackgroundColor([1, 1, 1]) view.orientationMarkerWidget().Off() app.gridObj.setProperty('Color', [0, 0, 0]) app.gridObj.setProperty('Surface Mode', 'Surface with edges') orbit = cameracontrol.OrbitController(view) screenGrabberPanel = screengrabberpanel.ScreenGrabberPanel(view) if app.getTestingInteractiveEnabled(): view.show() view.resize(1280, 1024) app.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 = drcargs.getDirectorConfig() neckPitchJoint = 'neck_ay' colorMode = 'URDF Colors' if 'colorMode' in directorConfig: assert directorConfig['colorMode'] in ['URDF Colors', 'Solid Color', 'Textures'] colorMode = directorConfig['colorMode'] 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, colorMode=colorMode) 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.getRobotOrangeColor(), visible=False, colorMode=colorMode) teleopRobotModel, teleopJointController = roboturdf.loadRobotModel('teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False, colorMode=colorMode) 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() handFactory = roboturdf.HandFactory(robotStateModel) handModels = [] for side in ['left', 'right']: if side in handFactory.defaultHandTypes: handModels.append(handFactory.getLoader(side)) ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels) manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner) affordanceManager = affordancemanager.AffordanceObjectModelManager(view) affordanceitems.MeshAffordanceItem.getMeshManager().initLCM() affordanceitems.MeshAffordanceItem.getMeshManager().collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() segmentation.affordanceManager = affordanceManager plannerPub = plannerPublisher.PlannerPublisher(ikPlanner,affordanceManager,ikRobotModel) 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) viewBehaviors = None robotSystemArgs = dict(locals()) for arg in ['globalsDict', 'self']: del robotSystemArgs[arg] if globalsDict is not None: globalsDict.update(robotSystemArgs) robotSystem = FieldContainer(**robotSystemArgs) robotSystem.viewBehaviors = robotviewbehaviors.RobotViewBehaviors(view, robotSystem) return robotSystem
def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) sliderZVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderZVelocity.connect('valueChanged(int)', self.onZVelocityChanged) sliderZVelocity.setMaximum(self.max_velocity/4.0) sliderZVelocity.setMinimum(-self.max_velocity/4.0) l.addWidget(sliderZVelocity) addAccelSphereButton = QtGui.QPushButton('Toggle Accel Sphere') addAccelSphereButton.connect('clicked()', self.onAddAccelSphereButton) l.addWidget(addAccelSphereButton) addGravityButton = QtGui.QPushButton('Toggle Gravity') addGravityButton.connect('clicked()', self.onAddGravityButton) l.addWidget(addGravityButton) sliderAccelThrust = QtGui.QSlider(QtCore.Qt.Horizontal) sliderAccelThrust.connect('valueChanged(int)', self.onAccelThrustChanged) sliderAccelThrust.setMaximum(self.ActionSet.a_max*10.0) sliderAccelThrust.setMinimum(9.8*10.0) l.addWidget(sliderAccelThrust) sliderRoll = QtGui.QSlider(QtCore.Qt.Horizontal) sliderRoll.connect('valueChanged(int)', self.onRollChanged) sliderRoll.setMaximum(np.pi/2.0*70.0/90.0*10.0) sliderRoll.setMinimum(-np.pi/2.0*70.0/90.0*10.0) l.addWidget(sliderRoll) sliderPitch = QtGui.QSlider(QtCore.Qt.Horizontal) sliderPitch.connect('valueChanged(int)', self.onPitchChanged) sliderPitch.setMaximum(np.pi/2.0*70.0/90.0*10.0) sliderPitch.setMinimum(-np.pi/2.0*70.0/90.0*10.0) l.addWidget(sliderPitch) sliderJerkTime = QtGui.QSlider(QtCore.Qt.Horizontal) sliderJerkTime.connect('valueChanged(int)', self.onJerkTimeChanged) sliderJerkTime.setMaximum(self.ActionSet.t_f*30) sliderJerkTime.setMinimum(0.0) l.addWidget(sliderJerkTime) firstRaycast = np.ones((self.Sensor.numRays,1))*10.0 + np.random.randn(self.Sensor.numRays,1)*1.0 self.drawFirstIntersections(self.frame, firstRaycast) # randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal') # randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton) # l.addWidget(randomGlobalGoalButton) randomObstaclesButton = QtGui.QPushButton('Initialize Random Obstacles') randomObstaclesButton.connect('clicked()', self.onRandomObstaclesButton) l.addWidget(randomObstaclesButton) self.setInitialStateAtZero() # buildWorldFromRandomObstaclesButton = QtGui.QPushButton('Generate Polygon World') # buildWorldFromRandomObstaclesButton.connect('clicked()', self.onBuildWorldFromRandomObstacles) # l.addWidget(buildWorldFromRandomObstaclesButton) # findLocalGoalButton = QtGui.QPushButton('Find Local Goal (Heuristic)') # findLocalGoalButton.connect('clicked()', self.onFindLocalGoalButton) # l.addWidget(findLocalGoalButton) drawActionSetButton = QtGui.QPushButton('Toggle Action Set') drawActionSetButton.connect('clicked()', self.onToggleActionSetButton) l.addWidget(drawActionSetButton) drawFunnelsButton = QtGui.QPushButton('Toggle 1-sigma') drawFunnelsButton.connect('clicked()', self.onDrawFunnelsButton) l.addWidget(drawFunnelsButton) sliderFunnelNumber = QtGui.QSlider(QtCore.Qt.Horizontal) sliderFunnelNumber.connect('valueChanged(int)', self.onFunnelNumberChanged) sliderFunnelNumber.setMaximum(np.size(self.ActionSet.a_vector,0)-1) sliderFunnelNumber.setMinimum(0) l.addWidget(sliderFunnelNumber) # runSimButton = QtGui.QPushButton('Simulate') # runSimButton.connect('clicked()', self.onRunSimButton) # l.addWidget(runSimButton) # playButton = QtGui.QPushButton('Play/Pause') # playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider4.setMaximum(self.sliderMax) # l.addWidget(slider4) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider6.setMaximum(self.sliderMax) # l.addWidget(slider6) # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider7.setMaximum(self.sliderMax) # l.addWidget(slider7) # l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.onRandomObstaclesButton() self.app.start()
# ## This adds random heat map # randomHeatMap = np.random.randn(len(pts)) # print "heatMap ", np.shape(randomHeatMap) # vnp.addNumpyToVtk(grid, randomHeatMap, 'heat_map') def clipByPlane(polyData, planeOrigin, planeNormal): f = vtk.vtkClipPolyData() f.SetInput(polyData) p = vtk.vtkPlane() p.SetOrigin(planeOrigin) p.SetNormal(planeNormal) f.SetClipFunction(p) f.Update() return shallowCopy(f.GetOutput()) # grid = clipByPlane(grid, (0,0,10), (0,0,1)) # grid = clipByPlane(grid, (0,0,30), (0,0,-1)) gridObj = vis.showPolyData(grid, 'heat map', colorByName='heat_map', parent='scene') gridObj.setProperty('Surface Mode', 'Surface with edges') prop = gridObj.actor.GetProperty() prop.LightingOff() #prop.LightingOn() applogic.resetCamera() view.show() app.start()
w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(view) l.addWidget(panel) w.showMaximized() world = buildWorld() robot = buildRobot() locator = buildCellLocator(world.polyData) frame = robot.getChildFrame() frame.setProperty('Edit', True) frame.widget.HandleRotationEnabledOff() rep = frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frame.connectFrameModified(updateDrawIntersection) updateDrawIntersection(frame) # Simulate evolvedState = simulate() applogic.resetCamera(viewDirection=[0.2,0,-1]) view.showMaximized() view.raise_() app.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 = drcargs.getDirectorConfig() neckPitchJoint = 'neck_ay' colorMode = 'URDF Colors' if 'colorMode' in directorConfig: assert directorConfig['colorMode'] in [ 'URDF Colors', 'Solid Color', 'Textures' ] colorMode = directorConfig['colorMode'] 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, colorMode=colorMode) 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.getRobotOrangeColor(), visible=False, colorMode=colorMode) teleopRobotModel, teleopJointController = roboturdf.loadRobotModel( 'teleop model', view, urdfFile=directorConfig['urdfConfig']['teleop'], parent='planning', color=roboturdf.getRobotBlueColor(), visible=False, colorMode=colorMode) 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() handFactory = roboturdf.HandFactory(robotStateModel) handModels = [] for side in ['left', 'right']: if side in handFactory.defaultHandTypes: handModels.append(handFactory.getLoader(side)) ikPlanner = ikplanner.IKPlanner(ikServer, ikRobotModel, ikJointController, handModels) manipPlanner = robotplanlistener.ManipulationPlanDriver(ikPlanner) affordanceManager = affordancemanager.AffordanceObjectModelManager( view) affordanceitems.MeshAffordanceItem.getMeshManager().initLCM() affordanceitems.MeshAffordanceItem.getMeshManager( ).collection.sendEchoRequest() affordanceManager.collection.sendEchoRequest() segmentation.affordanceManager = affordanceManager plannerPub = plannerPublisher.PlannerPublisher( ikPlanner, affordanceManager, ikRobotModel) 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) viewBehaviors = None robotSystemArgs = dict(locals()) for arg in ['globalsDict', 'self']: del robotSystemArgs[arg] if globalsDict is not None: globalsDict.update(robotSystemArgs) robotSystem = FieldContainer(**robotSystemArgs) robotSystem.viewBehaviors = robotviewbehaviors.RobotViewBehaviors( view, robotSystem) return robotSystem
numRays = 20 angleMin = -np.pi / 2 angleMax = np.pi / 2 angleGrid = np.linspace(angleMin, angleMax, numRays) rays = np.zeros((3, numRays)) rays[0, :] = np.cos(angleGrid) rays[1, :] = np.sin(angleGrid) app = ConsoleApp() view = app.createView() world = buildWorld() robot = buildRobot() locator = buildCellLocator(world.polyData) frame = robot.getChildFrame() frame.setProperty('Edit', True) frame.widget.HandleRotationEnabledOff() rep = frame.widget.GetRepresentation() rep.SetTranslateAxisEnabled(2, False) rep.SetRotateAxisEnabled(0, False) rep.SetRotateAxisEnabled(1, False) frame.connectFrameModified(updateIntersection) updateIntersection(frame) applogic.resetCamera(viewDirection=[0.2, 0, -1]) view.showMaximized() view.raise_() app.start()
def initView(self, fields): view = PythonQt.dd.ddQVTKWidgetView() applogic._defaultRenderView = view applogic.setCameraTerrainModeEnabled(view, True) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=view) return FieldContainer(view=view)
def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0 / self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect("valueChanged(int)", self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect("valueChanged(int)", self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) firstRaycast = np.ones((self.Sensor.numRays, 1)) * 10.0 + np.random.randn(self.Sensor.numRays, 1) * 1.0 self.drawFirstIntersections(self.frame, firstRaycast) randomGlobalGoalButton = QtGui.QPushButton("Initialize Random Global Goal") randomGlobalGoalButton.connect("clicked()", self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) randomObstaclesButton = QtGui.QPushButton("Initialize Random Obstacles") randomObstaclesButton.connect("clicked()", self.onRandomObstaclesButton) l.addWidget(randomObstaclesButton) buildWorldFromRandomObstaclesButton = QtGui.QPushButton("Generate Polygon World") buildWorldFromRandomObstaclesButton.connect("clicked()", self.onBuildWorldFromRandomObstacles) l.addWidget(buildWorldFromRandomObstaclesButton) findLocalGoalButton = QtGui.QPushButton("Find Local Goal (Heuristic)") findLocalGoalButton.connect("clicked()", self.onFindLocalGoalButton) l.addWidget(findLocalGoalButton) drawActionSetButton = QtGui.QPushButton("Draw Action Set") drawActionSetButton.connect("clicked()", self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton("Simulate") runSimButton.connect("clicked()", self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton("Play/Pause") playButton.connect("clicked()", self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect("valueChanged(int)", self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider4.setMaximum(self.sliderMax) # l.addWidget(slider4) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider6.setMaximum(self.sliderMax) # l.addWidget(slider6) # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider7.setMaximum(self.sliderMax) # l.addWidget(slider7) l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2, 0, -1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter / elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.onRandomObstaclesButton() self.app.start()
import numpy as np app = consoleapp.ConsoleApp() view = app.createView() cellSize = 0.5 numberOfCells = 20 grid = vtk.vtkGridSource() grid.SetScale(cellSize) grid.SetGridSize(numberOfCells) grid.SetSurfaceEnabled(True) grid.Update() grid = grid.GetOutput() pts = vnp.getNumpyFromVtk(grid, 'Points') heatMap = np.random.randn(len(pts)) vnp.addNumpyToVtk(grid, heatMap, 'heat_map') gridObj = vis.showPolyData(grid, 'heat map', colorByName='heat_map', parent='scene') gridObj.setProperty('Surface Mode', 'Surface with edges') applogic.resetCamera() view.show() app.start()
def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) self.max_velocity = 20.0 sliderXVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderXVelocity.connect('valueChanged(int)', self.onXVelocityChanged) sliderXVelocity.setMaximum(self.max_velocity) sliderXVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderXVelocity) sliderYVelocity = QtGui.QSlider(QtCore.Qt.Horizontal) sliderYVelocity.connect('valueChanged(int)', self.onYVelocityChanged) sliderYVelocity.setMaximum(self.max_velocity) sliderYVelocity.setMinimum(-self.max_velocity) l.addWidget(sliderYVelocity) randomGlobalGoalButton = QtGui.QPushButton('Initialize Random Global Goal') randomGlobalGoalButton.connect('clicked()', self.onRandomGlobalGoalButton) l.addWidget(randomGlobalGoalButton) drawActionSetButton = QtGui.QPushButton('Draw Action Set') drawActionSetButton.connect('clicked()', self.onDrawActionSetButton) l.addWidget(drawActionSetButton) runSimButton = QtGui.QPushButton('Simulate') runSimButton.connect('clicked()', self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider l.addWidget(playButton) l.addWidget(slider) toggleSensorNoiseButton = QtGui.QPushButton('Toggle Sensor Noise') toggleSensorNoiseButton.connect('clicked()', self.onToggleSensorNoise) l.addWidget(toggleSensorNoiseButton) showObstaclesButton = QtGui.QPushButton('Show Obsatacles') showObstaclesButton.connect('clicked()', self.onShowObstacles) l.addWidget(showObstaclesButton) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() # need to connect frames with DrawActionSet self.frame.connectFrameModified(self.updateDrawActionSet) self.updateDrawActionSet(self.frame) self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start()
#view.show() w = QtGui.QWidget() l = QtGui.QGridLayout(w) l.addWidget(view, 0, 0) l.addWidget(robotSystem.playbackPanel.widget, 1, 0) l.addWidget(robotSystem.teleopPanel.widget, 0, 1, 2, 1) l.setMargin(0) l.setSpacing(0) w.showMaximized() w.raise_() from director import applogic applogic.resetCamera(viewDirection=[-1,0,0], view=view) app.start() ''' Matlab constraints example: default_shrink_factor = 0.2; qsc_constraint_0 = QuasiStaticConstraint(r, [-inf, inf], 1); qsc_constraint_0 = qsc_constraint_0.setShrinkFactor(default_shrink_factor); qsc_constraint_0 = qsc_constraint_0.setActive(true); qsc_constraint_0 = qsc_constraint_0.addContact(links.l_foot, l_foot_pts);
def initView(self, fields): view = PythonQt.dd.ddQVTKWidgetView() applogic._defaultRenderView = view applogic.setCameraTerrainModeEnabled(view, True) applogic.resetCamera(viewDirection=[-1, -1, -0.3], view=view) return FieldContainer(view=view)
app.app.addWidgetToDock(robotSystem.teleopPanel.widget, QtCore.Qt.RightDockWidgetArea) app.app.addWidgetToDock(robotSystem.playbackPanel.widget, QtCore.Qt.BottomDockWidgetArea) setupToolbar() # show sim time in the status bar infoLabel = KukaSimInfoLabel(app.mainWindow.statusBar()) # use pydrake ik backend ikPlanner = robotSystem.ikPlanner if ikPlanner.planningMode == 'pydrake': ikPlanner.plannerPub._setupLocalServer() # change the default animation mode of the playback panel robotSystem.playbackPanel.animateOnExecute = True # disable pointwise ik by default ikPlanner.getIkOptions().setProperty('Use pointwise', False) ikPlanner.getIkOptions().setProperty('Max joint degrees/s', 60) # initialize the listener for the pose gui ikPlanner.addPostureGoalListener(robotSystem.robotStateJointController) # set the default camera view applogic.resetCamera(viewDirection=[-1, 0, 0], view=app.view) # start! app.app.start()
def __init__(self): self.applicationInstance().setOrganizationName('RobotLocomotionGroup') self.applicationInstance().setApplicationName('drake-visualizer') 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) # This setting improves the near plane clipping resolution. # Drake often draws a very large ground plane which is detrimental to # the near clipping for up close objects. The trade-off is Z buffer # resolution but in practice things look good with this setting. self.view.renderer().SetNearClippingPlaneTolerance(0.0005) # add view behaviors self.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.settings = QtCore.QSettings() self.fileMenu = self.mainWindow.menuBar().addMenu('&File') self.viewMenu = self.mainWindow.menuBar().addMenu('&View') self.viewMenuManager = PythonQt.dd.ddViewMenu(self.viewMenu) self.drakeVisualizer = DrakeVisualizer(self.view) self.lcmglManager = lcmgl.LCMGLManager( self.view) if lcmgl.LCMGL_AVAILABLE else None model = om.getDefaultObjectModel() model.getTreeWidget().setWindowTitle('Scene Browser') model.getPropertiesPanel().setWindowTitle('Properties Panel') self.sceneBrowserDock = self.addWidgetToDock( model.getTreeWidget(), QtCore.Qt.LeftDockWidgetArea, visible=False) self.propertiesDock = self.addWidgetToDock( self.wrapScrollArea(model.getPropertiesPanel()), QtCore.Qt.LeftDockWidgetArea, visible=False) self.addViewMenuSeparator() self.screenGrabberPanel = ScreenGrabberPanel(self.view) self.screenGrabberDock = self.addWidgetToDock( self.screenGrabberPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) self.cameraBookmarksPanel = camerabookmarks.CameraBookmarkWidget( self.view) self.cameraBookmarksDock = self.addWidgetToDock( self.cameraBookmarksPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) self.cameraControlPanel = cameracontrolpanel.CameraControlPanel( self.view) self.cameraControlDock = self.addWidgetToDock( self.cameraControlPanel.widget, QtCore.Qt.RightDockWidgetArea, visible=False) act = self.fileMenu.addAction('&Quit') act.setShortcut(QtGui.QKeySequence('Ctrl+Q')) act.connect('triggered()', self.applicationInstance().quit) self.fileMenu.addSeparator() act = self.fileMenu.addAction('&Open Data...') act.setShortcut(QtGui.QKeySequence('Ctrl+O')) act.connect('triggered()', self._onOpenDataFile) applogic.addShortcut(self.mainWindow, 'F1', self._toggleObjectModel) applogic.addShortcut(self.mainWindow, 'F8', applogic.showPythonConsole) self.applicationInstance().connect('aboutToQuit()', self._onAboutToQuit) for obj in om.getObjects(): obj.setProperty('Deletable', False) self.mainWindow.show() self._saveWindowState('MainWindowDefault') self._restoreWindowState('MainWindowCustom')
def setupPlayback(self): self.timer = TimerCallback(targetFps=30) self.timer.callback = self.tick playButtonFps = 1.0/self.dt print "playButtonFPS", playButtonFps self.playTimer = TimerCallback(targetFps=playButtonFps) self.playTimer.callback = self.playTimerCallback self.sliderMovedByPlayTimer = False panel = QtGui.QWidget() l = QtGui.QHBoxLayout(panel) showSensorsButton = QtGui.QPushButton('Initialize Sensors Randomly') showSensorsButton.connect('clicked()', self.onShowSensorsButton) l.addWidget(showSensorsButton) firstRaycast = np.ones((21,1))*10.0 + np.random.randn(21,1)*1.0 print "firstRaycast initially is ", firstRaycast self.drawFirstIntersections(self.frame, firstRaycast) randomObstaclesButton = QtGui.QPushButton('Initialize Random Obstacles') randomObstaclesButton.connect('clicked()', self.onRandomObstaclesButton) l.addWidget(randomObstaclesButton) buildWorldFromRandomObstaclesButton = QtGui.QPushButton('World From Obstacles') buildWorldFromRandomObstaclesButton.connect('clicked()', self.onBuildWorldFromRandomObstacles) l.addWidget(buildWorldFromRandomObstaclesButton) runSimButton = QtGui.QPushButton('Run simulation') runSimButton.connect('clicked()', self.onRunSimButton) l.addWidget(runSimButton) playButton = QtGui.QPushButton('Play/Pause') playButton.connect('clicked()', self.onPlayButton) slider = QtGui.QSlider(QtCore.Qt.Horizontal) slider.connect('valueChanged(int)', self.onSliderChanged) self.sliderMax = self.numTimesteps slider.setMaximum(self.sliderMax) self.slider = slider # slider2 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider2.setMaximum(self.sliderMax) # l.addWidget(slider2) # slider3 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider3.setMaximum(self.sliderMax) # l.addWidget(slider3) # slider4 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider4.setMaximum(self.sliderMax) # l.addWidget(slider4) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider5 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider5.setMaximum(self.sliderMax) # l.addWidget(slider5) # slider6 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider6.setMaximum(self.sliderMax) # l.addWidget(slider6) # slider7 = QtGui.QSlider(QtCore.Qt.Horizontal) # slider7.setMaximum(self.sliderMax) # l.addWidget(slider7) l.addWidget(playButton) l.addWidget(slider) w = QtGui.QWidget() l = QtGui.QVBoxLayout(w) l.addWidget(self.view) self.view.orientationMarkerWidget().Off() l.addWidget(panel) w.showMaximized() self.frame.connectFrameModified(self.updateDrawIntersection) self.updateDrawIntersection(self.frame) applogic.resetCamera(viewDirection=[0.2,0,-1]) self.view.showMaximized() self.view.raise_() panel = screengrabberpanel.ScreenGrabberPanel(self.view) panel.widget.show() cameracontrolpanel.CameraControlPanel(self.view).widget.show() elapsed = time.time() - self.startSimTime simRate = self.counter/elapsed print "Total run time", elapsed print "Ticks (Hz)", simRate print "Number of steps taken", self.counter self.app.start()