def _addPlanItem(plan, name, itemClass):
assert plan is not None
item = itemClass(name)
item.plan = plan
om.removeFromObjectModel(om.findObjectByName(name))
om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
return item
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def openUrdf(filename, view):
model = loadRobotModelFromFile(filename)
if model:
model = RobotModelItem(model)
om.addToObjectModel(model)
model.addToView(view)
return model
def showPolyData(polyData, name, color=None, colorByName=None, colorByRange=None, alpha=1.0, visible=True, view=None, parent='segmentation', cls=None):
view = view or app.getCurrentRenderView()
assert view
cls = cls or PolyDataItem
item = cls(name, polyData, view)
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
om.addToObjectModel(item, parentObj)
item.setProperty('Visible', visible)
item.setProperty('Alpha', alpha)
if colorByName and colorByName not in item.getArrayNames():
print 'showPolyData(colorByName=%s): array not found' % colorByName
colorByName = None
if colorByName:
item.setProperty('Color By', colorByName)
item.colorBy(colorByName, colorByRange)
else:
color = [1.0, 1.0, 1.0] if color is None else color
item.setProperty('Color', [float(c) for c in color])
item.colorBy(None)
return item
def openUrdf(filename, view):
model = loadRobotModelFromFile(filename)
if model:
model = RobotModelItem(model)
om.addToObjectModel(model)
model.addToView(view)
return model
def onPointCloud(self, msg, channel):
pointcloudName = channel.replace('DRAKE_POINTCLOUD_', '', 1)
polyData = vnp.numpyToPolyData(np.asarray(msg.points), createVertexCells=True)
# If the user provided color channels, then use them to colorize
# the pointcloud.
channels = {msg.channel_names[i]: msg.channels[i] for i in range(msg.n_channels)}
if "r" in channels and "g" in channels and "b" in channels:
colorized = True
colorArray = np.empty((msg.n_points, 3), dtype=np.uint8)
for (colorIndex, color) in enumerate(["r", "g", "b"]):
colorArray[:, colorIndex] = 255 * np.asarray(channels[color])
vnp.addNumpyToVtk(polyData, colorArray, "rgb")
else:
colorized = False
folder = self.getPointCloudFolder()
# If there was an existing point cloud by this name, then just
# set its polyData to the new point cloud.
# This has the effect of preserving all the user-specified properties
# like point size, coloration mode, alpha, etc.
previousPointcloud = folder.findChild(pointcloudName)
if previousPointcloud is not None:
previousPointcloud.setPolyData(polyData)
previousPointcloud._updateColorByProperty()
else:
item = vis.PolyDataItem(pointcloudName, polyData, view=None)
item.addToView(self.view)
if colorized:
item._updateColorByProperty()
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=folder)
def loadRobotModel(name, view=None, parent='planning', urdfFile=None, color=None, visible=True, colorMode='URDF Colors'):
if not urdfFile:
urdfFile = drcargs.getDirectorConfig()['urdfConfig']['default']
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
model = loadRobotModelFromFile(urdfFile)
if not model:
raise Exception('Error loading robot model from file: %s' % urdfFile)
obj = RobotModelItem(model)
om.addToObjectModel(obj, parent)
obj.setProperty('Visible', visible)
obj.setProperty('Name', name)
obj.setProperty('Color', color or getRobotGrayColor())
if colorMode == 'Textures':
obj.setProperty('Color Mode', 1)
elif colorMode == 'Solid Color':
obj.setProperty('Color Mode', 0)
elif colorMode == 'URDF Colors':
obj.setProperty('Color Mode', 2)
if view is not None:
obj.addToView(view)
jointController = jointcontrol.JointController([obj])
fixedPointFile = drcargs.getDirectorConfig()['fixedPointFile']
jointController.setPose('q_nom', jointController.loadPoseFromFile(fixedPointFile))
return obj, jointController
def _addPlanItem(plan, name, itemClass):
assert plan is not None
item = itemClass(name)
item.plan = plan
om.removeFromObjectModel(om.findObjectByName(name))
om.addToObjectModel(item, om.getOrCreateContainer('segmentation'))
return item
def onPointCloud(self, msg, channel):
pointcloudName = channel.replace("DRAKE_POINTCLOUD_", "", 1)
polyData = vnp.numpyToPolyData(np.asarray(msg.points), createVertexCells=True)
# If the user provided color channels, then use them to colorize
# the pointcloud.
channels = {msg.channel_names[i]: msg.channels[i] for i in range(msg.n_channels)}
if "r" in channels and "g" in channels and "b" in channels:
colorized = True
colorArray = np.empty((msg.n_points, 3), dtype=np.uint8)
for (colorIndex, color) in enumerate(["r", "g", "b"]):
colorArray[:, colorIndex] = 255 * np.asarray(channels[color])
vnp.addNumpyToVtk(polyData, colorArray, "rgb")
else:
colorized = False
folder = self.getPointCloudFolder()
# If there was an existing point cloud by this name, then just
# set its polyData to the new point cloud.
# This has the effect of preserving all the user-specified properties
# like point size, coloration mode, alpha, etc.
previousPointcloud = folder.findChild(pointcloudName)
if previousPointcloud is not None:
previousPointcloud.setPolyData(polyData)
previousPointcloud._updateColorByProperty()
else:
item = vis.PolyDataItem(pointcloudName, polyData, view=None)
item.addToView(self.view)
if colorized:
item._updateColorByProperty()
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=folder)
def showPolyData(polyData, name, color=None, colorByName=None, colorByRange=None, alpha=1.0, visible=True, view=None, parent='segmentation', cls=None):
view = view or app.getCurrentRenderView()
assert view
cls = cls or PolyDataItem
item = cls(name, polyData, view)
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
om.addToObjectModel(item, parentObj)
item.setProperty('Visible', visible)
item.setProperty('Alpha', alpha)
if colorByName and colorByName not in item.getArrayNames():
print 'showPolyData(colorByName=%s): array not found' % colorByName
colorByName = None
if colorByName:
item.setProperty('Color By', colorByName)
item.colorBy(colorByName, colorByRange)
else:
color = [1.0, 1.0, 1.0] if color is None else color
item.setProperty('Color', [float(c) for c in color])
item.colorBy(None)
return item
def promotePolyDataItem(cls, obj):
parent = obj.parent()
view = obj.views[0]
name = obj.getProperty('Name')
polyData = obj.polyData
props = obj.properties._properties
childFrame = obj.getChildFrame()
if childFrame:
t = transformUtils.copyFrame(childFrame.transform)
else:
t = vtk.vtkTransform()
t.PostMultiply()
t.Translate(filterUtils.computeCentroid(polyData))
polyData = filterUtils.transformPolyData(polyData,
t.GetLinearInverse())
children = [c for c in obj.children() if c is not childFrame]
meshId = cls.getMeshManager().add(polyData)
om.removeFromObjectModel(obj)
obj = MeshAffordanceItem(name, view)
obj.setProperty('Filename', meshId)
om.addToObjectModel(obj, parentObj=parent)
frame = vis.addChildFrame(obj)
frame.copyFrame(t)
for child in children:
om.addToObjectModel(child, parentObj=obj)
obj.syncProperties(props)
return obj
def addGeometry(self, path, geomItems):
folder = self.getPathFolder(path)
ancestors = findPathToAncestor(folder, self.getRootFolder())
geomTransform = vtk.vtkTransform()
for item in reversed(ancestors):
if not hasattr(item, "transform"):
item.transform = vtk.vtkTransform()
item.transform.PostMultiply()
geomTransform.Concatenate(item.transform)
for geom in geomItems:
existing_item = self.getItemByPath(path + ["geometry"])
item = geom.polyDataItem
if existing_item is not None:
for prop in existing_item.propertyNames():
item.setProperty(prop, existing_item.getProperty(prop))
om.removeFromObjectModel(existing_item)
else:
item.setProperty("Point Size", 2)
for colorBy in ["rgb", "intensity"]:
try:
item.setProperty("Color By", colorBy)
except ValueError:
pass
else:
break
item.addToView(self.view)
om.addToObjectModel(item, parentObj=folder)
item.actor.SetUserTransform(geomTransform)
return path
def initDepthPointCloud(imageManager, view):
openniDepthPointCloud = segmentation.DisparityPointCloudItem('openni point cloud', 'OPENNI_FRAME', 'OPENNI_FRAME_LEFT', imageManager)
openniDepthPointCloud.addToView(view)
om.addToObjectModel(openniDepthPointCloud, parentObj=om.findObjectByName('sensors'))
openniDepthPointCloud.setProperty('Visible', True)
openniDepthPointCloud.setProperty('Target FPS', 30)
return openniDepthPointCloud
def _loadAffordanceFromDescription(self, desc):
className = desc['classname']
cls = getattr(affordanceitems, className)
aff = cls(desc['Name'], self.view)
om.addToObjectModel(aff, parentObj=om.getOrCreateContainer('affordances'))
vis.addChildFrame(aff)
aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
self.registerAffordance(aff, notify=False)
def addURDFModel(self, path):
model = rob.loadRobotModelFromFile(path)
if model:
model = rob.RobotModelItem(model)
om.addToObjectModel(model, parentObj=self.getModelObjectsFolder())
model.addToView(app.getCurrentRenderView())
else:
print "bad urdf model"
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 _loadAffordanceFromDescription(self, desc):
className = desc['classname']
cls = getattr(affordanceitems, className)
aff = cls(desc['Name'], self.view)
om.addToObjectModel(aff, parentObj=om.getOrCreateContainer('affordances'))
frame = vis.addChildFrame(aff)
frame.setProperty('Deletable', False)
aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
self.registerAffordance(aff, notify=False)
def _loadAffordanceFromDescription(self, desc):
className = desc["classname"]
cls = getattr(affordanceitems, className)
aff = cls(desc["Name"], self.view)
om.addToObjectModel(aff, parentObj=om.getOrCreateContainer("affordances"))
frame = vis.addChildFrame(aff)
frame.setProperty("Deletable", False)
aff.loadDescription(desc, copyMode=aff.COPY_MODE_ALL)
self.registerAffordance(aff, notify=False)
def addLinkGeometry(self, geom, linkName, linkFolder):
geom.polyDataItem.addToView(self.view)
om.addToObjectModel(geom.polyDataItem, parentObj=linkFolder)
if linkName == 'world':
#geom.polyDataItem.actor.SetUseBounds(False)
#geom.polyDataItem.actor.GetProperty().LightingOff()
geom.polyDataItem.actor.GetProperty().SetSpecular(0.0)
else:
geom.polyDataItem.actor.GetProperty().SetSpecular(0.9)
geom.polyDataItem.actor.GetProperty().SetSpecularPower(20)
def addLinkGeometry(self, geom, linkName, linkFolder):
geom.polyDataItem.addToView(self.view)
om.addToObjectModel(geom.polyDataItem, parentObj=linkFolder)
if linkName == "world":
# geom.polyDataItem.actor.SetUseBounds(False)
# geom.polyDataItem.actor.GetProperty().LightingOff()
geom.polyDataItem.actor.GetProperty().SetSpecular(0.0)
else:
geom.polyDataItem.actor.GetProperty().SetSpecular(0.9)
geom.polyDataItem.actor.GetProperty().SetSpecularPower(20)
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 newTerrainItem(self, tform, uid=None, region=None):
if uid is None:
uid = self.getNewUID()
elif uid in self.regions:
return self.regions[uid]
view = app.getCurrentRenderView()
item = TerrainRegionItem(uid, view, tform, self, region)
parentObj = om.getOrCreateContainer('Safe terrain regions')
om.addToObjectModel(item, parentObj)
self.regions[uid] = item
return item
def init(view):
global KinectQueue, _kinectItem, _kinectSource
KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread())
KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
_kinectSource = KinectSource(view, KinectQueue)
_kinectSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_kinectItem = KinectItem(_kinectSource)
om.addToObjectModel(_kinectItem, sensorsFolder)
def _init(self, size):
items = self._getPolyDataItems()
for item in items:
om.removeFromObjectModel(item)
for i in range(0, size):
name = self.name if i == 0 else (self.name + str(i))
item = PolyDataItem(name, vtk.vtkPolyData(), view=None)
item.setProperty("Visible", self.getProperty("Visible"))
for view in self.views:
item.addToView(view)
om.addToObjectModel(item, self)
def init(view):
global PointCloudQueue, _pointcloudItem, _pointcloudSource
PointCloudQueue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread())
PointCloudQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
_pointcloudSource = PointCloudSource(view, PointCloudQueue)
_pointcloudSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_pointcloudItem = PointCloudItem(_pointcloudSource)
om.addToObjectModel(_pointcloudItem, sensorsFolder)
def _update_cloud(self, xyz, rgb):
poly_data = vnp.numpyToPolyData(xyz)
vnp.addNumpyToVtk(poly_data, rgb, "rgb")
item = self._parent_folder.findChild(self._name)
if item is not None:
item.setPolyData(poly_data)
else:
view = applogic.getCurrentRenderView()
item = vis.PolyDataItem(self._name, poly_data, view=view)
item.setProperty("Color By", "rgb")
om.addToObjectModel(item, parentObj=self._parent_folder)
item._updateColorByProperty()
def init(view):
global KinectQueue, _kinectItem, _kinectSource
KinectQueue = PythonQt.dd.ddKinectLCM(lcmUtils.getGlobalLCMThread())
KinectQueue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
_kinectSource = KinectSource(view, KinectQueue)
_kinectSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_kinectItem = KinectItem(_kinectSource)
om.addToObjectModel(_kinectItem, sensorsFolder)
def newTerrainItem(self, tform, uid=None, region=None):
if uid is None:
uid = self.getNewUID()
elif uid in self.regions:
return self.regions[uid]
view = app.getCurrentRenderView()
item = TerrainRegionItem(uid, view, tform, self, region)
parentObj = om.getOrCreateContainer('Safe terrain regions')
om.addToObjectModel(item, parentObj)
self.regions[uid] = item
return item
def add_contact_data(data, item_name):
# Exploit the fact that data.append is a vtkAppendPolyData
# instance. The number of input connections on port zero is the
# number of *actual* geometries added. If zero have been added,
# do no work.
if (data is None
or data.append.GetNumberOfInputConnections(0) == 0):
return
item = vis.PolyDataItem(item_name, data.getPolyData(), view)
om.addToObjectModel(item, contact_data_folder)
item.setProperty('Visible', True)
item.setProperty('Alpha', 1.0)
item.colorBy('RGB255')
def loadRobotModel(
name=None,
view=None,
parent="scene",
urdfFile=None,
color=None,
visible=True,
colorMode="URDF Colors",
useConfigFile=True,
robotName="",
):
if isinstance(parent, str):
parent = om.getOrCreateContainer(parent)
if urdfFile:
model = loadRobotModelFromFile(urdfFile)
haveModel = True
else:
model = None
useConfigFile = True # we can't extract joint names from the model
haveModel = False
colorModes = {"Solid Color": 0, "Textures": 1, "URDF Colors": 2}
obj = RobotModelItem(
model=model,
visible=visible,
color=color or getRobotGrayColor(),
colorMode=colorModes[colorMode],
robotName=robotName,
)
om.addToObjectModel(obj, parent)
if name:
obj.setProperty("Name", name)
if view is not None:
obj.addToView(view)
if useConfigFile:
jointNames = None
else:
jointNames = model.getJointNames()
jointController = jointcontrol.JointController([obj],
jointNames=jointNames,
robotName=robotName,
pushToModel=haveModel)
return obj, jointController
def addLink(self, link, robotNum, linkName):
self.robots.setdefault(robotNum, {})[linkName] = link
linkFolder = self.getLinkFolder(robotNum, linkName)
for geom in link.geometry:
geom.polyDataItem.addToView(self.view)
om.addToObjectModel(geom.polyDataItem, parentObj=linkFolder)
if linkName == 'world':
#geom.polyDataItem.actor.SetUseBounds(False)
#geom.polyDataItem.actor.GetProperty().LightingOff()
geom.polyDataItem.actor.GetProperty().SetSpecular(0.0)
else:
geom.polyDataItem.actor.GetProperty().SetSpecular(0.9)
geom.polyDataItem.actor.GetProperty().SetSpecularPower(20)
def init(view):
global PointCloudQueue, _pointcloudItem, _pointcloudSource
PointCloudQueue = PythonQt.dd.ddPointCloudLCM(
lcmUtils.getGlobalLCMThread())
PointCloudQueue.init(lcmUtils.getGlobalLCMThread(),
drcargs.args().config_file)
_pointcloudSource = PointCloudSource(view, PointCloudQueue)
_pointcloudSource.start()
sensorsFolder = om.getOrCreateContainer('sensors')
_pointcloudItem = PointCloudItem(_pointcloudSource)
om.addToObjectModel(_pointcloudItem, sensorsFolder)
def showFrame(frame, name, view=None, parent='segmentation', scale=0.35, visible=True):
view = view or app.getCurrentRenderView()
assert view
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
item = FrameItem(name, frame, view)
om.addToObjectModel(item, parentObj)
item.setProperty('Visible', visible)
item.setProperty('Scale', scale)
return item
def showFrame(frame, name, view=None, parent='segmentation', scale=0.35, visible=True):
view = view or app.getCurrentRenderView()
assert view
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
item = FrameItem(name, frame, view)
om.addToObjectModel(item, parentObj)
item.setProperty('Visible', visible)
item.setProperty('Scale', scale)
return item
def showText(text, name, fontSize=18, position=(10, 10), parent=None, view=None):
view = view or app.getCurrentRenderView()
assert view
item = TextItem(name, text, view=view)
item.setProperty('Font Size', fontSize)
item.setProperty('Position', list(position))
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
om.addToObjectModel(item, parentObj)
return item
def showText(text, name, fontSize=18, position=(10, 10), parent=None, view=None):
view = view or app.getCurrentRenderView()
assert view
item = TextItem(name, text, view=view)
item.setProperty('Font Size', fontSize)
item.setProperty('Position', list(position))
if isinstance(parent, str):
parentObj = om.getOrCreateContainer(parent)
else:
parentObj = parent
om.addToObjectModel(item, parentObj)
return item
def main():
objectTree = QtGui.QTreeWidget()
propertiesPanel = PythonQt.dd.ddPropertiesPanel()
om.init(objectTree, propertiesPanel)
p = om.ObjectModelItem("test parent item")
p.addProperty("foo", 1)
p.addProperty("Visible", True)
om.addToObjectModel(p)
c = om.ObjectModelItem("test child item")
om.addToObjectModel(c, p)
assert om.findObjectByName("test parent item") == p
assert om.findObjectByName("test child item") == c
assert p.children()[0] == c
assert c.children() == []
objectTree.show()
propertiesPanel.show()
objectTree2 = QtGui.QTreeWidget()
propertiesPanel2 = PythonQt.dd.ddPropertiesPanel()
tree = om.ObjectModelTree()
tree.init(objectTree2, propertiesPanel2)
p2 = om.ObjectModelItem("test parent item 2")
p2.addProperty("foo2", 1)
p2.addProperty("Visible", True)
tree.addToObjectModel(p2)
c2 = om.ObjectModelItem("test child item 2")
tree.addToObjectModel(c2, p2)
assert tree.findObjectByName("test parent item 2") == p2
assert tree.findObjectByName("test child item 2") == c2
assert p2.children()[0] == c2
assert c2.children() == []
objectTree2.show()
propertiesPanel2.show()
startApplication(enableQuitTimer=True)
def main():
objectTree = QtGui.QTreeWidget()
propertiesPanel = PythonQt.dd.ddPropertiesPanel()
om.init(objectTree, propertiesPanel)
p = om.ObjectModelItem('test parent item')
p.addProperty('foo', 1)
p.addProperty('Visible', True)
om.addToObjectModel(p)
c = om.ObjectModelItem('test child item')
om.addToObjectModel(c, p)
assert om.findObjectByName('test parent item') == p
assert om.findObjectByName('test child item') == c
assert p.children()[0] == c
assert c.children() == []
objectTree.show()
propertiesPanel.show()
objectTree2 = QtGui.QTreeWidget()
propertiesPanel2 = PythonQt.dd.ddPropertiesPanel()
tree = om.ObjectModelTree()
tree.init(objectTree2, propertiesPanel2)
p2 = om.ObjectModelItem('test parent item 2')
p2.addProperty('foo2', 1)
p2.addProperty('Visible', True)
tree.addToObjectModel(p2)
c2 = om.ObjectModelItem('test child item 2')
tree.addToObjectModel(c2, p2)
assert tree.findObjectByName('test parent item 2') == p2
assert tree.findObjectByName('test child item 2') == c2
assert p2.children()[0] == c2
assert c2.children() == []
objectTree2.show()
propertiesPanel2.show()
startApplication(enableQuitTimer=True)
def __init__(self, view, _PointCloudQueue):
self.view = view
self.PointCloudQueue = _PointCloudQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = PointCloudQueue.getPointCloudFromPointCloud(self.p)
self.polyDataObj = vis.PolyDataItem('pointcloud source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
def __init__(self, view, _KinectQueue):
self.view = view
self.KinectQueue = _KinectQueue
self.visible = True
self.p = vtk.vtkPolyData()
utime = KinectQueue.getPointCloudFromKinect(self.p)
self.polyDataObj = vis.PolyDataItem('kinect source', shallowCopy(self.p), view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.initialized = False
om.addToObjectModel(self.polyDataObj)
self.queue = PythonQt.dd.ddBotImageQueue(lcmUtils.getGlobalLCMThread())
self.queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
self.targetFps = 30
self.timerCallback = TimerCallback(targetFps=self.targetFps)
self.timerCallback.callback = self._updateSource
def addAllObjects(self):
drawObject = self.getDrawObject("COLLECTIONS")
if not drawObject:
return
drawObject.getCollectionsInfo()
for coll in drawObject.collectionInfos:
# If the icon exists, don't re-add it
existing = False
for existingCollection in drawObject.children():
if coll.id == existingCollection.collectionInfo.id:
existing = True
continue
if existing:
continue
actor = vtk.vtkCollections()
obj = CollectionInfoObject(coll, actor)
om.addToObjectModel(obj, drawObject)
obj.addToView(self.view)
def init(view):
global _multisenseItem
global multisenseDriver
m = MultiSenseSource(view)
m.start()
multisenseDriver = m
sensorsFolder = om.getOrCreateContainer('sensors')
_multisenseItem = MultisenseItem(m)
om.addToObjectModel(_multisenseItem, sensorsFolder)
useMapServer = hasattr(drc, 'vtkMapServerSource')
if useMapServer:
mapServerSource = MapServerSource(view, callbackFunc=view.render)
mapsServerContainer = om.ObjectModelItem('Map Server',
icon=om.Icons.Robot)
mapsServerContainer.source = mapServerSource
om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
mapServerSource.start()
else:
mapServerSource = None
spindleDebug = SpindleAxisDebug(multisenseDriver)
spindleDebug.addToView(view)
om.addToObjectModel(spindleDebug, sensorsFolder)
return multisenseDriver, mapServerSource
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 init(view):
global _multisenseItem
global multisenseDriver
m = MultiSenseSource(view)
m.start()
multisenseDriver = m
sensorsFolder = om.getOrCreateContainer('sensors')
_multisenseItem = MultisenseItem(m)
om.addToObjectModel(_multisenseItem, sensorsFolder)
useMapServer = hasattr(drc, 'vtkMapServerSource')
if useMapServer:
mapServerSource = MapServerSource(view, callbackFunc=view.render)
mapsServerContainer = om.ObjectModelItem('Map Server', icon=om.Icons.Robot)
mapsServerContainer.source = mapServerSource
om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
mapServerSource.start()
else:
mapServerSource = None
spindleDebug = SpindleAxisDebug(multisenseDriver)
spindleDebug.addToView(view)
om.addToObjectModel(spindleDebug, sensorsFolder)
return multisenseDriver, mapServerSource
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 setGeometry(self, path, geometry):
folder = self.getPathFolder(path)
ancestors = findPathToAncestor(folder, self.getRootFolder())
geomTransform = vtk.vtkTransform()
for item in reversed(ancestors):
if not hasattr(item, "transform"):
item.transform = vtk.vtkTransform()
item.transform.PostMultiply()
geomTransform.Concatenate(item.transform)
geometryName = folder.getProperty("Name")
item = folder.findChild(geometryName)
if item is None:
item = geometry.createPolyDataItem(name=geometryName)
item.addToView(self.view)
om.addToObjectModel(item, parentObj=folder)
else:
item.setPolyData(geometry.polyData)
geometry.updatePolyDataItemProperties(item)
self.setDefaultColorBy(item)
item.actor.SetUserTransform(geomTransform)
return path
def setGeometry(self, path, geometry):
folder = self.getPathFolder(path)
ancestors = findPathToAncestor(folder, self.getRootFolder())
geomTransform = vtk.vtkTransform()
for item in reversed(ancestors):
if not hasattr(item, "transform"):
item.transform = vtk.vtkTransform()
item.transform.PostMultiply()
geomTransform.Concatenate(item.transform)
geometryName = folder.getProperty("Name")
item = folder.findChild(geometryName)
if item is None:
item = geometry.createPolyDataItem(name=geometryName)
item.addToView(self.view)
om.addToObjectModel(item, parentObj=folder)
else:
item.setPolyData(geometry.polyData)
geometry.updatePolyDataItemProperties(item)
self.setDefaultColorBy(item)
item.actor.SetUserTransform(geomTransform)
return path
def init(view, robotStateJointController):
global _multisenseItem
sensorsFolder = om.getOrCreateContainer(
"sensors", om.getOrCreateContainer(robotStateJointController.robotName)
)
config = drcargs.getRobotConfig(robotStateJointController.robotName)[
"perceptionSources"
]
validSourceTypes = ["gridMap", "depthImagePointCloud", "pointCloud"]
perceptionSources = {}
for sourceType in config:
if sourceType not in validSourceTypes:
raise ValueError(
"Source type {} is not a recognised perception source. Valid types are {}. Check your"
" director configuration.".format(sourceType, validSourceTypes)
)
# TODO might be nice to avoid the if statement in the loop by having a function to create each source
for sourceConfig in config[sourceType]:
if sourceType == "gridMap":
source = RosGridMap(
robotStateJointController,
sourceConfig["name"],
callbackFunc=view.render,
)
source.addToView(view)
om.addToObjectModel(source, sensorsFolder)
if sourceType == "depthImagePointCloud":
source = DepthImagePointCloudSource(
sourceConfig["name"],
sourceConfig["sensor"],
None,
robotStateJointController,
)
source.addToView(view)
om.addToObjectModel(source, sensorsFolder)
if sourceType == "pointCloud":
source = PointCloudSource(
sourceConfig["name"],
robotStateJointController,
callbackFunc=view.render,
)
source.addToView(view)
om.addToObjectModel(source, sensorsFolder)
if "properties" in sourceConfig:
for prop, value in sourceConfig["properties"].items():
source.setProperty(prop, value)
perceptionSources[sourceConfig["robotSystemKey"]] = source
return perceptionSources
def init(view):
global _multisenseItem
global multisenseDriver
sensorsFolder = om.getOrCreateContainer('sensors')
m = MultiSenseSource(view)
m.start()
multisenseDriver = m
_multisenseItem = MultisenseItem(m)
om.addToObjectModel(_multisenseItem, sensorsFolder)
queue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread())
queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
lidarNames = queue.getLidarNames()
for lidar in lidarNames:
if queue.displayLidar(lidar):
l = LidarSource(view, queue.getLidarChannelName(lidar),
queue.getLidarCoordinateFrame(lidar),
queue.getLidarFriendlyName(lidar),
queue.getLidarIntensity(lidar))
l.start()
lidarDriver = l
_lidarItem = LidarItem(l)
om.addToObjectModel(_lidarItem, sensorsFolder)
useMapServer = hasattr(drc, 'vtkMapServerSource')
if useMapServer:
mapServerSource = MapServerSource(view, callbackFunc=view.render)
mapsServerContainer = om.ObjectModelItem('Map Server',
icon=om.Icons.Robot)
mapsServerContainer.source = mapServerSource
om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
mapServerSource.start()
else:
mapServerSource = None
spindleDebug = SpindleAxisDebug(multisenseDriver)
spindleDebug.addToView(view)
om.addToObjectModel(spindleDebug, sensorsFolder)
return multisenseDriver, mapServerSource
def init(view):
global _multisenseItem
global multisenseDriver
sensorsFolder = om.getOrCreateContainer('sensors')
m = MultiSenseSource(view)
m.start()
multisenseDriver = m
_multisenseItem = MultisenseItem(m)
om.addToObjectModel(_multisenseItem, sensorsFolder)
queue = PythonQt.dd.ddPointCloudLCM(lcmUtils.getGlobalLCMThread())
queue.init(lcmUtils.getGlobalLCMThread(), drcargs.args().config_file)
lidarNames = queue.getLidarNames()
for lidar in lidarNames:
if queue.displayLidar(lidar):
l = LidarSource(view, queue.getLidarChannelName(lidar), queue.getLidarCoordinateFrame(lidar), queue.getLidarFriendlyName(lidar), queue.getLidarIntensity(lidar))
l.start()
lidarDriver = l
_lidarItem = LidarItem(l)
om.addToObjectModel(_lidarItem, sensorsFolder)
useMapServer = hasattr(drc, 'vtkMapServerSource')
if useMapServer:
mapServerSource = MapServerSource(view, callbackFunc=view.render)
mapsServerContainer = om.ObjectModelItem('Map Server', icon=om.Icons.Robot)
mapsServerContainer.source = mapServerSource
om.addToObjectModel(mapsServerContainer, parentObj=sensorsFolder)
mapServerSource.start()
else:
mapServerSource = None
spindleDebug = SpindleAxisDebug(multisenseDriver)
spindleDebug.addToView(view)
om.addToObjectModel(spindleDebug, sensorsFolder)
return multisenseDriver, mapServerSource
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 addDrawObject(self, name, msgBytes):
actor = vtk.vtkLCMGLProp()
obj = LCMGLObject(name, actor)
om.addToObjectModel(obj, om.getOrCreateContainer('LCM GL'))
obj.addToView(self.view)
return obj
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# Though strangely named, DebugData() is the object through which
# drawing is done in DrakeVisualizer.
d = DebugData()
# Set the color map.
color_map = self.create_color_map()
# The scale value attributable to auto-scale.
auto_force_scale = 1.0
auto_moment_scale = 1.0
auto_traction_scale = 1.0
auto_slip_velocity_scale = 1.0
max_force = -1
max_moment = -1
max_traction = -1
max_slip = -1
# TODO(sean-curtis-TRI) Remove the following comment when this
# code can be exercised.
# The following code is not exercised presently because the
# magnitude mode is always set to kFixedLength.
# Determine scaling magnitudes if autoscaling is activated.
if self.magnitude_mode == ContactVisModes.kAutoScale:
if self.show_spatial_force:
for surface in msg.hydroelastic_contacts:
force = np.array([
surface.force_C_W[0], surface.force_C_W[1],
surface.force_C_W[2]
])
moment = np.array([
surface.moment_C_W[0], surface.moment_C_W[1],
surface.moment_C_W[2]
])
force_mag = np.linalg.norm(force)
moment_mag = np.linalg.norm(moment)
if force_mag > max_force:
max_force = force_mag
if moment_mag > max_moment:
max_moment = moment_mag
# Prepare scaling information for the traction vectors.
if self.show_traction_vectors:
for quad_point_data in surface.quadrature_point_data:
traction = np.array([
quad_point_data.traction_Aq_W[0],
quad_point_data.traction_Aq_W[1],
quad_point_data.traction_Aq_W[2]
])
max_traction = max(max_traction, np.linalg.norm(traction))
# Prepare scaling information for the slip velocity vectors.
if self.show_slip_velocity_vectors:
for quad_point_data in surface.quadrature_point_data:
slip_speed = np.array([
quad_point_data.vt_BqAq_W[0],
quad_point_data.vt_BqAq_W[1],
quad_point_data.vt_BqAq_W[2]
])
max_slip_speed = max(max_slip_speed,
np.linalg.norm(slip_speed))
# Compute scaling factors.
auto_force_scale = 1.0 / max_force
auto_moment_scale = 1.0 / max_moment
auto_traction_scale = 1.0 / max_traction
auto_slip_velocity_scale = 1.0 / max_slip_speed
# TODO(drum) Consider exiting early if no visualization options are
# enabled.
for surface in msg.hydroelastic_contacts:
# Draw the spatial force.
if self.show_spatial_force:
point = np.array([
surface.centroid_W[0], surface.centroid_W[1],
surface.centroid_W[2]
])
force = np.array([
surface.force_C_W[0], surface.force_C_W[1],
surface.force_C_W[2]
])
moment = np.array([
surface.moment_C_W[0], surface.moment_C_W[1],
surface.moment_C_W[2]
])
force_mag = np.linalg.norm(force)
moment_mag = np.linalg.norm(moment)
# Draw the force arrow if it's of sufficient magnitude.
if force_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode == ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this force would be
# skipped.
scale /= force_mag
d.addArrow(start=point,
end=point + auto_force_scale * force * scale,
tubeRadius=0.005,
headRadius=0.01,
color=[1, 0, 0])
# Draw the moment arrow if it's of sufficient magnitude.
if moment_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode == ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this moment would be
# skipped.
scale /= moment_mag
d.addArrow(start=point,
end=point + auto_moment_scale * moment * scale,
tubeRadius=0.005,
headRadius=0.01,
color=[0, 0, 1])
# Iterate over all quadrature points, drawing traction and slip
# velocity vectors.
if self.show_traction_vectors or self.show_slip_velocity_vectors:
for quad_point_data in surface.quadrature_point_data:
origin = np.array([
quad_point_data.p_WQ[0], quad_point_data.p_WQ[1],
quad_point_data.p_WQ[2]
])
if self.show_traction_vectors:
traction = np.array([
quad_point_data.traction_Aq_W[0],
quad_point_data.traction_Aq_W[1],
quad_point_data.traction_Aq_W[2]
])
traction_mag = np.linalg.norm(traction)
# Draw the arrow only if it's of sufficient magnitude.
if traction_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode ==\
ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this traction
# would be skipped.
scale /= traction_mag
offset = auto_traction_scale * traction * scale
d.addArrow(start=origin,
end=origin + offset,
tubeRadius=0.000125,
headRadius=0.00025,
color=[1, 0, 1])
else:
d.addSphere(center=origin,
radius=0.000125,
color=[1, 0, 1])
if self.show_slip_velocity_vectors:
slip = np.array([
quad_point_data.vt_BqAq_W[0],
quad_point_data.vt_BqAq_W[1],
quad_point_data.vt_BqAq_W[2]
])
slip_mag = np.linalg.norm(slip)
# Draw the arrow only if it's of sufficient magnitude.
if slip_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode ==\
ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this slip
# vector would be skipped.
scale /= slip_mag
offset = auto_slip_velocity_scale * slip * scale
d.addArrow(start=origin,
end=origin + offset,
tubeRadius=0.000125,
headRadius=0.00025,
color=[0, 1, 1])
else:
d.addSphere(center=origin,
radius=0.000125,
color=[0, 1, 1])
# Iterate over all triangles.
for tri in surface.triangles:
va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]])
vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]])
vc = np.array([tri.p_WC[0], tri.p_WC[1], tri.p_WC[2]])
# Save the maximum pressure.
self.max_pressure_observed = max(self.max_pressure_observed,
tri.pressure_A)
self.max_pressure_observed = max(self.max_pressure_observed,
tri.pressure_B)
self.max_pressure_observed = max(self.max_pressure_observed,
tri.pressure_C)
# TODO(drum) Vertex color interpolation may be insufficiently
# granular if a single triangle spans too large a range of
# pressures. Suggested solution is to use a texture map.
# Get the colors at the vertices.
color_a = color_map.get_color(tri.pressure_A)
color_b = color_map.get_color(tri.pressure_B)
color_c = color_map.get_color(tri.pressure_C)
if self.show_pressure:
# TODO(drum) Use a better method for this; the current
# approach is susceptible to z-fighting under certain
# zoom levels.
# Compute a normal to the triangle. We need this normal
# because the visualized pressure surface can be coplanar
# with parts of the visualized geometry, in which case a
# dithering type effect would appear. So we use the normal
# to draw two triangles slightly offset to both sides of
# the contact surface.
# Note that if the area of this triangle is very small, we
# won't waste time visualizing it, which also means that
# won't have to worry about degenerate triangles).
# TODO(edrumwri) Consider allowing the user to set these
# next two values programmatically.
min_area = 1e-8
offset_scalar = 1e-4
normal = np.cross(vb - va, vc - vb)
norm_normal = np.linalg.norm(normal)
if norm_normal >= min_area:
unit_normal = normal / np.linalg.norm(normal)
offset = unit_normal * offset_scalar
d.addPolygon([va + offset, vb + offset, vc + offset],
color=[color_a, color_b, color_c])
d.addPolygon([va - offset, vb - offset, vc - offset],
color=[color_a, color_b, color_c])
# TODO(drum) Consider drawing shared edges just once.
if self.show_contact_edges:
contrasting_color = color_map.get_contrasting_color()
d.addPolyLine(points=(va, vb, vc),
isClosed=True,
color=contrasting_color)
item_name = '{}, {}'.format(surface.body1_name, surface.body2_name)
cls = vis.PolyDataItem
view = applogic.getCurrentRenderView()
item = cls(item_name, d.getPolyData(), view)
om.addToObjectModel(item, folder)
item.setProperty('Visible', True)
item.setProperty('Alpha', 1.0)
# Conditional necessary to keep DrakeVisualizer from spewing
# messages to the console when the contact surface is empty.
if len(msg.hydroelastic_contacts) > 0:
item.colorBy('RGB255')
def addDrawObject(self, name, msgBytes):
actor = vtk.vtkOctomap()
obj = OctomapObject(name, actor)
om.addToObjectModel(obj, om.getOrCreateContainer('Octomap'))
obj.addToView(self.view)
return obj
colorize.init()
cameraview.cameraView.initImageRotations(robotStateModel)
cameraview.cameraView.rayCallback = segmentation.extractPointsAlongClickRay
if useMultisense:
multisensepanel.init(perception.multisenseDriver)
else:
app.removeToolbarMacro('ActionMultisensePanel')
sensordatarequestpanel.init()
# for kintinuous, use 'CAMERA_FUSED', 'CAMERA_TSDF'
disparityPointCloud = segmentation.DisparityPointCloudItem('stereo point cloud', 'CAMERA', 'CAMERA_LEFT', cameraview.imageManager)
disparityPointCloud.addToView(view)
om.addToObjectModel(disparityPointCloud, parentObj=om.findObjectByName('sensors'))
def createPointerTracker():
return trackers.PointerTracker(robotStateModel, disparityPointCloud)
if useOpenniDepthImage:
openniDepthPointCloud = segmentation.DisparityPointCloudItem('openni point cloud', 'OPENNI_FRAME', 'OPENNI_FRAME_LEFT', cameraview.imageManager)
openniDepthPointCloud.addToView(view)
om.addToObjectModel(openniDepthPointCloud, parentObj=om.findObjectByName('sensors'))
if useGrid:
grid = vis.showGrid(view, color=[0,0,0], alpha=0.1)
grid.setProperty('Surface Mode', 'Surface with edges')
def openOtdf(filename, view):
model = OtdfModelItem(filename)
om.addToObjectModel(model)
model.addToView(view)
return model
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")
