def createPolyLine(params):
d = DebugData()
points = [np.asarray(p) for p in params["points"]]
color = params.get("color", DEFAULT_COLOR)[:3]
radius = params.get("radius", 0.01)
startHead = params.get("start_head", False)
endHead = params.get("end_head", False)
headRadius = params.get("head_radius", 0.05)
headLength = params.get("head_length", headRadius)
isClosed = params.get("closed", False)
if startHead:
normal = points[0] - points[1]
normal = normal / np.linalg.norm(normal)
points[0] = points[0] - 0.5 * headLength * normal
d.addCone(origin=points[0],
normal=normal,
radius=headRadius,
height=headLength,
color=color,
fill=True)
if endHead:
normal = points[-1] - points[-2]
normal = normal / np.linalg.norm(normal)
points[-1] = points[-1] - 0.5 * headLength * normal
d.addCone(origin=points[-1],
normal=normal,
radius=headRadius,
height=headLength,
color=color,
fill=True)
d.addPolyLine(points, isClosed, radius=radius, color=color)
return [d.getPolyData()]
def createPolyLine(params):
d = DebugData()
points = [np.asarray(p) for p in params["points"]]
color = params.get("color", DEFAULT_COLOR)[:3]
radius = params.get("radius", 0.01)
startHead = params.get("start_head", False)
endHead = params.get("end_head", False)
headRadius = params.get("head_radius", 0.05)
headLength = params.get("head_length", headRadius)
isClosed = params.get("closed", False)
if startHead:
normal = points[0] - points[1]
normal = normal / np.linalg.norm(normal)
points[0] = points[0] - 0.5 * headLength * normal
d.addCone(origin=points[0], normal=normal, radius=headRadius,
height=headLength, color=color, fill=True)
if endHead:
normal = points[-1] - points[-2]
normal = normal / np.linalg.norm(normal)
points[-1] = points[-1] - 0.5 * headLength * normal
d.addCone(origin=points[-1], normal=normal, radius=headRadius,
height=headLength, color=color, fill=True)
d.addPolyLine(points, isClosed, radius=radius, color=color)
return [d.getPolyData()]
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')
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) show(d, (0, 2, 0)) d = DebugData() d.addArrow((0, 0, 0), (0, 1, 0))
def drawShape(self, currShape, next_loc, msg, rotation_matrix=None):
'''
Function for drawing shapes. Currently this supports lines, points, and
3D axes
currShape: the current shape to be drawn
next_loc: the location where to draw the shape
msg: the message from the LCM hendler that called this function
rotation_matrix: the rotation matrix to be used for the axis shape.
Mainly used by the abstract handler
'''
# draw a continuous line
if (currShape.type == "line"):
# check if the duration has been initialized
if (currShape.duration == None or len(currShape.points) == 0):
currShape.duration = msg.utime / 1000000
# visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5
if (((msg.utime / 1000000) - currShape.duration <=
currShape.history) or currShape.history <= 0):
# make sure to add at least 2 points before starting to check for the distance between points
if (len(currShape.points) < 2):
currShape.points.append(next_loc)
d = DebugData()
d.addPolyLine(currShape.points,
radius=currShape.thickness,
color=currShape.color)
if (currShape.object == None):
currShape.object = vis.showPolyData(
d.getPolyData(), currShape.name)
currShape.object.setProperty('Color', currShape.color)
else:
currShape.object.setPolyData(d.getPolyData())
else:
if (np.linalg.norm(
np.array(next_loc) -
np.array(currShape.points[-1])) >= 10e-5):
currShape.points.append(next_loc)
d = DebugData()
d.addPolyLine(currShape.points,
radius=currShape.thickness,
color=currShape.color)
if (currShape.object == None):
currShape.object = vis.showPolyData(
d.getPolyData(), currShape.name)
else:
currShape.object.setPolyData(d.getPolyData())
elif (currShape.history > 0):
if (len(currShape.points) == 0):
currShape.duration = msg.utime / 1000000
else:
# visualize and trace line for 'history' seconds, adding points at a distance at least 10e-5
if (np.linalg.norm(
np.array(next_loc) -
np.array(currShape.points[-1])) >= 10e-5):
currShape.points.popleft()
currShape.points.append(next_loc)
d = DebugData()
d.addPolyLine(currShape.points,
radius=currShape.thickness,
color=currShape.color)
if (currShape.object == None):
currShape.object = vis.showPolyData(
d.getPolyData(), currShape.name)
else:
currShape.object.setPolyData(d.getPolyData())
# draw a point
elif (currShape.type == "point"):
d = DebugData()
d.addSphere(next_loc, radius=currShape.radius)
# create a new point
if (currShape.created == True):
currShape.object = vis.showPolyData(d.getPolyData(),
currShape.name)
# set color and transparency of point
currShape.object.setProperty('Color', currShape.color)
currShape.object.setProperty('Alpha', currShape.alpha)
currShape.created = False
else:
# update the location of the last point
currShape.object.setPolyData(d.getPolyData())
# draw a set of axes
elif (currShape.type == "axes"):
# get the rotation matrix
rot_matrix = None
if (currShape.category != "lcm"):
rigTrans = self.plant.EvalBodyPoseInWorld(
self.context, self.plant.GetBodyByName(currShape.frame))
rot_matrix = rigTrans.rotation().matrix().transpose()
else:
rot_matrix = rotation_matrix
colors = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
d = DebugData()
for i in range(3):
d.addArrow(next_loc,
next_loc + (rot_matrix[i] * currShape.length),
headRadius=0.03,
color=colors[i])
# create the 3 axes
if (currShape.created == True):
currShape.object = vis.showPolyData(d.getPolyData(),
currShape.name,
colorByName='RGB255')
currShape.object.setProperty('Alpha', currShape.alpha)
currShape.created = False
else:
# update the location of the last point
currShape.object.setPolyData(d.getPolyData())
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')
