def resetTargetPath(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target frame desired':
om.removeFromObjectModel(obj)
for obj in om.getObjects():
if obj.getProperty('Name') == 'target frame desired path':
om.removeFromObjectModel(obj)
def resetTargetPath(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target frame desired':
om.removeFromObjectModel(obj)
for obj in om.getObjects():
if obj.getProperty('Name') == 'target frame desired path':
om.removeFromObjectModel(obj)
def getCandidates(self):
namePrefix = self.properties.getProperty('Candidate name prefix')
matchStr = '^%s [0-9]+$' % namePrefix
return [
obj for obj in om.getObjects()
if re.match(matchStr, obj.getProperty('Name'))
]
def getObjectByProp(prop):
if not prop:
return None
for obj in om.getObjects():
if isinstance(obj, FrameItem) and obj.widget.GetRepresentation() == prop:
return obj
if isinstance(prop, vtk.vtkActor):
return getObjectByDataSet(prop.GetMapper().GetInput())
def getObjectByProp(prop):
if not prop:
return None
for obj in om.getObjects():
if isinstance(obj, FrameItem) and obj.widget.GetRepresentation() == prop:
return obj
if isinstance(prop, vtk.vtkActor):
return getObjectByDataSet(prop.GetMapper().GetInput())
def updatePolyData(self, viewId, polyData):
obj = self.polyDataObjects.get(viewId)
if obj not in om.getObjects():
obj = None
if not obj:
hiddenMapIds = [9999]
visibleDefault = False if viewId in hiddenMapIds else True
obj = vis.PolyDataItem(self.getNameForViewId(viewId), polyData,
self.view)
obj.setProperty('Visible', visibleDefault)
if obj._isPointCloud():
obj.setProperty('Color', [1, 1, 1])
obj.setProperty('Alpha', 0.5)
else:
obj.setProperty('Color', [0, 0.68, 1])
if viewId == lcmdrc.data_request_t.HEIGHT_MAP_SCENE:
obj.setProperty('Surface Mode', 'Wireframe')
folder = om.findObjectByName('Map Server')
folder.addProperty('Min Range',
self.reader.GetDistanceRange()[0],
attributes=om.PropertyAttributes(
decimals=2,
minimum=0.0,
maximum=100.0,
singleStep=0.25,
hidden=False))
folder.addProperty('Max Range',
self.reader.GetDistanceRange()[1],
attributes=om.PropertyAttributes(
decimals=2,
minimum=0.0,
maximum=100.0,
singleStep=0.25,
hidden=False))
folder.addProperty('Edge Filter Angle',
self.reader.GetEdgeAngleThreshold(),
attributes=om.PropertyAttributes(decimals=0,
minimum=0.0,
maximum=60.0,
singleStep=1,
hidden=False))
om.addToObjectModel(obj, folder)
om.expand(folder)
self.folder = folder
self.polyDataObjects[viewId] = obj
else:
obj.setPolyData(polyData)
if self.colorizeCallback:
self.colorizeCallback(obj)
def approveRefit():
for obj in om.getObjects():
if isinstance(obj, segmentation.BlockAffordanceItem):
if 'refit' in obj.getProperty('Name'):
originalObj = om.findObjectByName(obj.getProperty('Name').replace(' refit', ''))
if originalObj:
originalObj.params = obj.params
originalObj.polyData.DeepCopy(obj.polyData)
originalObj.actor.GetUserTransform().SetMatrix(obj.actor.GetUserTransform().GetMatrix())
originalObj.actor.GetUserTransform().Modified()
obj.setProperty('Visible', False)
def approveRefit():
for obj in om.getObjects():
if isinstance(obj, segmentation.BlockAffordanceItem):
if 'refit' in obj.getProperty('Name'):
originalObj = om.findObjectByName(obj.getProperty('Name').replace(' refit', ''))
if originalObj:
originalObj.params = obj.params
originalObj.polyData.DeepCopy(obj.polyData)
originalObj.actor.GetUserTransform().SetMatrix(obj.actor.GetUserTransform().GetMatrix())
originalObj.actor.GetUserTransform().Modified()
obj.setProperty('Visible', False)
def _onChecked(self):
if self.action.checked:
vtk.vtkFrameWidgetRepresentation.SetBigSizeHandles()
else:
vtk.vtkFrameWidgetRepresentation.SetDefaultSizeHandles()
for obj in om.getObjects():
if obj.getProperty(
"Name") == "walking goal" and type(obj) is vis.FrameItem:
# increase size of the tool to move walking goal
rep = obj.widget.GetRepresentation()
rep.RedrawHandles()
obj._renderAllViews()
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6, 0.2, 0.6],
[180, 0, 90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0, 0, 0]),
np.array([-self.boxLength, 0, 0]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, 0]),
np.array([-self.boxLength, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([-self.boxLength, 0, self.boxLength]),
np.array([0, 0, self.boxLength]),
radius=0.02) # main bar
z.addLine(np.array([0, 0, self.boxLength]),
np.array([0, 0, 0]),
radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(
targetMesh,
'target',
color=[0.0, 1.0, 0.0],
cls=affordanceitems.FrameAffordanceItem,
parent=folder,
alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame,
'target frame',
parent=self.targetAffordance,
visible=False,
scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength,
otdf_type='target',
friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def updatePolyData(self, viewId, polyData):
obj = self.polyDataObjects.get(viewId)
if obj not in om.getObjects():
obj = None
if not obj:
hiddenMapIds = [9999]
visibleDefault = False if viewId in hiddenMapIds else True
obj = vis.PolyDataItem(self.getNameForViewId(viewId), polyData, self.view)
obj.setProperty('Visible', visibleDefault)
if obj._isPointCloud():
obj.setProperty('Color', [1, 1, 1])
obj.setProperty('Alpha', 0.5)
else:
obj.setProperty('Color', [0, 0.68, 1])
if viewId == lcmmaps.data_request_t.HEIGHT_MAP_SCENE:
obj.setProperty('Surface Mode', 'Wireframe')
folder = om.findObjectByName('Map Server')
folder.addProperty('Min Range', self.reader.GetDistanceRange()[0],
attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
folder.addProperty('Max Range', self.reader.GetDistanceRange()[1],
attributes=om.PropertyAttributes(decimals=2, minimum=0.0, maximum=100.0, singleStep=0.25, hidden=False))
folder.addProperty('Edge Filter Angle', self.reader.GetEdgeAngleThreshold(),
attributes=om.PropertyAttributes(decimals=0, minimum=0.0, maximum=60.0, singleStep=1, hidden=False))
folder.addProperty('Min Height', self.reader.GetHeightRange()[0],
attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))
folder.addProperty('Max Height', self.reader.GetHeightRange()[1],
attributes=om.PropertyAttributes(decimals=2, minimum=-80.0, maximum=80.0, singleStep=0.25, hidden=False))
om.addToObjectModel(obj, folder)
om.expand(folder)
self.folder = folder
self.polyDataObjects[viewId] = obj
else:
obj.setPolyData(polyData)
if self.colorizeCallback:
self.colorizeCallback(obj)
def spawnTargetAffordance(self):
for obj in om.getObjects():
if obj.getProperty('Name') == 'target':
om.removeFromObjectModel(obj)
targetFrame = transformUtils.frameFromPositionAndRPY([0.6,0.2,0.6],[180,0,90])
folder = om.getOrCreateContainer('affordances')
z = DebugData()
z.addLine(np.array([0,0,0]), np.array([-self.boxLength,0,0]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,0]), np.array([-self.boxLength,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([-self.boxLength,0,self.boxLength]), np.array([0,0,self.boxLength]), radius=0.02) # main bar
z.addLine(np.array([0,0,self.boxLength]), np.array([0,0,0]), radius=0.02) # main bar
targetMesh = z.getPolyData()
self.targetAffordance = vis.showPolyData(targetMesh, 'target', color=[0.0, 1.0, 0.0], cls=affordanceitems.FrameAffordanceItem, parent=folder, alpha=0.3)
self.targetAffordance.actor.SetUserTransform(targetFrame)
self.targetFrame = vis.showFrame(targetFrame, 'target frame', parent=self.targetAffordance, visible=False, scale=0.2)
self.targetFrame = self.targetFrame.transform
params = dict(length=self.boxLength, otdf_type='target', friendly_name='target')
self.targetAffordance.setAffordanceParams(params)
self.targetAffordance.updateParamsFromActorTransform()
def getObjectByDataSet(polyData):
for obj in om.getObjects():
if obj.hasDataSet(polyData):
return obj
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")
taskPanels['Terrain'] = terrainTaskPanel.widget
taskPanels['Continuous Walking'] = continuousWalkingTaskPanel.widget
tasklaunchpanel.init(taskPanels)
splinewidget.init(view, handFactory, robotStateModel)
rt.robotSystem = robotSystem
taskManagerPanel = taskmanagerwidget.init()
for taskDescription in loadTaskDescriptions():
taskManagerPanel.taskQueueWidget.loadTaskDescription(taskDescription[0], taskDescription[1])
taskManagerPanel.taskQueueWidget.setCurrentQueue('Task library')
for obj in om.getObjects():
obj.setProperty('Deletable', False)
if useCOPMonitor and not ikPlanner.fixedBaseArm:
copMonitor = copmonitor.COPMonitor(robotSystem, view);
if useLoggingWidget:
w = lcmloggerwidget.LCMLoggerWidget(statusBar=app.getMainWindow().statusBar())
app.getMainWindow().statusBar().addPermanentWidget(w.button)
if useControllerRate:
def getExistingRobotModels():
return [obj for obj in om.getObjects() if isinstance(obj, RobotModelItem)]
def getObjectByDataSet(polyData):
for obj in om.getObjects():
if obj.hasDataSet(polyData):
return obj
def getAffordances(self):
return [obj for obj in om.getObjects() if isinstance(obj, affordanceitems.AffordanceItem)]
def getAffordances(self):
return [obj for obj in om.getObjects() if isinstance(obj, affordanceitems.AffordanceItem)]
def getCandidates(self):
namePrefix = self.properties.getProperty('Candidate name prefix')
matchStr = '^%s [0-9]+$' % namePrefix
return [obj for obj in om.getObjects() if re.match(matchStr, obj.getProperty('Name'))]
def getExistingRobotModels():
return [obj for obj in om.getObjects() if isinstance(obj, RobotModelItem)]
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')
taskPanels['Terrain'] = terrainTaskPanel.widget
taskPanels['Continuous Walking'] = continuousWalkingTaskPanel.widget
tasklaunchpanel.init(taskPanels)
splinewidget.init(view, handFactory, robotStateModel)
rt.robotSystem = robotSystem
taskManagerPanel = taskmanagerwidget.init()
for taskDescription in loadTaskDescriptions():
taskManagerPanel.taskQueueWidget.loadTaskDescription(
taskDescription[0], taskDescription[1])
taskManagerPanel.taskQueueWidget.setCurrentQueue('Task library')
for obj in om.getObjects():
obj.setProperty('Deletable', False)
if useCOPMonitor and not ikPlanner.fixedBaseArm:
copMonitor = copmonitor.COPMonitor(robotSystem, view)
if useLoggingWidget:
w = lcmloggerwidget.LCMLoggerWidget(
statusBar=app.getMainWindow().statusBar())
app.getMainWindow().statusBar().addPermanentWidget(w.button)
if useControllerRate:
class ControllerRateLabel(object):
'''
Displays a controller frequency in the status bar
