def handle_init_message(self, msg):
        """Creates the polydata for the deformable mesh specified in msg."""
        folder = om.getOrCreateContainer(self._folder_name)
        for poly_item in self._poly_item_list:
            om.removeFromObjectModel(poly_item)

        self._names = []
        self._vertex_counts = []
        self._poly_data_list = []
        self._poly_item_list = []
        for mesh in msg.meshes:
            self._names.append(mesh.name)
            # Initial vertex positions are garbage; meaningful values will be
            # set by the update message.
            points = vtk.vtkPoints()
            self._vertex_counts.append(mesh.num_vertices)
            for i in range(mesh.num_vertices):
                points.InsertNextPoint(0, 0, 0)
            triangles = vtk.vtkCellArray()
            for tri in mesh.tris:
                triangles.InsertNextCell(3)
                for i in tri.vertices:
                    triangles.InsertCellPoint(i)
            poly_data = vtk.vtkPolyData()
            poly_data.SetPoints(points)
            poly_data.SetPolys(triangles)
            poly_item = vis.showPolyData(poly_data, mesh.name, parent=folder)
            poly_item.setProperty("Surface Mode", "Surface with edges")
            self._poly_data_list.append(poly_data)
            self._poly_item_list.append(poly_item)
Ejemplo n.º 2
0
    def add_vtk_polygon(vertices):
        """
        Create a 3d polygon data with vtk.
        :type vertices: vertices data of polygon
        """

        # Setup four points
        points = vtk.vtkPoints()
        for point in vertices:
            points.InsertNextPoint(point[0], point[1], point[2])

        # Create the polygon
        polygon = vtk.vtkPolygon()
        polygon.GetPointIds().SetNumberOfIds(4)  # make a quad
        polygon.GetPointIds().SetId(0, 0)
        polygon.GetPointIds().SetId(1, 1)
        polygon.GetPointIds().SetId(2, 2)
        polygon.GetPointIds().SetId(3, 3)

        # Add the polygon to a list of polygons
        polygons = vtk.vtkCellArray()
        polygons.InsertNextCell(polygon)

        # Create a PolyData
        polygon_poly_data = vtk.vtkPolyData()
        polygon_poly_data.SetPoints(points)
        polygon_poly_data.SetPolys(polygons)
        return polygon_poly_data
Ejemplo n.º 3
0
 def getTraceData(self):
     t = self.frame.findChild(self.traceName)
     if not t:
         pts = vtk.vtkPoints()
         pts.SetDataTypeToDouble()
         pts.InsertNextPoint(self.frame.transform.GetPosition())
         pd = vtk.vtkPolyData()
         pd.SetPoints(pts)
         pd.SetLines(vtk.vtkCellArray())
         t = showPolyData(pd, self.traceName, parent=self.frame)
     return t
Ejemplo n.º 4
0
 def getTraceData(self):
     t = self.frame.findChild(self.traceName)
     if not t:
         pts = vtk.vtkPoints()
         pts.SetDataTypeToDouble()
         pts.InsertNextPoint(self.frame.transform.GetPosition())
         pd = vtk.vtkPolyData()
         pd.SetPoints(pts)
         pd.SetLines(vtk.vtkCellArray())
         t = showPolyData(pd, self.traceName, parent=self.frame)
     return t
    def createPolyDataFromMeshArrays(pts, faces):
        pd = vtk.vtkPolyData()
        pd.SetPoints(vtk.vtkPoints())
        pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))

        assert len(faces) % 3 == 0
        cells = vtk.vtkCellArray()
        for i in xrange(len(faces) / 3):
            tri = vtk.vtkTriangle()
            tri.GetPointIds().SetId(0, faces[i * 3 + 0])
            tri.GetPointIds().SetId(1, faces[i * 3 + 1])
            tri.GetPointIds().SetId(2, faces[i * 3 + 2])
            cells.InsertNextCell(tri)

        pd.SetPolys(cells)
        return pd
Ejemplo n.º 6
0
    def createPolyDataFromMeshArrays(pts, faces):
        pd = vtk.vtkPolyData()
        pd.SetPoints(vtk.vtkPoints())
        pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))

        assert len(faces) % 3 == 0
        cells = vtk.vtkCellArray()
        for i in xrange(len(faces) / 3):
            tri = vtk.vtkTriangle()
            tri.GetPointIds().SetId(0, faces[i * 3 + 0])
            tri.GetPointIds().SetId(1, faces[i * 3 + 1])
            tri.GetPointIds().SetId(2, faces[i * 3 + 2])
            cells.InsertNextCell(tri)

        pd.SetPolys(cells)
        return pd
Ejemplo n.º 7
0
    def createPolyDataFromMeshArrays(pts, faces):
        pd = vtk.vtkPolyData()
        pd.SetPoints(vtk.vtkPoints())
        pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))

        cells = vtk.vtkCellArray()
        for face in faces:
            assert len(face) == 3, "Non-triangular faces are not supported."
            tri = vtk.vtkTriangle()
            tri.GetPointIds().SetId(0, face[0])
            tri.GetPointIds().SetId(1, face[1])
            tri.GetPointIds().SetId(2, face[2])
            cells.InsertNextCell(tri)

        pd.SetPolys(cells)
        return pd
Ejemplo n.º 8
0
    def onPlanarLidar(self, msg, channel):

        linkName = channel.replace('DRAKE_PLANAR_LIDAR_', '', 1)
        robotNum, linkName = linkName.split('_', 1)
        robotNum = int(robotNum)

        try:
            link = self.getLink(robotNum, linkName)
        except KeyError:
            if linkName not in self.linkWarnings:
                print 'Error locating link name:', linkName
                self.linkWarnings.add(linkName)
        else:
            if len(link.geometry):
                polyData = link.geometry[0].polyDataItem
            else:
                polyData = vtk.vtkPolyData()

                name = linkName + ' geometry data'
                visInfo = FieldContainer(color=[1.0, 0.0, 0.0],
                                         alpha=1.0,
                                         texture=None)
                g = Geometry(name, polyData, visInfo)
                link.geometry.append(g)

                linkFolder = self.getLinkFolder(robotNum, linkName)
                self.addLinkGeometry(g, linkName, linkFolder)
                g.polyDataItem.actor.SetUserTransform(link.transform)

            points = vtk.vtkPoints()
            verts = vtk.vtkCellArray()

            t = msg.rad0
            for r in msg.ranges:
                if r >= 0:
                    x = r * math.cos(t)
                    y = r * math.sin(t)

                    pointId = points.InsertNextPoint([x, y, 0])
                    verts.InsertNextCell(1)
                    verts.InsertCellPoint(pointId)

                t += msg.radstep

            polyData.SetPoints(points)
            polyData.SetVerts(verts)
Ejemplo n.º 9
0
    def onPlanarLidar(self, msg, channel):

        linkName = channel.replace("DRAKE_PLANAR_LIDAR_", "", 1)
        robotNum, linkName = linkName.split("_", 1)
        robotNum = int(robotNum)

        try:
            link = self.getLink(robotNum, linkName)
        except KeyError:
            if linkName not in self.linkWarnings:
                print "Error locating link name:", linkName
                self.linkWarnings.add(linkName)
        else:
            if len(link.geometry):
                polyData = link.geometry[0].polyDataItem
            else:
                polyData = vtk.vtkPolyData()

                name = linkName + " geometry data"
                geom = type("", (), {})
                geom.color = [1.0, 0.0, 0.0, 1.0]
                g = Geometry(name, geom, polyData, link.transform)
                link.geometry.append(g)

                linkFolder = self.getLinkFolder(robotNum, linkName)
                self.addLinkGeometry(g, linkName, linkFolder)
                g.polyDataItem.actor.SetUserTransform(link.transform)

            points = vtk.vtkPoints()
            verts = vtk.vtkCellArray()

            t = msg.rad0
            for r in msg.ranges:
                if r >= 0:
                    x = r * math.cos(t)
                    y = r * math.sin(t)

                    pointId = points.InsertNextPoint([x, y, 0])
                    verts.InsertNextCell(1)
                    verts.InsertCellPoint(pointId)

                t += msg.radstep

            polyData.SetPoints(points)
            polyData.SetVerts(verts)
Ejemplo n.º 10
0
    def onPlanarLidar(self, msg, channel):

        linkName = channel.replace('DRAKE_PLANAR_LIDAR_', '', 1)
        robotNum, linkName = linkName.split('_', 1)
        robotNum = int(robotNum)

        try:
            link = self.getLink(robotNum, linkName)
        except KeyError:
            if linkName not in self.linkWarnings:
                print 'Error locating link name:', linkName
                self.linkWarnings.add(linkName)
        else:
            if len(link.geometry):
                polyData = link.geometry[0].polyDataItem
            else:
                polyData = vtk.vtkPolyData()

                name = linkName + ' geometry data'
                visInfo = FieldContainer(color=[1.0,0.0,0.0], alpha=1.0, texture=None)
                g = Geometry(name, polyData, visInfo)
                link.geometry.append(g)

                linkFolder = self.getLinkFolder(robotNum, linkName)
                self.addLinkGeometry(g, linkName, linkFolder)
                g.polyDataItem.actor.SetUserTransform(link.transform)

            points = vtk.vtkPoints()
            verts = vtk.vtkCellArray()

            t = msg.rad0
            for r in msg.ranges:
                if r >= 0:
                    x = r * math.cos(t)
                    y = r * math.sin(t)

                    pointId = points.InsertNextPoint([x,y,0])
                    verts.InsertNextCell(1)
                    verts.InsertCellPoint(pointId)

                t += msg.radstep

            polyData.SetPoints(points)
            polyData.SetVerts(verts)
Ejemplo n.º 11
0
def assimpMeshToPolyData(mesh):

    verts = mesh.vertices
    faces = mesh.faces

    nfaces = faces.shape[0]
    nverts = verts.shape[0]

    assert verts.shape[1] == 3
    assert faces.shape[1] == 3

    points = vnp.getVtkPointsFromNumpy(verts)

    cells = vtk.vtkCellArray()

    for i in xrange(nfaces):
        face = faces[i]
        tri = vtk.vtkTriangle()
        tri.GetPointIds().SetId(0, face[0])
        tri.GetPointIds().SetId(1, face[1])
        tri.GetPointIds().SetId(2, face[2])
        cells.InsertNextCell(tri)

    polyData = vtk.vtkPolyData()
    polyData.SetPoints(points)
    polyData.SetPolys(cells)


    if mesh.normals.shape[0] > 0:
        assert mesh.normals.shape[0] == nverts
        normals = vnp.getVtkFromNumpy(mesh.normals)
        normals.SetName('normals')
        polyData.GetPointData().AddArray(normals)
        polyData.GetPointData().SetNormals(normals)


    for i, tcoords in enumerate(mesh.texturecoords):
        tcoordArray = assimpTextureCoordsToArray(tcoords)
        tcoordArray.SetName('tcoords_%d' % i)
        polyData.GetPointData().AddArray(tcoordArray)
        if i == 0:
            polyData.GetPointData().SetTCoords(tcoordArray)

    return polyData
Ejemplo n.º 12
0
    def createPolyDataFromMeshArrays(pts, faces):
        pd = vtk.vtkPolyData()
        pd.SetPoints(vtk.vtkPoints())

        if pts.size > 0:
            pd.GetPoints().SetData(vnp.getVtkFromNumpy(pts.copy()))

            cells = vtk.vtkCellArray()
            tri = vtk.vtkTriangle()
            setId = tri.GetPointIds().SetId  # bind the method for convenience
            for face in faces:
                assert len(face) == 3, "Non-triangular faces are not supported."
                setId(0, face[0])
                setId(1, face[1])
                setId(2, face[2])
                cells.InsertNextCell(tri)

            pd.SetPolys(cells)
        return pd
Ejemplo n.º 13
0
    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)
Ejemplo n.º 14
0
    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)