def _onPropertyChanged(self, propertySet, propertyName):
om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Visible':
self.actor.SetVisibility(self.getProperty(propertyName))
self._renderAllViews()
elif propertyName == 'Text':
view = app.getCurrentRenderView()
self.actor.SetInput(self.getProperty(propertyName))
elif propertyName == 'Position':
pos = self.getProperty(propertyName)
self.actor.SetPosition(pos[0], pos[1])
elif propertyName == 'Font Size':
self.actor.GetTextProperty().SetFontSize(
self.getProperty(propertyName))
elif propertyName == 'Bold Size':
self.actor.GetTextProperty().SetBold(
self.getProperty(propertyName))
elif propertyName == 'Italic':
self.actor.GetTextProperty().SetItalic(
self.getProperty(propertyName))
if self.getProperty('Visible'):
self._renderAllViews()
def _onPropertyChanged(self, propertySet, propertyName):
om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Visible":
self.actor.SetVisibility(self.getProperty(propertyName))
self._renderAllViews()
elif propertyName == "Text":
view = app.getCurrentRenderView()
self.actor.SetInput(self.getProperty(propertyName))
elif propertyName == "Position":
pos = self.getProperty(propertyName)
self.actor.SetPosition(pos[0], pos[1])
elif propertyName == "Font Size":
self.actor.GetTextProperty().SetFontSize(
self.getProperty(propertyName))
elif propertyName == "Bold Size":
self.actor.GetTextProperty().SetBold(
self.getProperty(propertyName))
elif propertyName == "Italic":
self.actor.GetTextProperty().SetItalic(
self.getProperty(propertyName))
if self.getProperty("Visible"):
self._renderAllViews()
def create(view=None, globalsDict=None, options=None, planningOnly=False):
'''
Convenience function for initializing a robotSystem
with the default options and populating a globals()
dictionary with all the constructed objects.
'''
from director import applogic
view = view or applogic.getCurrentRenderView()
factory = ComponentFactory()
factory.register(RobotSystemFactory)
options = options or factory.getDefaultOptions()
if planningOnly:
options = factory.getDisabledOptions()
factory.setDependentOptions(options, usePlannerPublisher=True, useTeleop=True)
robotSystem = factory.construct(options, view=view)
if globalsDict is not None:
globalsDict.update(dict(robotSystem))
return robotSystem
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 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 __init__(self):
#Loading
filename = os.path.join(os.environ['SPARTAN_SOURCE_DIR'],
'apps/chris/ddGroundTruthAnnotationPanel.ui')
loader = QtUiTools.QUiLoader()
uifile = QtCore.QFile(filename)
assert uifile.open(uifile.ReadOnly)
self.widget = loader.load(uifile)
uifile.close()
#UI elements
self.ui = WidgetDict(self.widget.children())
self.view = app.getCurrentRenderView()
self.ui.enabledCheck.connect('toggled(bool)', self.onEnabledCheckBox)
self.ui.clearButton.connect('clicked()', self.onClear)
self.ui.saveScene.connect('clicked()', self.saveCurrentScene)
self.ui.align.connect('clicked()', self.vtkICP)
self.eventFilter = MyEventFilter(view, self)
self.annotation = vis.PolyDataItem('annotation',
self.makeSphere((0, 0, 0)), view)
self.annotation.setProperty('Color', [0, 1, 0])
self.annotation.actor.SetPickable(False)
self.annotation.actor.SetUserTransform(vtk.vtkTransform())
self.setEnabled(False)
#State
self.pickPoints = []
self.objects = []
self.isModifyingBoundingBox = False
#init folders
self.getRootFolder()
self.getModelObjectsFolder()
self.getTransformedObjectsFolder()
def _onPropertyChanged(self, propertySet, propertyName):
PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Scale':
scale = self.getProperty(propertyName)
self.rep.SetWorldSize(scale)
self._updateAxesGeometry()
elif propertyName == 'Edit':
view = app.getCurrentRenderView()
if view not in self.views:
view = self.views[0]
self.widget.SetInteractor(view.renderWindow().GetInteractor())
self.widget.SetEnabled(self.getProperty(propertyName))
isEditing = self.getProperty(propertyName)
if isEditing:
frameupdater.registerFrame(self)
elif propertyName == 'Trace':
trace = self.getProperty(propertyName)
if trace and not self.traceData:
self.traceData = FrameTraceVisualizer(self)
elif not trace and self.traceData:
om.removeFromObjectModel(self.traceData.getTraceData())
self.traceData = None
elif propertyName == 'Tube':
self._updateAxesGeometry()
def _onPropertyChanged(self, propertySet, propertyName):
PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Scale':
scale = self.getProperty(propertyName)
self.rep.SetWorldSize(scale)
self._updateAxesGeometry()
elif propertyName == 'Edit':
view = app.getCurrentRenderView()
if view not in self.views:
view = self.views[0]
self.widget.SetInteractor(view.renderWindow().GetInteractor())
self.widget.SetEnabled(self.getProperty(propertyName))
isEditing = self.getProperty(propertyName)
if isEditing:
frameupdater.registerFrame(self)
elif propertyName == 'Trace':
trace = self.getProperty(propertyName)
if trace and not self.traceData:
self.traceData = FrameTraceVisualizer(self)
elif not trace and self.traceData:
om.removeFromObjectModel(self.traceData.getTraceData())
self.traceData = None
elif propertyName == 'Tube':
self.properties.setPropertyAttribute('Tube Width', 'hidden', not self.getProperty(propertyName))
self._updateAxesGeometry()
def create(view=None,
globalsDict=None,
options=None,
planningOnly=False,
**kwargs):
"""
Convenience function for initializing a robotSystem
with the default options and populating a globals()
dictionary with all the constructed objects.
"""
from director import applogic
view = view or applogic.getCurrentRenderView()
factory = ComponentFactory()
factory.register(RobotSystemFactory)
options = options or factory.getDefaultOptions()
if planningOnly:
options = factory.getDisabledOptions()
factory.setDependentOptions(options,
usePlannerPublisher=True,
useTeleop=True)
robotSystem = factory.construct(options, view=view, **kwargs)
if globalsDict is not None:
globalsDict.update(dict(robotSystem))
return robotSystem
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 onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal,
'walking goal',
parent='planning',
scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1])
polyData = segmentation.thresholdPoints(
polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(
vnp.getNumpyFromVtk(polyData, 'Points')[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(),
'walking goal terrain handle',
parent=frameObj,
visible=True,
color=[1, 1, 0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle,
terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def pickPoint(displayPoint,
view,
obj=None,
pickType='points',
tolerance=0.01,
returnNormal=False):
assert pickType in ('points', 'cells', 'render')
view = view or app.getCurrentRenderView()
assert view
if isinstance(obj, str):
obj = om.findObjectByName(obj)
assert obj
if pickType == 'render':
picker = vtk.vtkPropPicker()
else:
picker = vtk.vtkPointPicker(
) if pickType == 'points' else vtk.vtkCellPicker()
picker.SetTolerance(tolerance)
if obj:
if isinstance(obj, list):
for o in obj:
picker.AddPickList(o.actor)
obj = None
else:
picker.AddPickList(obj.actor)
picker.PickFromListOn()
picker.Pick(displayPoint[0], displayPoint[1], 0, view.renderer())
pickedProp = picker.GetViewProp()
pickedPoint = np.array(picker.GetPickPosition())
pickedDataset = pickedProp.GetMapper().GetInput() if isinstance(
pickedProp, vtk.vtkActor) else None
pickedNormal = np.zeros(3)
if returnNormal:
if pickType == 'cells':
pickedNormal = np.array(picker.GetPickNormal())
elif pickType == 'points' and pickedDataset:
pointId = picker.GetPointId()
normals = pickedDataset.GetPointData().GetNormals()
if normals:
pickedNormal = np.array(normals.GetTuple3(pointId))
if obj:
if returnNormal:
return (pickedPoint, pickedNormal) if pickedProp else (None, None)
else:
return pickedPoint if pickedProp else None
else:
return (pickedPoint, pickedProp, pickedDataset,
pickedNormal) if returnNormal else (pickedPoint, pickedProp,
pickedDataset)
def pickPoint(displayPoint, view, obj=None, pickType='points', tolerance=0.01, returnNormal=False):
assert pickType in ('points', 'cells', 'render')
view = view or app.getCurrentRenderView()
assert view
if isinstance(obj, str):
obj = om.findObjectByName(obj)
assert obj
if pickType == 'render':
picker = vtk.vtkPropPicker()
else:
picker = vtk.vtkPointPicker() if pickType == 'points' else vtk.vtkCellPicker()
picker.SetTolerance(tolerance)
if obj is not None:
if isinstance(obj, list):
for o in obj:
picker.AddPickList(o.actor)
obj = None
else:
picker.AddPickList(obj.actor)
picker.PickFromListOn()
picker.Pick(displayPoint[0], displayPoint[1], 0, view.renderer())
pickedProp = picker.GetViewProp()
pickedPoint = np.array(picker.GetPickPosition())
pickedDataset = pickedProp.GetMapper().GetInput() if isinstance(pickedProp, vtk.vtkActor) else None
pickedNormal = np.zeros(3)
if returnNormal:
if pickType == 'cells':
pickedNormal = np.array(picker.GetPickNormal())
elif pickType == 'points' and pickedDataset:
pointId = picker.GetPointId()
normals = pickedDataset.GetPointData().GetNormals()
if normals:
pickedNormal = np.array(normals.GetTuple3(pointId))
#if pickedDataset and pickType == 'cells':
# print 'point id:', pickedDataset.GetCell(picker.GetCellId()).GetPointIds().GetId(picker.GetSubId())
#if pickType == 'points':
# print 'point id:', picker.GetPointId()
if obj:
if returnNormal:
return (pickedPoint, pickedNormal) if pickedProp else (None, None)
else:
return pickedPoint if pickedProp else None
else:
return (pickedPoint, pickedProp, pickedDataset, pickedNormal) if returnNormal else (pickedPoint, pickedProp, pickedDataset)
def update_screen_text(self):
folder = om.getOrCreateContainer(self._folder_name)
my_text = 'Point contact vector: {}'.format(
ContactVisModes.get_mode_string(self.magnitude_mode))
# TODO(SeanCurtis-TRI): Figure out how to anchor this in the bottom-
# right corner as opposed to floating in the middle.
w = applogic.getCurrentRenderView().size.width()
vis.updateText(my_text, 'contact_text',
**{'position': (w/2, 10), 'parent': folder})
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 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 _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 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 onNewWalkingGoal(self, walkingGoal=None):
walkingGoal = walkingGoal or self.newWalkingGoalFrame(self.robotModel)
frameObj = vis.updateFrame(walkingGoal, 'walking goal', parent='planning', scale=0.25)
frameObj.setProperty('Edit', True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
if self.placer:
self.placer.stop()
terrain = om.findObjectByName('HEIGHT_MAP_SCENE')
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(polyData, pos, pos+[0,0,1])
polyData = segmentation.thresholdPoints(polyData, 'distance_to_line', [0.0, 0.1])
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, 'Points')[:,2])
frameObj.transform.Translate(pos - np.array(frameObj.transform.GetPosition()))
d = DebugData()
d.addSphere((0,0,0), radius=0.03)
handle = vis.showPolyData(d.getPolyData(), 'walking goal terrain handle', parent=frameObj, visible=True, color=[1,1,0])
handle.actor.SetUserTransform(frameObj.transform)
self.placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == 'Edit':
if propertySet.getProperty(propertyName):
self.placer.start()
else:
self.placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, 'Edit')
frameObj.connectFrameModified(self.onWalkingGoalModified)
self.onWalkingGoalModified(frameObj)
def create(view=None, globalsDict=None, options=None):
'''
Convenience function for initializing a robotSystem
with the default options and populating a globals()
dictionary with all the constructed objects.
'''
from director import applogic
view = view or applogic.getCurrentRenderView()
factory = RobotSystemFactory()
options = options or factory.getDefaultOptions()
robotSystem = factory.construct(options, view=view)
if globalsDict is not None:
globalsDict.update(dict(robotSystem))
return robotSystem
def create(view=None, globalsDict=None, options=None):
'''
Convenience function for initializing a robotSystem
with the default options and populating a globals()
dictionary with all the constructed objects.
'''
from director import applogic
view = view or applogic.getCurrentRenderView()
factory = RobotSystemFactory()
options = options or factory.getDefaultOptions()
robotSystem = factory.construct(options, view=view)
if globalsDict is not None:
globalsDict.update(dict(robotSystem))
return robotSystem
def showCaptionWidget(position, text, view=None):
view = view or app.getCurrentRenderView()
assert view
global captionWidget
if not captionWidget:
rep = vtk.vtkCaptionRepresentation()
rep.SetPosition(0.2, 0.8)
w = vtk.vtkCaptionWidget()
w.SetInteractor(view.renderWindow().GetInteractor())
w.SetRepresentation(rep)
w.On()
captionWidget = w
rep = captionWidget.GetRepresentation()
rep.SetAnchorPosition(position)
rep.GetCaptionActor2D().SetCaption(text)
a = rep.GetCaptionActor2D()
pr = a.GetTextActor().GetTextProperty()
pr.SetJustificationToCentered()
pr.SetVerticalJustificationToCentered()
pr.SetItalic(0)
pr.SetBold(0)
pr.SetShadow(0)
pr.SetFontFamilyToArial()
c2 = rep.GetPosition2Coordinate()
c2.SetCoordinateSystemToDisplay()
c2.SetValue(12*len(text),30)
# disable border
#rep.SetShowBorder(0)
a.SetThreeDimensionalLeader(0)
a.SetLeaderGlyphSize(0.005)
captionWidget.On()
captionWidget.Render()
def showCaptionWidget(position, text, view=None):
view = view or app.getCurrentRenderView()
assert view
global captionWidget
if not captionWidget:
rep = vtk.vtkCaptionRepresentation()
rep.SetPosition(0.2, 0.8)
w = vtk.vtkCaptionWidget()
w.SetInteractor(view.renderWindow().GetInteractor())
w.SetRepresentation(rep)
w.On()
captionWidget = w
rep = captionWidget.GetRepresentation()
rep.SetAnchorPosition(position)
rep.GetCaptionActor2D().SetCaption(text)
a = rep.GetCaptionActor2D()
pr = a.GetTextActor().GetTextProperty()
pr.SetJustificationToCentered()
pr.SetVerticalJustificationToCentered()
pr.SetItalic(0)
pr.SetBold(0)
pr.SetShadow(0)
pr.SetFontFamilyToArial()
c2 = rep.GetPosition2Coordinate()
c2.SetCoordinateSystemToDisplay()
c2.SetValue(12 * len(text), 30)
# disable border
#rep.SetShowBorder(0)
a.SetThreeDimensionalLeader(0)
a.SetLeaderGlyphSize(0.005)
captionWidget.On()
captionWidget.Render()
def _onPropertyChanged(self, propertySet, propertyName):
om.ObjectModelItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == 'Visible':
self.actor.SetVisibility(self.getProperty(propertyName))
self._renderAllViews()
elif propertyName == 'Text':
view = app.getCurrentRenderView()
self.actor.SetInput(self.getProperty(propertyName))
elif propertyName == 'Position':
pos = self.getProperty(propertyName)
self.actor.SetPosition(pos[0], pos[1])
elif propertyName == 'Font Size':
self.actor.GetTextProperty().SetFontSize(self.getProperty(propertyName))
elif propertyName == 'Bold Size':
self.actor.GetTextProperty().SetBold(self.getProperty(propertyName))
elif propertyName == 'Italic':
self.actor.GetTextProperty().SetItalic(self.getProperty(propertyName))
if self.getProperty('Visible'):
self._renderAllViews()
def _onPropertyChanged(self, propertySet, propertyName):
PolyDataItem._onPropertyChanged(self, propertySet, propertyName)
if propertyName == "Scale":
scale = self.getProperty(propertyName)
self.rep.SetWorldSize(scale)
self._updateAxesGeometry()
elif propertyName == "Visible":
pass
if self.widget.GetInteractor():
# Need to add this to show or hide the editing handles for modifying the frame along with the frame
# display. Without it the editing handles will remain visible when the lines representing the frame
# are not
self.widget.SetEnabled(self.getProperty(propertyName))
elif propertyName == "Edit":
view = app.getCurrentRenderView()
if view not in self.views:
view = self.views[0]
self.widget.SetInteractor(view.renderWindow().GetInteractor())
self.widget.SetEnabled(self.getProperty(propertyName))
isEditing = self.getProperty(propertyName)
if isEditing:
frameupdater.registerFrame(self)
elif propertyName == "Trace":
trace = self.getProperty(propertyName)
if trace and not self.traceData:
self.traceData = FrameTraceVisualizer(self)
elif not trace and self.traceData:
om.removeFromObjectModel(self.traceData.getTraceData())
self.traceData = None
elif propertyName == "Tube":
self.properties.setPropertyAttribute(
"Tube Width", "hidden", not self.getProperty(propertyName))
self._updateAxesGeometry()
elif propertyName == "Tube Width":
self._updateAxesGeometry()
def startButton():
view = app.getCurrentRenderView()
init(view)
_kinectSource.start()
def startButton():
view = app.getCurrentRenderView()
init(view)
_pointcloudSource.start()
def startButton():
view = app.getCurrentRenderView()
init(view)
_pointcloudSource.start()
def init(self, view=None, globalsDict=None):
view = view or applogic.getCurrentRenderView()
useRobotState = True
usePerception = True
useFootsteps = True
useHands = True
usePlanning = True
useAtlasDriver = True
useAtlasConvexHull = False
useWidgets = False
directorConfig = 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 newWalkingGoal(self, displayPoint, view):
# put walking goal at robot's base
mainLink = drcargs.getRobotConfig(self.robotName)["pelvisLink"]
footFrame = self.robotModel.getLinkFrame(mainLink)
if not footFrame:
print(
"ERROR: The link '{}' provided for the key 'pelvisLink' in the configuration file does not exist in "
"the robot's URDF. Cannot place walking goal.".format(mainLink)
)
return
worldPt1, worldPt2 = vis.getRayFromDisplayPoint(view, displayPoint)
groundOrigin = footFrame.GetPosition()
groundNormal = [0.0, 0.0, 1.0]
selectedGroundPoint = [0.0, 0.0, 0.0]
t = vtk.mutable(0.0)
vtk.vtkPlane.IntersectWithLine(
worldPt1, worldPt2, groundNormal, groundOrigin, t, selectedGroundPoint
)
walkingTarget = transformUtils.frameFromPositionAndRPY(
selectedGroundPoint, np.array(footFrame.GetOrientation())
)
frameObj = vis.updateFrame(
walkingTarget,
self.robotName + " walking goal",
parent="planning",
scale=0.25,
)
frameObj.setProperty("Edit", True)
rep = frameObj.widget.GetRepresentation()
rep.SetTranslateAxisEnabled(2, False)
rep.SetRotateAxisEnabled(0, False)
rep.SetRotateAxisEnabled(1, False)
frameObj.widget.HandleRotationEnabledOff()
terrain = om.findObjectByName("HEIGHT_MAP_SCENE")
if terrain:
pos = np.array(frameObj.transform.GetPosition())
polyData = filterUtils.removeNonFinitePoints(terrain.polyData)
if polyData.GetNumberOfPoints():
polyData = segmentation.labelDistanceToLine(
polyData, pos, pos + [0, 0, 1]
)
polyData = segmentation.thresholdPoints(
polyData, "distance_to_line", [0.0, 0.1]
)
if polyData.GetNumberOfPoints():
pos[2] = np.nanmax(vnp.getNumpyFromVtk(polyData, "Points")[:, 2])
frameObj.transform.Translate(
pos - np.array(frameObj.transform.GetPosition())
)
d = DebugData()
d.addSphere((0, 0, 0), radius=0.03)
handle = vis.showPolyData(
d.getPolyData(),
"walking goal terrain handle " + self.robotName,
parent=frameObj,
visible=True,
color=[1, 1, 0],
)
handle.actor.SetUserTransform(frameObj.transform)
placer = PlacerWidget(app.getCurrentRenderView(), handle, terrain)
def onFramePropertyModified(propertySet, propertyName):
if propertyName == "Edit":
if propertySet.getProperty(propertyName):
placer.start()
else:
placer.stop()
frameObj.properties.connectPropertyChanged(onFramePropertyModified)
onFramePropertyModified(frameObj, "Edit")
frameObj.connectFrameModified(self.onWalkingGoalModified)
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)
# 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_speed = -1
# Determine scaling magnitudes if autoscaling is activated.
if self.magnitude_mode == ContactVisModes.kAutoScale:
for surface in msg.hydroelastic_contacts:
if self.show_spatial_force:
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. We don't want division by zero.
# We don't want division by negative numbers.
if max_force > 0:
auto_force_scale = 1.0 / max_force
if max_moment > 0:
auto_moment_scale = 1.0 / max_moment
if max_traction > 0:
auto_traction_scale = 1.0 / max_traction
if max_slip_speed > 0:
auto_slip_velocity_scale = 1.0 / max_slip_speed
# TODO(drum) Consider exiting early if no visualization options are
# enabled.
view = applogic.getCurrentRenderView()
for surface in msg.hydroelastic_contacts:
contact_data_folder = om.getOrCreateContainer(
f'Contact data between {surface.body1_name} and '
f'{surface.body2_name}', folder)
# Adds a collection of debug data to the console with the given
# item name.
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')
# Draw the spatial force.
if self.show_spatial_force:
force_data = DebugData()
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
force_data.addArrow(start=point,
end=point +
auto_force_scale * force * scale,
tubeRadius=0.001,
headRadius=0.002,
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
force_data.addArrow(start=point,
end=point +
auto_moment_scale * moment * scale,
tubeRadius=0.001,
headRadius=0.002,
color=[0, 0, 1])
add_contact_data(force_data, "Spatial force")
# Iterate over all quadrature points, drawing traction and slip
# velocity vectors.
if self.show_traction_vectors or self.show_slip_velocity_vectors:
traction_data = DebugData()
slip_data = DebugData()
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
traction_data.addArrow(start=origin,
end=origin + offset,
tubeRadius=0.000125,
headRadius=0.00025,
color=[1, 0, 1])
else:
traction_data.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
slip_data.addArrow(start=origin,
end=origin + offset,
tubeRadius=0.000125,
headRadius=0.00025,
color=[0, 1, 1])
else:
slip_data.addSphere(center=origin,
radius=0.000125,
color=[0, 1, 1])
add_contact_data(traction_data, "Traction")
add_contact_data(slip_data, "Slip velocity")
if self.show_pressure or self.show_contact_edges:
pos, uvs, tri_mesh, seg_mesh = \
self.process_triangles(surface)
if self.show_pressure and len(tri_mesh) > 0:
# Copy data to VTK objects.
vtk_uvs = vnp.getVtkFromNumpy(uvs)
vtk_tris = vtk.vtkCellArray()
vtk_tris.Allocate(len(tri_mesh))
for tri in tri_mesh:
vtk_tris.InsertNextCell(3, tri)
vtk_polydata_tris = vtk.vtkPolyData()
vtk_polydata_tris.SetPoints(vnp.getVtkPointsFromNumpy(pos))
vtk_polydata_tris.SetPolys(vtk_tris)
vtk_polydata_tris.GetPointData().SetTCoords(vtk_uvs)
vtk_mapper = vtk.vtkPolyDataMapper()
vtk_mapper.SetInputData(vtk_polydata_tris)
# Feed VTK objects into director.
item_name = 'Contact surface'
polydata_item = vis.PolyDataItem(item_name, vtk_polydata_tris,
view)
polydata_item.actor.SetMapper(vtk_mapper)
polydata_item.actor.SetTexture(self.texture)
om.addToObjectModel(polydata_item, contact_data_folder)
if self.show_contact_edges and len(seg_mesh) > 0:
# Copy data to VTK objects.
vtk_segs = vtk.vtkCellArray()
vtk_segs.Allocate(len(seg_mesh))
for seg in seg_mesh:
vtk_segs.InsertNextCell(2, seg)
vtk_polydata_segs = vtk.vtkPolyData()
vtk_polydata_segs.SetPoints(vnp.getVtkPointsFromNumpy(pos))
vtk_polydata_segs.SetLines(vtk_segs)
vtk_mapper = vtk.vtkPolyDataMapper()
vtk_mapper.SetInputData(vtk_polydata_segs)
vtk_mapper.Update()
# Feed VTK objects into director.
item_name = 'Mesh edges'
polydata_item = vis.PolyDataItem(item_name, vtk_polydata_segs,
view)
polydata_item.actor.SetMapper(vtk_mapper)
[r, g, b] = color_map.get_contrasting_color()
contrasting_color = [r * 255, g * 255, b * 255]
polydata_item.actor.GetProperty().SetColor(contrasting_color)
om.addToObjectModel(polydata_item, contact_data_folder)
def _handleRigidBodies(self, rigid_bodies):
# Get the list of existing rigid bodies so we can track any
# which disappear.
remaining_body_names = set(
[x.getProperty('Name') for x in self.rigid_bodies.children()])
bodies_added = False
bodies_removed = False
for body in rigid_bodies:
body_name = 'Body ' + str(body.id)
for desc in self.data_descriptions.rigid_bodies:
if desc.id == body.id:
body_name = desc.name
if body_name in remaining_body_names:
body_obj = om.findObjectByName(body_name,
parent=self.rigid_bodies)
else:
bodies_added = True
# The use of a box here is arbitrary.
body_obj = affordanceitems.BoxAffordanceItem(
body_name, applogic.getCurrentRenderView())
om.addToObjectModel(body_obj, parentObj=self.rigid_bodies)
vis.addChildFrame(body_obj).setProperty('Deletable', False)
body_obj.setProperty('Surface Mode', 'Wireframe')
body_obj.setProperty('Color', [1, 0, 0])
remaining_body_names.discard(body_name)
x, y, z, w = body.quat
quat = (w, x, y, z)
objToOptitrack = transformUtils.transformFromPose(body.xyz, quat)
# Dimension our box based on a bounding across all of our
# markers.
all_xyz = body.marker_xyz + [body.xyz]
all_xyz = [(xyz[0] - body.xyz[0], xyz[1] - body.xyz[1],
xyz[2] - body.xyz[2]) for xyz in all_xyz]
marker_transforms = [
transformUtils.transformFromPose(xyz, (1, 0, 0, 0))
for xyz in all_xyz
]
inv_transform = transformUtils.transformFromPose((0, 0, 0),
(w, -x, -y, -z))
marker_transforms = [
transformUtils.concatenateTransforms([t, inv_transform])
for t in marker_transforms
]
all_xyz = [t.GetPosition() for t in marker_transforms]
(min_x, min_y, min_z) = (min(xyz[0] for xyz in all_xyz),
min(xyz[1] for xyz in all_xyz),
min(xyz[2] for xyz in all_xyz))
(max_x, max_y, max_z) = (max(xyz[0] for xyz in all_xyz),
max(xyz[1] for xyz in all_xyz),
max(xyz[2] for xyz in all_xyz))
dimensions = (max(0.01, max_x - min_x), max(0.01, max_y - min_y),
max(0.01, max_z - min_z))
#print "max, min", (max_x, max_y, max_z), (min_x, min_y, min_z)
body_obj.setProperty('Dimensions', dimensions)
objToWorld = transformUtils.concatenateTransforms(
[objToOptitrack, self.optitrackToWorld])
body_obj.getChildFrame().copyFrame(objToWorld)
folder = om.getOrCreateContainer('Models', parentObj=body_obj)
self._updateMarkerCollection(
body_name + '.', folder, body.marker_ids,
body.marker_xyz) #, base_transform=transform)
if len(remaining_body_names):
bodies_removed = True
for remaining_body in remaining_body_names:
obj = om.findObjectByName(remaining_body, self.rigid_bodies)
om.removeFromObjectModel(obj)
if bodies_added or bodies_removed:
self.callbacks.process(
'RIGID_BODY_LIST_CHANGED',
sorted([
x.getProperty('Name')
for x in self.rigid_bodies.children()
]))
def pickPoint(displayPoint, view, obj=None, pickType='points', tolerance=0.01):
"""
:param displayPoint:
:param view:
:param obj:
:param pickType:
:param tolerance:
:return: FieldContainer with fields
pickedPoint
pickedProp
pickedDataset
pickedNormal - is None if no normal can be comp
pickedCellId - is None unless pickType="cells"
"""
assert pickType in ('points', 'cells', 'render')
view = view or app.getCurrentRenderView()
assert view
if isinstance(obj, str):
obj = om.findObjectByName(obj)
assert obj
if pickType == 'render':
picker = vtk.vtkPropPicker()
else:
picker = vtk.vtkPointPicker(
) if pickType == 'points' else vtk.vtkCellPicker()
picker.SetTolerance(tolerance)
if obj is not None:
if isinstance(obj, list):
for o in obj:
picker.AddPickList(o.actor)
obj = None
else:
picker.AddPickList(obj.actor)
picker.PickFromListOn()
picker.Pick(displayPoint[0], displayPoint[1], 0, view.renderer())
pickedProp = picker.GetViewProp()
pickedPoint = np.array(picker.GetPickPosition())
pickedDataset = pickedProp.GetMapper().GetInput() if isinstance(
pickedProp, vtk.vtkActor) else None
if pickType == "cells":
pickedCellId = picker.GetCellId()
else:
pickedCellId = None
# populate pickedNormal if possible
pickedNormal = None
if pickType == 'cells':
pickedNormal = np.array(picker.GetPickNormal())
elif pickType == 'points' and pickedDataset:
pointId = picker.GetPointId()
normals = pickedDataset.GetPointData().GetNormals()
if normals:
pickedNormal = np.array(normals.GetTuple3(pointId))
#if pickedDataset and pickType == 'cells':
# print 'point id:', pickedDataset.GetCell(picker.GetCellId()).GetPointIds().GetId(picker.GetSubId())
#if pickType == 'points':
# print 'point id:', picker.GetPointId()
fields = FieldContainer(pickedPoint=pickedPoint,
pickedProp=pickedProp,
pickedDataset=pickedDataset,
pickedNormal=pickedNormal,
pickedCellId=pickedCellId)
return fields
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:
view = applogic.getCurrentRenderView()
# Keep track if any DebugData is written to.
# Necessary to keep DrakeVisualizer from spewing messages to the
# console when no DebugData is sent to director.
has_debug_data = False
# 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])
has_debug_data = True
# 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])
has_debug_data = True
# Iterate over all quadrature points, drawing traction and slip
# velocity vectors.
if self.show_traction_vectors or self.show_slip_velocity_vectors:
# Arrows and/or spheres are drawn through debug data if there
# exists a quadrature point.
if surface.num_quadrature_points > 0:
has_debug_data = True
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])
# Send everything except pressure and contact edges to director.
if has_debug_data:
item_name = '{}, {}'.format(
surface.body1_name, surface.body2_name)
cls = vis.PolyDataItem
item = cls(item_name, d.getPolyData(), view)
om.addToObjectModel(item, folder)
item.setProperty('Visible', True)
item.setProperty('Alpha', 1.0)
# Coloring for force and moment vectors.
item.colorBy('RGB255')
if self.show_pressure or self.show_contact_edges:
pos, pos_above, pos_below, uvs, tri_mesh, seg_mesh = \
self.process_triangles(surface)
if self.show_pressure and len(tri_mesh) > 0:
# Copy data to VTK objects.
vtk_uvs = vnp.getVtkFromNumpy(uvs)
vtk_tris_above = vtk.vtkCellArray()
vtk_tris_below = vtk.vtkCellArray()
vtk_tris_above.Allocate(len(tri_mesh))
vtk_tris_below.Allocate(len(tri_mesh))
for tri in tri_mesh:
vtk_tris_above.InsertNextCell(3, tri)
vtk_tris_below.InsertNextCell(3, tri)
vtk_polydata_tris_above = vtk.vtkPolyData()
vtk_polydata_tris_above.SetPoints(
vnp.getVtkPointsFromNumpy(pos_above))
vtk_polydata_tris_above.SetPolys(vtk_tris_above)
vtk_polydata_tris_above.GetPointData().SetTCoords(vtk_uvs)
vtk_polydata_tris_below = vtk.vtkPolyData()
vtk_polydata_tris_below.SetPoints(
vnp.getVtkPointsFromNumpy(pos_below))
vtk_polydata_tris_below.SetPolys(vtk_tris_below)
vtk_polydata_tris_below.GetPointData().SetTCoords(vtk_uvs)
vtk_mapper_above = vtk.vtkPolyDataMapper()
vtk_mapper_above.SetInputData(vtk_polydata_tris_above)
vtk_mapper_below = vtk.vtkPolyDataMapper()
vtk_mapper_below.SetInputData(vtk_polydata_tris_below)
# Feed VTK objects into director.
item_name = 'Pressure between {}, {}'.format(
surface.body1_name, surface.body2_name)
polydata_item_above = vis.PolyDataItem(
item_name, vtk_polydata_tris_above, view)
polydata_item_above.actor.SetMapper(vtk_mapper_above)
polydata_item_above.actor.SetTexture(self.texture)
om.addToObjectModel(polydata_item_above, folder)
item_name = 'Pressure between {}, {}'.format(
surface.body1_name, surface.body2_name)
polydata_item_below = vis.PolyDataItem(
item_name, vtk_polydata_tris_below, view)
polydata_item_below.actor.SetMapper(vtk_mapper_below)
polydata_item_below.actor.SetTexture(self.texture)
om.addToObjectModel(polydata_item_below, folder)
if self.show_contact_edges and len(seg_mesh) > 0:
# Copy data to VTK objects.
vtk_segs = vtk.vtkCellArray()
vtk_segs.Allocate(len(seg_mesh))
for seg in seg_mesh:
vtk_segs.InsertNextCell(2, seg)
vtk_polydata_segs = vtk.vtkPolyData()
vtk_polydata_segs.SetPoints(
vnp.getVtkPointsFromNumpy(pos))
vtk_polydata_segs.SetLines(vtk_segs)
vtk_mapper = vtk.vtkPolyDataMapper()
vtk_mapper.SetInputData(vtk_polydata_segs)
vtk_mapper.Update()
# Feed VTK objects into director.
item_name = 'Contact edges between {}, {}'.format(
surface.body1_name, surface.body2_name)
polydata_item = vis.PolyDataItem(
item_name, vtk_polydata_segs, view)
polydata_item.actor.SetMapper(vtk_mapper)
[r, g, b] = color_map.get_contrasting_color()
contrasting_color = [r*255, g*255, b*255]
polydata_item.actor.GetProperty().SetColor(contrasting_color)
om.addToObjectModel(polydata_item, folder)
def pickPoint(displayPoint, view, obj=None, pickType='points', tolerance=0.01):
"""
:param displayPoint:
:param view:
:param obj:
:param pickType:
:param tolerance:
:return: FieldContainer with fields
pickedPoint
pickedProp
pickedDataset
pickedNormal - is None if no normal can be comp
pickedCellId - is None unless pickType="cells"
"""
assert pickType in ('points', 'cells', 'render')
view = view or app.getCurrentRenderView()
assert view
if isinstance(obj, str):
obj = om.findObjectByName(obj)
assert obj
if pickType == 'render':
picker = vtk.vtkPropPicker()
else:
picker = vtk.vtkPointPicker() if pickType == 'points' else vtk.vtkCellPicker()
picker.SetTolerance(tolerance)
if obj is not None:
if isinstance(obj, list):
for o in obj:
picker.AddPickList(o.actor)
obj = None
else:
picker.AddPickList(obj.actor)
picker.PickFromListOn()
picker.Pick(displayPoint[0], displayPoint[1], 0, view.renderer())
pickedProp = picker.GetViewProp()
pickedPoint = np.array(picker.GetPickPosition())
pickedDataset = pickedProp.GetMapper().GetInput() if isinstance(pickedProp, vtk.vtkActor) else None
if pickType == "cells":
pickedCellId = picker.GetCellId()
else:
pickedCellId = None
# populate pickedNormal if possible
pickedNormal = None
if pickType == 'cells':
pickedNormal = np.array(picker.GetPickNormal())
elif pickType == 'points' and pickedDataset:
pointId = picker.GetPointId()
normals = pickedDataset.GetPointData().GetNormals()
if normals:
pickedNormal = np.array(normals.GetTuple3(pointId))
#if pickedDataset and pickType == 'cells':
# print 'point id:', pickedDataset.GetCell(picker.GetCellId()).GetPointIds().GetId(picker.GetSubId())
#if pickType == 'points':
# print 'point id:', picker.GetPointId()
fields = FieldContainer(
pickedPoint=pickedPoint,
pickedProp=pickedProp,
pickedDataset=pickedDataset,
pickedNormal=pickedNormal,
pickedCellId=pickedCellId
)
return fields
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 onOpenUrdf(filename):
model = roboturdf.openUrdf(filename, app.getCurrentRenderView())
if not model:
app.showErrorMessage('Failed to read urdf file: %s' % filename, title='Read urdf error')
def onOpenOtdf(filename):
model = otdfmodel.openOtdf(filename, app.getCurrentRenderView())
def 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()
# TODO(drum) Consider exiting early if no visualization options are
# enabled.
# Iterate over all triangles.
for surface in msg.hydroelastic_contacts:
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(surface.triangles) > 0:
item.colorBy('RGB255')
def startButton():
view = app.getCurrentRenderView()
init(view)
_kinectSource.start()
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')
image[i, j, :] = normal
# add some rgb conversion step, maybe png writer does that???
return image
if __name__ == '__main__':
#setup
view_height = 640
view_width = 480
data_dir = sys.argv[1]
num_im = int(sys.argv[2])
mesh = sys.argv[3]
#launch app
application = mainwindowapp.construct() #globals?
view = app.getCurrentRenderView()
#app.resetCamera(viewDirection=[-1,0,0], view=view)
#load mesh TODO: load raw point cloud and convert to mesh automatically
polyData = ioUtils.readPolyData(data_dir + '/' + sys.argv[3])
obj = vis.showPolyData(polyData, name=mesh)
#setup
view.setFixedSize(view_height, view_width)
setCameraInstrinsicsAsus(view)
setCameraTransform(view.camera(), vtk.vtkTransform())
view.forceRender()
filter1 = vtk.vtkWindowToImageFilter()
filter1 = vtk.vtkWindowToImageFilter()
imageWriter = vtk.vtkBMPWriter()
