return pts app = consoleapp.ConsoleApp() view = app.createView() view.show() d = DebugData() d.addLine((0,0,0), (1,0,0), radius=0.03) show(d, (0, 0, 0)) d = DebugData() d.addPolygon([[0,0,0], [0.8, 0, 0], [1, 0.6, 0], [0.4, 1, 0], [-0.2, 0.6, 0]]) show(d, (2, 0, 0)) d = DebugData() d.addPolyLine(getHelixPoints(), radius=0.01) show(d, (4, 0, 0)) d = DebugData() d.addSphere([0, 0, 0], radius=0.3) show(d, (6, 0, 0)) d = DebugData() d.addFrame(vtk.vtkTransform(), scale=0.5, tubeRadius=0.03)
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(30)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# Though strangely named, DebugData() is the object through which
# drawing is done in DrakeVisualizer.
d = DebugData()
# Set the color map.
color_map = self.create_color_map()
# The scale value attributable to auto-scale.
auto_force_scale = 1.0
auto_moment_scale = 1.0
auto_traction_scale = 1.0
auto_slip_velocity_scale = 1.0
max_force = -1
max_moment = -1
max_traction = -1
max_slip = -1
# TODO(sean-curtis-TRI) Remove the following comment when this
# code can be exercised.
# The following code is not exercised presently because the
# magnitude mode is always set to kFixedLength.
# Determine scaling magnitudes if autoscaling is activated.
if self.magnitude_mode == ContactVisModes.kAutoScale:
if self.show_spatial_force:
for surface in msg.hydroelastic_contacts:
force = np.array([
surface.force_C_W[0], surface.force_C_W[1],
surface.force_C_W[2]
])
moment = np.array([
surface.moment_C_W[0], surface.moment_C_W[1],
surface.moment_C_W[2]
])
force_mag = np.linalg.norm(force)
moment_mag = np.linalg.norm(moment)
if force_mag > max_force:
max_force = force_mag
if moment_mag > max_moment:
max_moment = moment_mag
# Prepare scaling information for the traction vectors.
if self.show_traction_vectors:
for quad_point_data in surface.quadrature_point_data:
traction = np.array([
quad_point_data.traction_Aq_W[0],
quad_point_data.traction_Aq_W[1],
quad_point_data.traction_Aq_W[2]
])
max_traction = max(max_traction, np.linalg.norm(traction))
# Prepare scaling information for the slip velocity vectors.
if self.show_slip_velocity_vectors:
for quad_point_data in surface.quadrature_point_data:
slip_speed = np.array([
quad_point_data.vt_BqAq_W[0],
quad_point_data.vt_BqAq_W[1],
quad_point_data.vt_BqAq_W[2]
])
max_slip_speed = max(max_slip_speed,
np.linalg.norm(slip_speed))
# Compute scaling factors.
auto_force_scale = 1.0 / max_force
auto_moment_scale = 1.0 / max_moment
auto_traction_scale = 1.0 / max_traction
auto_slip_velocity_scale = 1.0 / max_slip_speed
# TODO(drum) Consider exiting early if no visualization options are
# enabled.
for surface in msg.hydroelastic_contacts:
# Draw the spatial force.
if self.show_spatial_force:
point = np.array([
surface.centroid_W[0], surface.centroid_W[1],
surface.centroid_W[2]
])
force = np.array([
surface.force_C_W[0], surface.force_C_W[1],
surface.force_C_W[2]
])
moment = np.array([
surface.moment_C_W[0], surface.moment_C_W[1],
surface.moment_C_W[2]
])
force_mag = np.linalg.norm(force)
moment_mag = np.linalg.norm(moment)
# Draw the force arrow if it's of sufficient magnitude.
if force_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode == ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this force would be
# skipped.
scale /= force_mag
d.addArrow(start=point,
end=point + auto_force_scale * force * scale,
tubeRadius=0.005,
headRadius=0.01,
color=[1, 0, 0])
# Draw the moment arrow if it's of sufficient magnitude.
if moment_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode == ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this moment would be
# skipped.
scale /= moment_mag
d.addArrow(start=point,
end=point + auto_moment_scale * moment * scale,
tubeRadius=0.005,
headRadius=0.01,
color=[0, 0, 1])
# Iterate over all quadrature points, drawing traction and slip
# velocity vectors.
if self.show_traction_vectors or self.show_slip_velocity_vectors:
for quad_point_data in surface.quadrature_point_data:
origin = np.array([
quad_point_data.p_WQ[0], quad_point_data.p_WQ[1],
quad_point_data.p_WQ[2]
])
if self.show_traction_vectors:
traction = np.array([
quad_point_data.traction_Aq_W[0],
quad_point_data.traction_Aq_W[1],
quad_point_data.traction_Aq_W[2]
])
traction_mag = np.linalg.norm(traction)
# Draw the arrow only if it's of sufficient magnitude.
if traction_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode ==\
ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this traction
# would be skipped.
scale /= traction_mag
offset = auto_traction_scale * traction * scale
d.addArrow(start=origin,
end=origin + offset,
tubeRadius=0.000125,
headRadius=0.00025,
color=[1, 0, 1])
else:
d.addSphere(center=origin,
radius=0.000125,
color=[1, 0, 1])
if self.show_slip_velocity_vectors:
slip = np.array([
quad_point_data.vt_BqAq_W[0],
quad_point_data.vt_BqAq_W[1],
quad_point_data.vt_BqAq_W[2]
])
slip_mag = np.linalg.norm(slip)
# Draw the arrow only if it's of sufficient magnitude.
if slip_mag > self.min_magnitude:
scale = self.global_scale
if self.magnitude_mode ==\
ContactVisModes.kFixedLength:
# magnitude must be > 0 otherwise this slip
# vector would be skipped.
scale /= slip_mag
offset = auto_slip_velocity_scale * slip * scale
d.addArrow(start=origin,
end=origin + offset,
tubeRadius=0.000125,
headRadius=0.00025,
color=[0, 1, 1])
else:
d.addSphere(center=origin,
radius=0.000125,
color=[0, 1, 1])
# Iterate over all triangles.
for tri in surface.triangles:
va = np.array([tri.p_WA[0], tri.p_WA[1], tri.p_WA[2]])
vb = np.array([tri.p_WB[0], tri.p_WB[1], tri.p_WB[2]])
vc = np.array([tri.p_WC[0], tri.p_WC[1], tri.p_WC[2]])
# Save the maximum pressure.
self.max_pressure_observed = max(self.max_pressure_observed,
tri.pressure_A)
self.max_pressure_observed = max(self.max_pressure_observed,
tri.pressure_B)
self.max_pressure_observed = max(self.max_pressure_observed,
tri.pressure_C)
# TODO(drum) Vertex color interpolation may be insufficiently
# granular if a single triangle spans too large a range of
# pressures. Suggested solution is to use a texture map.
# Get the colors at the vertices.
color_a = color_map.get_color(tri.pressure_A)
color_b = color_map.get_color(tri.pressure_B)
color_c = color_map.get_color(tri.pressure_C)
if self.show_pressure:
# TODO(drum) Use a better method for this; the current
# approach is susceptible to z-fighting under certain
# zoom levels.
# Compute a normal to the triangle. We need this normal
# because the visualized pressure surface can be coplanar
# with parts of the visualized geometry, in which case a
# dithering type effect would appear. So we use the normal
# to draw two triangles slightly offset to both sides of
# the contact surface.
# Note that if the area of this triangle is very small, we
# won't waste time visualizing it, which also means that
# won't have to worry about degenerate triangles).
# TODO(edrumwri) Consider allowing the user to set these
# next two values programmatically.
min_area = 1e-8
offset_scalar = 1e-4
normal = np.cross(vb - va, vc - vb)
norm_normal = np.linalg.norm(normal)
if norm_normal >= min_area:
unit_normal = normal / np.linalg.norm(normal)
offset = unit_normal * offset_scalar
d.addPolygon([va + offset, vb + offset, vc + offset],
color=[color_a, color_b, color_c])
d.addPolygon([va - offset, vb - offset, vc - offset],
color=[color_a, color_b, color_c])
# TODO(drum) Consider drawing shared edges just once.
if self.show_contact_edges:
contrasting_color = color_map.get_contrasting_color()
d.addPolyLine(points=(va, vb, vc),
isClosed=True,
color=contrasting_color)
item_name = '{}, {}'.format(surface.body1_name, surface.body2_name)
cls = vis.PolyDataItem
view = applogic.getCurrentRenderView()
item = cls(item_name, d.getPolyData(), view)
om.addToObjectModel(item, folder)
item.setProperty('Visible', True)
item.setProperty('Alpha', 1.0)
# Conditional necessary to keep DrakeVisualizer from spewing
# messages to the console when the contact surface is empty.
if len(msg.hydroelastic_contacts) > 0:
item.colorBy('RGB255')
def 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')
