def addTestData():
d = DebugData()
# d.addSphere((0,0,0), radius=0.5)
d.addArrow((0, 0, 0), (0, 0, 1), color=[1, 0, 0])
d.addArrow((0, 0, 1), (0, .5, 1), color=[0, 1, 0])
vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255')
view.resetCamera()
def newMesh():
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceManager.newAffordanceFromDescription(desc)
def handle_message(self, msg):
# Limits the rate of message handling, since redrawing is done in the
# message handler.
self._sub.setSpeedLimit(0.1)
# Removes the folder completely.
om.removeFromObjectModel(om.findObjectByName(self._folder_name))
# Recreates folder.
folder = om.getOrCreateContainer(self._folder_name)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for arrow in msg.arrow_info:
point = np.array([
arrow.arrow_origin[0], arrow.arrow_origin[1],
arrow.arrow_origin[2]
])
vec = np.array([
arrow.arrow_vector[0], arrow.arrow_vector[1],
arrow.arrow_vector[2]
])
d = DebugData()
d.addArrow(start=point,
end=vec + point,
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(0),
parent=folder,
color=[arrow.rgb[0], arrow.rgb[1], arrow.rgb[2]])
def addTestData():
d = DebugData()
# d.addSphere((0,0,0), radius=0.5)
d.addArrow((0,0,0), (0,0,1), color=[1,0,0])
d.addArrow((0,0,1), (0,.5,1), color=[0,1,0])
vis.showPolyData(d.getPolyData(), 'debug data', colorByName='RGB255')
view.resetCamera()
def newMesh():
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, pose=((0.5,0.0,1.0), (1,0,0,0)))
return affordanceManager.newAffordanceFromDescription(desc)
def onSpawnMesh(self):
d = DebugData()
d.addArrow((0,0,0), (0,0,0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
def onSpawnMesh(self):
d = DebugData()
d.addArrow((0, 0, 0), (0, 0, 0.3))
pd = d.getPolyData()
meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)
pose = transformUtils.poseFromTransform(self.getSpawnFrame())
desc = dict(classname='MeshAffordanceItem',
Name='mesh',
Filename=meshId,
uuid=newUUID(),
pose=pose)
return self.affordanceManager.newAffordanceFromDescription(desc)
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)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.contact_info:
point = np.array([
contact.contact_point[0], contact.contact_point[1],
contact.contact_point[2]
])
force = np.array([
contact.contact_force[0], contact.contact_force[1],
contact.contact_force[2]
])
mag = np.linalg.norm(force)
if mag > 1e-4:
mag = 0.3 / mag
key1 = (str(contact.body1_name), str(contact.body2_name))
key2 = (str(contact.body2_name), str(contact.body1_name))
if key1 in collision_pair_to_forces:
collision_pair_to_forces[key1].append(
(point, point + mag * force))
elif key2 in collision_pair_to_forces:
collision_pair_to_forces[key2].append(
(point, point + mag * force))
else:
collision_pair_to_forces[key1] = [(point, point + mag * force)]
for key, list_of_forces in collision_pair_to_forces.iteritems():
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(key),
parent=folder,
color=[0, 1, 0])
def doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and
self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1.*rft[1]
rft[3] = -1.*rft[3]
rft[4] = -1.*rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0/25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3]*scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1.,0.,0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def buildAccelArrow(center, accelThrust, roll, pitch, color=[1,0.3,0.0], alpha=0.8):
d = DebugData()
a_x = accelThrust*np.sin(pitch)
a_y = -accelThrust*np.cos(pitch)*np.sin(roll)
a_z = accelThrust*np.cos(pitch)*np.cos(roll)-9.8
end = [center[0] + a_x*0.125, center[1] + a_y*0.125, center[2] + a_z*0.125]
d.addArrow(center, end, tubeRadius=0.03, headRadius=0.15, color=color)
polyData = d.getPolyData()
#t = vtk.vtkTransform()
#t.Translate(center)
#polyData = filterUtils.transformPolyData(polyData, t)
obj = vis.updatePolyData(polyData, 'accel_arrow', color=color, alpha=alpha)
return polyData
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)
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.contact_info:
point = np.array([contact.contact_point[0],
contact.contact_point[1],
contact.contact_point[2]])
force = np.array([contact.contact_force[0],
contact.contact_force[1],
contact.contact_force[2]])
mag = np.linalg.norm(force)
if mag > 1e-4:
mag = 0.3 / mag
key1 = (str(contact.body1_name), str(contact.body2_name))
key2 = (str(contact.body2_name), str(contact.body1_name))
if key1 in collision_pair_to_forces:
collision_pair_to_forces[key1].append(
(point, point + mag * force))
elif key2 in collision_pair_to_forces:
collision_pair_to_forces[key2].append(
(point, point + mag * force))
else:
collision_pair_to_forces[key1] = [(point, point + mag * force)]
for key, list_of_forces in iteritems(collision_pair_to_forces):
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(
d.getPolyData(), str(key), parent=folder, color=[0, 1, 0])
def handle_message(self, msg):
print("frame channel was called")
if set(self._link_name_published) != set(msg.link_name):
# Removes the folder completely.
self.remove_folder()
self._link_name_published = msg.link_name
folder = self._get_folder()
for i in range(0, msg.num_links):
name = msg.link_name[i]
transform = transformUtils.transformFromPose(
msg.position[i], msg.quaternion[i])
# `vis.updateFrame` will either create or update the frame
# according to its name within its parent folder.
vis.updateFrame(transform, name, parent=folder, scale=0.1)
# Create map of body names to a list of contact forces
collision_pair_to_forces = {}
if msg.num_links > 1:
for i in range(1, msg.num_links):
name = msg.link_name[i]
# msg.position[i] is tuple and can be transformed into np array.
point1 = np.array(msg.position[i - 1])
point2 = np.array(msg.position[i])
collision_pair_to_forces[name] = [(point1, point2)]
for key, list_of_forces in iteritems(collision_pair_to_forces):
d = DebugData()
for force_pair in list_of_forces:
d.addArrow(start=force_pair[0],
end=force_pair[1],
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(key),
parent=folder,
color=[0, 1, 0])
def segmentTable(scenePolyData=None,
searchRadius=0.3,
visualize=True,
thickness=0.01,
pointOnTable=None,
pointAboveTable=None,
computeAboveTablePolyData=False):
"""
This requires two clicks using measurement panel. One on the table, one above the table on one of the objects. Call them point0, point1. Then we will attempt to fit a plane that passes through point0 with approximate normal point1 - point0
:param scenePolyData:
:param searchRadius:
:return:
"""
if scenePolyData is None:
scenePolyData = om.findObjectByName('reconstruction').polyData
assert scenePolyData is not None
assert pointOnTable is not None
assert pointAboveTable is not None
expectedNormal = pointAboveTable - pointOnTable
expectedNormal = expectedNormal / np.linalg.norm(expectedNormal)
polyData, normal = segmentation.applyPlaneFit(
scenePolyData,
searchOrigin=pointOnTable,
searchRadius=searchRadius,
expectedNormal=expectedNormal)
# get points above plane
abovePolyData = None
belowPolyData = None
if computeAboveTablePolyData:
abovePolyData = filterUtils.thresholdPoints(
polyData, 'dist_to_plane', [thickness / 2.0, np.inf])
belowPolyData = filterUtils.thresholdPoints(
polyData, 'dist_to_plane', [-np.inf, -thickness / 2.0])
# some debugging visualization
if visualize:
visFolder = om.getOrCreateContainer('debug')
if abovePolyData is not None:
vis.showPolyData(abovePolyData,
'above table segmentation',
color=[0, 1, 0],
parent=visFolder)
arrowLength = 0.3
headRadius = 0.02
d = DebugData()
visFolder = om.getOrCreateContainer('debug')
d.addArrow(pointOnTable,
pointOnTable + arrowLength * expectedNormal,
headRadius=headRadius)
vis.showPolyData(d.getPolyData(),
'expected normal',
color=[1, 0, 0],
parent=visFolder)
d = DebugData()
d.addArrow(pointOnTable,
pointOnTable + arrowLength * normal,
headRadius=headRadius)
vis.showPolyData(d.getPolyData(),
'computed normal',
color=[0, 1, 0],
parent=visFolder)
returnData = dict()
returnData['abovePolyData'] = abovePolyData
returnData['polyData'] = polyData
returnData['normal'] = normal
returnData['pointOnTable'] = pointOnTable
return returnData
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)
# The scale value attributable to auto-scale.
auto_scale = 1.0
max_force = -1
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.point_pair_contact_info:
point = np.array([
contact.contact_point[0], contact.contact_point[1],
contact.contact_point[2]
])
force = np.array([
contact.contact_force[0], contact.contact_force[1],
contact.contact_force[2]
])
mag = np.linalg.norm(force)
if mag < self.min_magnitude:
continue
if mag > max_force:
max_force = mag
scale = self.global_scale
if self.magnitude_mode == ContactVisModes.kFixedLength:
# mag must be > 0 otherwise this force would be skipped.
scale /= mag
vis_force = force * scale
key1 = (str(contact.body1_name), str(contact.body2_name))
key2 = (str(contact.body2_name), str(contact.body1_name))
if key1 in collision_pair_to_forces:
collision_pair_to_forces[key1].append((point, vis_force))
elif key2 in collision_pair_to_forces:
collision_pair_to_forces[key2].append((point, vis_force))
else:
collision_pair_to_forces[key1] = [(point, vis_force)]
if self.magnitude_mode == ContactVisModes.kAutoScale:
auto_scale = 1.0 / max_force
for key, list_of_forces in collision_pair_to_forces.items():
d = DebugData()
for p, v in list_of_forces:
d.addArrow(start=p,
end=p + auto_scale * v,
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(key),
parent=folder,
color=[0.2, 0.8, 0.2])
self.update_screen_text()
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)
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)) show(d, (2, 2, 0)) d = DebugData() d.addEllipsoid((0, 0, 0), radii=(0.5, 0.35, 0.2)) show(d, (4, 2, 0)) d = DebugData() d.addTorus(radius=0.5, thickness=0.2) show(d, (6, 2, 0)) d = DebugData() d.addCone(origin=(0,0,0), normal=(0,1,0), radius=0.3, height=0.8, color=[1, 1, 0])
def doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage')
and self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_hand_force + self.robotStateJointController.
lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1. * rft[
0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1. * rft[1]
rft[3] = -1. * rft[3]
rft[4] = -1. * rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(
self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_hand_force + self.robotStateJointController.
lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(
self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0 / 25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3] * scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1., 0., 0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet * np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame,
vis_names[i] + 'frame',
view=self.view,
parent='wristft',
visible=False,
scale=0.2)
vis.updatePolyData(d.getPolyData(),
vis_names[i],
view=self.view,
parent='wristft')
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()
# 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 drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
for step in folder.children():
om.removeFromObjectModel(step)
allTransforms = []
volFolder = getWalkingVolumesFolder()
map(om.removeFromObjectModel, volFolder.children())
slicesFolder = getTerrainSlicesFolder()
map(om.removeFromObjectModel, slicesFolder.children())
for i, footstep in enumerate(msg.footsteps):
trans = footstep.pos.translation
trans = [trans.x, trans.y, trans.z]
quat = footstep.pos.rotation
quat = [quat.w, quat.x, quat.y, quat.z]
footstepTransform = transformUtils.transformFromPose(trans, quat)
allTransforms.append(footstepTransform)
if i < 2:
continue
if footstep.is_right_foot:
mesh = getRightFootMesh()
if (right_color is None):
color = getRightFootColor()
else:
color = right_color
else:
mesh = getLeftFootMesh()
if (left_color is None):
color = getLeftFootColor()
else:
color = left_color
# add gradual shading to steps to indicate destination
frac = float(i)/ float(msg.num_steps-1)
this_color = [0,0,0]
this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
this_color[2] = 0.25*color[2] + 0.75*frac*color[2]
if self.show_contact_slices:
self.drawContactVolumes(footstepTransform, color)
contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
if footstep.is_right_foot:
sole_offset = np.mean(contact_pts_right, axis=0)
else:
sole_offset = np.mean(contact_pts_left, axis=0)
t_sole_prev = frameFromPositionMessage(msg.footsteps[i-2].pos)
t_sole_prev.PreMultiply()
t_sole_prev.Translate(sole_offset)
t_sole = transformUtils.copyFrame(footstepTransform)
t_sole.Translate(sole_offset)
yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
t_sole.GetPosition()[0] - t_sole_prev.GetPosition()[0])
T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
[0, 0, math.degrees(yaw)])
path_dist = np.array(footstep.terrain_path_dist)
height = np.array(footstep.terrain_height)
# if np.any(height >= trans[2]):
terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
d = DebugData()
for j in range(terrain_pts_in_local.shape[1]-1):
d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
obj.actor.SetUserTransform(T_terrain_to_world)
renderInfeasibility = False
if renderInfeasibility and footstep.infeasibility > 1e-6:
d = DebugData()
start = allTransforms[i-1].GetPosition()
end = footstepTransform.GetPosition()
d.addArrow(start, end, 0.02, 0.005,
startHead=True,
endHead=True)
vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])
stepName = 'step %d' % (i-1)
obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
obj.setIcon(om.Icons.Feet)
frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
obj.actor.SetUserTransform(footstepTransform)
obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
obj.properties.setPropertyIndex('Support Contact Groups', 0)
obj.footstep_index = i
obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))
self.drawContactPts(obj, footstep, color=this_color)
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)
# The scale value attributable to auto-scale.
auto_scale = 1.0
max_force = -1
# A map from pair of body names to a list of contact forces
collision_pair_to_forces = {}
for contact in msg.point_pair_contact_info:
point = np.array([
contact.contact_point[0], contact.contact_point[1],
contact.contact_point[2]
])
force = np.array([
contact.contact_force[0], contact.contact_force[1],
contact.contact_force[2]
])
mag = np.linalg.norm(force)
if mag < self.min_magnitude:
continue
if mag > max_force:
max_force = mag
scale = self.global_scale
if self.magnitude_mode == ContactVisModes.kFixedLength:
# mag must be > 0 otherwise this force would be skipped.
scale /= mag
vis_force = force * scale
# In point_pair_contact_info, the force is defined as the force
# *on body B*. That is what's placed in
# lcmt_point_pair_contact_info_for_viz.
# However, for two bodies (1, 2), MBP provides no guarantees which
# body is "body1" and which is "body2". Some contacts could be
# reported as (1, 2) and some as (2, 1). In order to aggregate all
# such interactions into a single debug artifact, we need to
# reconcile the ordering.
force_format = "Force on {} from {}"
key = force_format.format(contact.body2_name, contact.body1_name)
alt_key = force_format.format(contact.body1_name,
contact.body2_name)
if key in collision_pair_to_forces:
collision_pair_to_forces[key].append((point, vis_force))
elif alt_key in collision_pair_to_forces:
collision_pair_to_forces[alt_key].append((point, -vis_force))
else:
collision_pair_to_forces[key] = [(point, vis_force)]
if self.magnitude_mode == ContactVisModes.kAutoScale:
auto_scale = 1.0 / max_force
for key, list_of_forces in collision_pair_to_forces.items():
d = DebugData()
for p, v in list_of_forces:
d.addArrow(start=p,
end=p + auto_scale * v,
tubeRadius=0.005,
headRadius=0.01)
vis.showPolyData(d.getPolyData(),
str(key),
parent=folder,
color=[0.2, 0.8, 0.2])
self.update_screen_text()
