def handle_init_message(self, msg):
"""Creates the polydata for the deformable mesh specified in msg."""
folder = om.getOrCreateContainer(self._folder_name)
for poly_item in self._poly_item_list:
om.removeFromObjectModel(poly_item)
self._names = []
self._vertex_counts = []
self._poly_data_list = []
self._poly_item_list = []
for mesh in msg.meshes:
self._names.append(mesh.name)
# Initial vertex positions are garbage; meaningful values will be
# set by the update message.
points = vtk.vtkPoints()
self._vertex_counts.append(mesh.num_vertices)
for i in range(mesh.num_vertices):
points.InsertNextPoint(0, 0, 0)
triangles = vtk.vtkCellArray()
for tri in mesh.tris:
triangles.InsertNextCell(3)
for i in tri.vertices:
triangles.InsertCellPoint(i)
poly_data = vtk.vtkPolyData()
poly_data.SetPoints(points)
poly_data.SetPolys(triangles)
poly_item = vis.showPolyData(poly_data, mesh.name, parent=folder)
poly_item.setProperty("Surface Mode", "Surface with edges")
self._poly_data_list.append(poly_data)
self._poly_item_list.append(poly_item)
def add_vtk_polygon(vertices):
"""
Create a 3d polygon data with vtk.
:type vertices: vertices data of polygon
"""
# Setup four points
points = vtk.vtkPoints()
for point in vertices:
points.InsertNextPoint(point[0], point[1], point[2])
# Create the polygon
polygon = vtk.vtkPolygon()
polygon.GetPointIds().SetNumberOfIds(4) # make a quad
polygon.GetPointIds().SetId(0, 0)
polygon.GetPointIds().SetId(1, 1)
polygon.GetPointIds().SetId(2, 2)
polygon.GetPointIds().SetId(3, 3)
# Add the polygon to a list of polygons
polygons = vtk.vtkCellArray()
polygons.InsertNextCell(polygon)
# Create a PolyData
polygon_poly_data = vtk.vtkPolyData()
polygon_poly_data.SetPoints(points)
polygon_poly_data.SetPolys(polygons)
return polygon_poly_data
def getTraceData(self):
t = self.frame.findChild(self.traceName)
if not t:
pts = vtk.vtkPoints()
pts.SetDataTypeToDouble()
pts.InsertNextPoint(self.frame.transform.GetPosition())
pd = vtk.vtkPolyData()
pd.SetPoints(pts)
pd.SetLines(vtk.vtkCellArray())
t = showPolyData(pd, self.traceName, parent=self.frame)
return t
def getTraceData(self):
t = self.frame.findChild(self.traceName)
if not t:
pts = vtk.vtkPoints()
pts.SetDataTypeToDouble()
pts.InsertNextPoint(self.frame.transform.GetPosition())
pd = vtk.vtkPolyData()
pd.SetPoints(pts)
pd.SetLines(vtk.vtkCellArray())
t = showPolyData(pd, self.traceName, parent=self.frame)
return t
def createPolyDataFromMeshArrays(pts, faces):
pd = vtk.vtkPolyData()
pd.SetPoints(vtk.vtkPoints())
pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))
assert len(faces) % 3 == 0
cells = vtk.vtkCellArray()
for i in xrange(len(faces) / 3):
tri = vtk.vtkTriangle()
tri.GetPointIds().SetId(0, faces[i * 3 + 0])
tri.GetPointIds().SetId(1, faces[i * 3 + 1])
tri.GetPointIds().SetId(2, faces[i * 3 + 2])
cells.InsertNextCell(tri)
pd.SetPolys(cells)
return pd
def createPolyDataFromMeshArrays(pts, faces):
pd = vtk.vtkPolyData()
pd.SetPoints(vtk.vtkPoints())
pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))
assert len(faces) % 3 == 0
cells = vtk.vtkCellArray()
for i in xrange(len(faces) / 3):
tri = vtk.vtkTriangle()
tri.GetPointIds().SetId(0, faces[i * 3 + 0])
tri.GetPointIds().SetId(1, faces[i * 3 + 1])
tri.GetPointIds().SetId(2, faces[i * 3 + 2])
cells.InsertNextCell(tri)
pd.SetPolys(cells)
return pd
def createPolyDataFromMeshArrays(pts, faces):
pd = vtk.vtkPolyData()
pd.SetPoints(vtk.vtkPoints())
pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))
cells = vtk.vtkCellArray()
for face in faces:
assert len(face) == 3, "Non-triangular faces are not supported."
tri = vtk.vtkTriangle()
tri.GetPointIds().SetId(0, face[0])
tri.GetPointIds().SetId(1, face[1])
tri.GetPointIds().SetId(2, face[2])
cells.InsertNextCell(tri)
pd.SetPolys(cells)
return pd
def onPlanarLidar(self, msg, channel):
linkName = channel.replace('DRAKE_PLANAR_LIDAR_', '', 1)
robotNum, linkName = linkName.split('_', 1)
robotNum = int(robotNum)
try:
link = self.getLink(robotNum, linkName)
except KeyError:
if linkName not in self.linkWarnings:
print 'Error locating link name:', linkName
self.linkWarnings.add(linkName)
else:
if len(link.geometry):
polyData = link.geometry[0].polyDataItem
else:
polyData = vtk.vtkPolyData()
name = linkName + ' geometry data'
visInfo = FieldContainer(color=[1.0, 0.0, 0.0],
alpha=1.0,
texture=None)
g = Geometry(name, polyData, visInfo)
link.geometry.append(g)
linkFolder = self.getLinkFolder(robotNum, linkName)
self.addLinkGeometry(g, linkName, linkFolder)
g.polyDataItem.actor.SetUserTransform(link.transform)
points = vtk.vtkPoints()
verts = vtk.vtkCellArray()
t = msg.rad0
for r in msg.ranges:
if r >= 0:
x = r * math.cos(t)
y = r * math.sin(t)
pointId = points.InsertNextPoint([x, y, 0])
verts.InsertNextCell(1)
verts.InsertCellPoint(pointId)
t += msg.radstep
polyData.SetPoints(points)
polyData.SetVerts(verts)
def onPlanarLidar(self, msg, channel):
linkName = channel.replace("DRAKE_PLANAR_LIDAR_", "", 1)
robotNum, linkName = linkName.split("_", 1)
robotNum = int(robotNum)
try:
link = self.getLink(robotNum, linkName)
except KeyError:
if linkName not in self.linkWarnings:
print "Error locating link name:", linkName
self.linkWarnings.add(linkName)
else:
if len(link.geometry):
polyData = link.geometry[0].polyDataItem
else:
polyData = vtk.vtkPolyData()
name = linkName + " geometry data"
geom = type("", (), {})
geom.color = [1.0, 0.0, 0.0, 1.0]
g = Geometry(name, geom, polyData, link.transform)
link.geometry.append(g)
linkFolder = self.getLinkFolder(robotNum, linkName)
self.addLinkGeometry(g, linkName, linkFolder)
g.polyDataItem.actor.SetUserTransform(link.transform)
points = vtk.vtkPoints()
verts = vtk.vtkCellArray()
t = msg.rad0
for r in msg.ranges:
if r >= 0:
x = r * math.cos(t)
y = r * math.sin(t)
pointId = points.InsertNextPoint([x, y, 0])
verts.InsertNextCell(1)
verts.InsertCellPoint(pointId)
t += msg.radstep
polyData.SetPoints(points)
polyData.SetVerts(verts)
def onPlanarLidar(self, msg, channel):
linkName = channel.replace('DRAKE_PLANAR_LIDAR_', '', 1)
robotNum, linkName = linkName.split('_', 1)
robotNum = int(robotNum)
try:
link = self.getLink(robotNum, linkName)
except KeyError:
if linkName not in self.linkWarnings:
print 'Error locating link name:', linkName
self.linkWarnings.add(linkName)
else:
if len(link.geometry):
polyData = link.geometry[0].polyDataItem
else:
polyData = vtk.vtkPolyData()
name = linkName + ' geometry data'
visInfo = FieldContainer(color=[1.0,0.0,0.0], alpha=1.0, texture=None)
g = Geometry(name, polyData, visInfo)
link.geometry.append(g)
linkFolder = self.getLinkFolder(robotNum, linkName)
self.addLinkGeometry(g, linkName, linkFolder)
g.polyDataItem.actor.SetUserTransform(link.transform)
points = vtk.vtkPoints()
verts = vtk.vtkCellArray()
t = msg.rad0
for r in msg.ranges:
if r >= 0:
x = r * math.cos(t)
y = r * math.sin(t)
pointId = points.InsertNextPoint([x,y,0])
verts.InsertNextCell(1)
verts.InsertCellPoint(pointId)
t += msg.radstep
polyData.SetPoints(points)
polyData.SetVerts(verts)
def assimpMeshToPolyData(mesh):
verts = mesh.vertices
faces = mesh.faces
nfaces = faces.shape[0]
nverts = verts.shape[0]
assert verts.shape[1] == 3
assert faces.shape[1] == 3
points = vnp.getVtkPointsFromNumpy(verts)
cells = vtk.vtkCellArray()
for i in xrange(nfaces):
face = faces[i]
tri = vtk.vtkTriangle()
tri.GetPointIds().SetId(0, face[0])
tri.GetPointIds().SetId(1, face[1])
tri.GetPointIds().SetId(2, face[2])
cells.InsertNextCell(tri)
polyData = vtk.vtkPolyData()
polyData.SetPoints(points)
polyData.SetPolys(cells)
if mesh.normals.shape[0] > 0:
assert mesh.normals.shape[0] == nverts
normals = vnp.getVtkFromNumpy(mesh.normals)
normals.SetName('normals')
polyData.GetPointData().AddArray(normals)
polyData.GetPointData().SetNormals(normals)
for i, tcoords in enumerate(mesh.texturecoords):
tcoordArray = assimpTextureCoordsToArray(tcoords)
tcoordArray.SetName('tcoords_%d' % i)
polyData.GetPointData().AddArray(tcoordArray)
if i == 0:
polyData.GetPointData().SetTCoords(tcoordArray)
return polyData
def createPolyDataFromMeshArrays(pts, faces):
pd = vtk.vtkPolyData()
pd.SetPoints(vtk.vtkPoints())
if pts.size > 0:
pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))
cells = vtk.vtkCellArray()
tri = vtk.vtkTriangle()
setId = tri.GetPointIds().SetId # bind the method for convenience
for face in faces:
assert len(face) == 3, "Non-triangular faces are not supported."
setId(0, face[0])
setId(1, face[1])
setId(2, face[2])
cells.InsertNextCell(tri)
pd.SetPolys(cells)
return pd
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 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)