def __init__(self, name, polyData, view):
om.ObjectModelItem.__init__(self, name, om.Icons.Robot)
self.views = []
self.polyData = polyData
self.mapper = vtk.vtkPolyDataMapper()
self.mapper.SetInput(self.polyData)
self.actor = vtk.vtkActor()
self.actor.SetMapper(self.mapper)
self.shadowActor = None
self.scalarBarWidget = None
self.extraViewRenderers = {}
self.rangeMap = dict(PolyDataItem.defaultScalarRangeMap)
self.addProperty('Color By', 0, attributes=om.PropertyAttributes(enumNames=['Solid Color']))
self.addProperty('Visible', True)
self.addProperty('Alpha', 1.0,
attributes=om.PropertyAttributes(decimals=2, minimum=0, maximum=1.0, singleStep=0.1, hidden=False))
self.addProperty('Point Size', self.actor.GetProperty().GetPointSize(),
attributes=om.PropertyAttributes(decimals=0, minimum=1, maximum=20, singleStep=1, hidden=False))
self.addProperty('Surface Mode', 0,
attributes=om.PropertyAttributes(enumNames=['Surface', 'Wireframe', 'Surface with edges', 'Points'], hidden=True))
self.addProperty('Color', [1.0, 1.0, 1.0])
self.addProperty('Show Scalar Bar', False)
self._updateSurfaceProperty()
self._updateColorByProperty()
if view is not None:
self.addToView(view)
def __init__(self, name, polyData, view):
om.ObjectModelItem.__init__(self, name, om.Icons.Robot)
self.views = []
self.polyData = polyData
self.mapper = vtk.vtkPolyDataMapper()
self.mapper.SetInputData(self.polyData)
self.actor = vtk.vtkActor()
self.actor.SetMapper(self.mapper)
self.shadowActor = None
self.scalarBarWidget = None
self.extraViewRenderers = {}
self.rangeMap = dict(PolyDataItem.defaultScalarRangeMap)
self.addProperty(
'Color By',
0,
attributes=om.PropertyAttributes(enumNames=['Solid Color']))
self.addProperty('Visible', True)
self.addProperty('Alpha',
1.0,
attributes=om.PropertyAttributes(decimals=2,
minimum=0,
maximum=1.0,
singleStep=0.1,
hidden=False))
self.addProperty('Point Size',
self.actor.GetProperty().GetPointSize(),
attributes=om.PropertyAttributes(decimals=0,
minimum=1,
maximum=20,
singleStep=1,
hidden=False))
self.addProperty('Line Width',
self.actor.GetProperty().GetLineWidth(),
attributes=om.PropertyAttributes(decimals=0,
minimum=1,
maximum=20,
singleStep=1,
hidden=False))
self.addProperty('Surface Mode',
0,
attributes=om.PropertyAttributes(enumNames=[
'Surface', 'Wireframe', 'Surface with edges',
'Points'
],
hidden=True))
self.addProperty('Color', [1.0, 1.0, 1.0])
self.addProperty('Show Scalar Bar', False)
self._updateSurfaceProperty()
self._updateColorByProperty()
if view is not None:
self.addToView(view)
def __init__(self, name, polyData, view):
om.ObjectModelItem.__init__(self, name, om.Icons.Robot)
self.views = []
self.polyData = polyData
self.mapper = vtk.vtkPolyDataMapper()
self.mapper.SetInput(self.polyData)
self.actor = vtk.vtkActor()
self.actor.SetMapper(self.mapper)
self.shadowActor = None
self.scalarBarWidget = None
self.extraViewRenderers = {}
self.rangeMap = {
'intensity': (400, 4000),
#'z' : (0.0, 2.0),
#'distance' : (0.5, 4.0),
'spindle_angle': (0, 360),
'azimuth': (-2.5, 2.5),
'scan_delta': (0.0, 0.3)
}
self.addProperty(
'Color By',
0,
attributes=om.PropertyAttributes(enumNames=['Solid Color']))
self.addProperty('Visible', True)
self.addProperty('Alpha',
1.0,
attributes=om.PropertyAttributes(decimals=2,
minimum=0,
maximum=1.0,
singleStep=0.1,
hidden=False))
self.addProperty('Point Size',
self.actor.GetProperty().GetPointSize(),
attributes=om.PropertyAttributes(decimals=0,
minimum=1,
maximum=20,
singleStep=1,
hidden=False))
self.addProperty('Surface Mode',
0,
attributes=om.PropertyAttributes(enumNames=[
'Surface', 'Wireframe', 'Surface with edges',
'Points'
],
hidden=True))
self.addProperty('Color', [1.0, 1.0, 1.0])
self.addProperty('Show Scalar Bar', False)
self._updateSurfaceProperty()
self._updateColorByProperty()
if view is not None:
self.addToView(view)
def render_depth(renWin,
renderer,
camera,
data_dir,
data_dir_name,
num_im,
out_dir,
use_mesh,
object_dir,
mesh='meshed_scene.ply',
keyword=None):
actor = vtk.vtkActor()
filter1 = vtk.vtkWindowToImageFilter()
imageWriter = vtk.vtkPNGWriter()
scale = vtk.vtkImageShiftScale()
if use_mesh: #use meshed version of scene
if not glob.glob(data_dir + "/" + mesh):
out = None
if glob.glob(data_dir + "/original_log.lcmlog.ply"):
out = "original_log.lcmlog.ply"
elif glob.glob(data_dir + "/trimmed_log.lcmlog.ply"):
out = "trimmed_log.lcmlog.ply"
elif glob.glob('*.ply'):
out = glob.glob('*.ply')[0]
else:
return
mesher = mesh_wrapper.Mesh(out_dir=data_dir)
status = mesher.mesh_cloud(out)
print status
#blocks until done
mapper = vtk.vtkPolyDataMapper()
fileReader = vtk.vtkPLYReader()
fileReader.SetFileName(data_dir + "/" + mesh)
mapper.SetInputConnection(fileReader.GetOutputPort())
actor.SetMapper(mapper)
renderer.AddActor(actor)
else: #import just the objects
objects = common.Objects(data_dir, object_dir)
objects.loadObjectMeshes("/registration_result.yaml",
renderer,
keyword=keyword)
#setup filters
filter1.SetInput(renWin)
filter1.SetMagnification(1)
filter1.SetInputBufferTypeToZBuffer()
windowToColorBuffer = vtk.vtkWindowToImageFilter()
windowToColorBuffer.SetInput(renWin)
windowToColorBuffer.SetInputBufferTypeToRGB()
scale.SetOutputScalarTypeToUnsignedShort()
scale.SetScale(1000)
poses = common.CameraPoses(data_dir + "/posegraph.posegraph")
for i in range(1, num_im + 1):
try:
utimeFile = open(
data_dir + "/images/" + str(i).zfill(10) + "_utime.txt", 'r')
utime = int(utimeFile.read())
#update camera transform
cameraToCameraStart = poses.getCameraPoseAtUTime(utime)
t = cameraToCameraStart
common.setCameraTransform(camera, t)
renWin.Render()
#update filters
filter1.Modified()
filter1.Update()
windowToColorBuffer.Modified()
windowToColorBuffer.Update()
#extract depth image
depthImage = vtk.vtkImageData()
pts = vtk.vtkPoints()
ptColors = vtk.vtkUnsignedCharArray()
vtk.vtkDepthImageUtils.DepthBufferToDepthImage(
filter1.GetOutput(), windowToColorBuffer.GetOutput(), camera,
depthImage, pts, ptColors)
scale.SetInputData(depthImage)
scale.Update()
#write out depth image
imageWriter.SetFileName(out_dir + str(i).zfill(10) + "_" +
data_dir_name + "_depth_ground_truth.png")
imageWriter.SetInputConnection(scale.GetOutputPort())
imageWriter.Write()
except (IOError):
break
renderer.RemoveAllViewProps()
renWin.Render()
def render_normals(renWin,
renderer,
camera,
data_dir,
data_dir_name,
num_im,
out_dir,
use_mesh,
object_dir,
mesh='meshed_scene.ply',
keyword=None):
#setup rendering enviornment
actor = vtk.vtkActor()
filter1 = vtk.vtkWindowToImageFilter()
imageWriter = vtk.vtkPNGWriter()
scale = vtk.vtkImageShiftScale()
if use_mesh: #use meshed version of scene
if not glob.glob(data_dir + "/" + mesh):
out = "original_log.lcmlog.ply" if glob.glob(
data_dir +
"/original_log.lcmlog.ply") else "trimmed_log.lcmlog.ply"
mesher = mesh_wrapper.Mesh(out_dir=data_dir)
status = mesher.mesh_cloud(out)
print status
#blocks until done
mapper = vtk.vtkPolyDataMapper()
#shading
#set_material_prop(actor)
set_shader_input(mapper)
fileReader = vtk.vtkPLYReader()
fileReader.SetFileName(data_dir + "/" + mesh)
mapper.SetInputConnection(fileReader.GetOutputPort())
actor.SetMapper(mapper)
renderer.AddActor(actor)
else: #import just the objects
objects = common.Objects(data_dir, object_dir)
objects.loadObjectMeshes("/registration_result.yaml",
renderer,
keyword=keyword,
shader=set_shader_input)
#setup rendering enviornment
windowToColorBuffer = vtk.vtkWindowToImageFilter()
windowToColorBuffer.SetInput(renWin)
windowToColorBuffer.SetInputBufferTypeToRGB()
#setup camera calibration
common.set_up_camera_params(camera)
poses = common.CameraPoses(data_dir + "/posegraph.posegraph")
for i in range(1, num_im + 1):
try:
utimeFile = open(
data_dir + "/images/" + str(i).zfill(10) + "_utime.txt", 'r')
utime = int(utimeFile.read())
#update camera transform
cameraToCameraStart = poses.getCameraPoseAtUTime(utime)
t = cameraToCameraStart
common.setCameraTransform(camera, t)
renWin.Render()
windowToColorBuffer.Modified()
windowToColorBuffer.Update()
#write out depth image
imageWriter.SetFileName(out_dir + str(i).zfill(10) + "_" +
data_dir_name + "_normal_ground_truth.png")
imageWriter.SetInputConnection(windowToColorBuffer.GetOutputPort())
imageWriter.Write()
except (IOError):
break
renderer.RemoveAllViewProps()
renWin.Render()
tolerance=tolerance)
normal = np.array(pickPointFields.pickedNormal)
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]
mapper = vtk.vtkPolyDataMapper()
actor = vtk.vtkActor()
renderer = vtk.vtkRenderer()
renWin = vtk.vtkRenderWindow()
interactor = vtk.vtkRenderWindowInteractor()
fileReader = vtk.vtkPLYReader()
filter1 = vtk.vtkWindowToImageFilter()
imageWriter = vtk.vtkBMPWriter()
scale = vtk.vtkImageShiftScale()
fileReader.SetFileName(sys.argv[1] + "/" + sys.argv[3])
renWin.SetSize(view_height, view_width)
camera = vtk.vtkCamera()
renderer.SetActiveCamera(camera)
mapper.SetInputConnection(fileReader.GetOutputPort())
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)
