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