def __init__(
self, view, channelName, coordinateFrame, sensorName, intensityRange=(400, 4000)
):
TimerCallback.__init__(self)
self.view = view
self.channelName = channelName
self.reader = None
self.displayedRevolution = -1
self.lastScanLine = 0
self.numberOfScanLines = 100
self.nextScanLineId = 0
self.scanLines = []
self.pointSize = 1
self.alpha = 0.5
self.visible = True
self.colorBy = "Solid Color"
self.intensityRange = intensityRange
self.initScanLines()
self.sensorName = sensorName
self.coordinateFrame = coordinateFrame
self.revPolyData = vtk.vtkPolyData()
self.polyDataObj = vis.PolyDataItem("Lidar Sweep", self.revPolyData, view)
self.polyDataObj.actor.SetPickable(1)
self.polyDataObj.setRangeMap("intensity", intensityRange)
self.setPointSize(self.pointSize)
self.setAlpha(self.alpha)
self.targetFps = 60
self.colorizeCallback = None
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 __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 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 _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 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 __init__(self, name, geom, polyData, parentTransform):
self.polyDataItem = vis.PolyDataItem(name, polyData, view=None)
self.polyDataItem.setProperty('Alpha', geom.color[3])
self.polyDataItem.actor.SetTexture(Geometry.TextureCache.get( Geometry.getTextureFileName(polyData) ))
if self.polyDataItem.actor.GetTexture():
self.polyDataItem.setProperty('Color', QtGui.QColor(255, 255, 255))
else:
self.polyDataItem.setProperty('Color', QtGui.QColor(geom.color[0]*255, geom.color[1]*255, geom.color[2]*255))
if USE_SHADOWS:
self.polyDataItem.shadowOn()
def createSkybox(imageMap, view):
objs = {}
planes = createSkyboxPlanes()
for side, imageFilename in imageMap.iteritems():
texture = createTexture(imageFilename)
obj = vis.PolyDataItem("skybox %s" % side, planes[side], view=None)
obj.actor.SetTexture(texture)
obj.actor.GetProperty().LightingOff()
view.backgroundRenderer().AddActor(obj.actor)
objs[side] = obj
return objs
def initScanLines(self):
for scanLine in self.scanLines:
scanLine.removeFromAllViews()
self.scanLines = []
self.nextScanLineId = 0
self.lastScanLine = max(self.lastScanLine - self.numberOfScanLines, 0)
for i in xrange(self.numberOfScanLines):
polyData = vtk.vtkPolyData()
scanLine = vis.PolyDataItem('scan line %d' % i, polyData, self.view)
scanLine.actor.SetPickable(0)
#scanLine.setSolidColor((0,1,0))
self.scanLines.append(scanLine)
def __init__(self, app, view):
uipanel.UiPanel.__init__(self, 'ddMeasurementPanel.ui')
self.view = view
self.ui.enabledCheck.connect('toggled(bool)', self.onEnabledCheckBox)
self.ui.clearButton.connect('clicked()', self.onClear)
#self.snapshotTextShortcut = applogic.addShortcut(app.mainWindow, ' ', self.snapshotText)
#self.snapshotGeometryShortcut = applogic.addShortcut(app.mainWindow, 'Shift+ ', self.snapshotGeometry)
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.pickPoints = []
self.setEnabled(False)
def __init__(self, view):
vieweventfilter.ViewEventFilter.__init__(self, view)
d = DebugData()
d.addSphere((0, 0, 0), radius=1.0)
self.cameraCenterObj = vis.PolyDataItem('mouse point', d.getPolyData(), view)
self.cameraCenterObj.setProperty('Visible', False)
self.cameraCenterObj.setProperty('Color', [1, 1, 0])
self.cameraCenterObj.actor.SetUserTransform(vtk.vtkTransform())
self.cameraCenterObj.actor.SetPickable(False)
self.renderWindowobserver = view.renderWindow().AddObserver('StartEvent', self.onStartRender)
self.renderWindowobserver = view.renderWindow().AddObserver('EndEvent', self.onEndRender)
self.style = vtk.vtkPickCenteredInteractorStyle()
self.style.SetMotionFactor(3)
self.showOnMove = False
self.showCenter = False
self.lastHitPoint = None
self._enabled = False
self.setEnabled(True)
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 __init__(self, view):
TimerCallback.__init__(self)
self.view = view
self.reader = None
self.displayedRevolution = -1
self.lastScanLine = 0
self.numberOfScanLines = 1000
self.nextScanLineId = 0
self.scanLines = []
self.pointSize = 1
self.alpha = 0.5
self.visible = True
self.colorBy = 'Solid Color'
self.initScanLines()
self.revPolyData = vtk.vtkPolyData()
self.polyDataObj = vis.PolyDataItem('Lidar Sweep', self.revPolyData, view)
self.polyDataObj.actor.SetPickable(1)
self.setPointSize(self.pointSize)
self.setAlpha(self.alpha)
self.targetFps = 60
self.colorizeCallback = None
def __init__(self, name, polyData, visInfo):
self.polyDataItem = vis.PolyDataItem(name, polyData, view=None)
self.polyDataItem.setProperty('Color', visInfo.color)
self.polyDataItem.setProperty('Alpha', visInfo.alpha)
self.polyDataItem.actor.SetTexture(visInfo.texture)
def createPolyDataItem(self, name="geometry"):
polyDataItem = vis.PolyDataItem(name, self.polyData, view=None)
polyDataItem.setProperty("Point Size", 2)
self.updatePolyDataItemProperties(polyDataItem)
return polyDataItem
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 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)
