def assimpTextureCoordsToArray(tcoords): if tcoords.shape[1] >= 3 and not tcoords[:,2].any(): tcoords = tcoords[:,:2].copy() tcoordArray = vnp.getVtkFromNumpy(tcoords) tcoordArray.SetName('tcoords') return tcoordArray
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
def decodePolyData(data): '''Given a numpy int8 array, deserializes the data to construct a new vtkPolyData object and returns the result.''' if not hasattr(vtk, 'vtkCommunicator'): r = vtk.vtkPolyDataReader() r.ReadFromInputStringOn() r.SetInputString(str(data.data)) r.Update() return shallowCopy(r.GetOutput()) charArray = vnp.getVtkFromNumpy(data) assert isinstance(charArray, vtk.vtkCharArray) polyData = vtk.vtkPolyData() vtk.vtkCommunicator.UnMarshalDataObject(charArray, polyData) return polyData
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
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
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
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Though strangely named, DebugData() is the object through which # drawing is done in DrakeVisualizer. d = DebugData() # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip = -1 # TODO(sean-curtis-TRI) Remove the following comment when this # code can be exercised. # The following code is not exercised presently because the # magnitude mode is always set to kFixedLength. # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: if self.show_spatial_force: for surface in msg.hydroelastic_contacts: force = np.array([surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2]]) moment = np.array([surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2]]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2]]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2]]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. auto_force_scale = 1.0 / max_force auto_moment_scale = 1.0 / max_moment auto_traction_scale = 1.0 / max_traction auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. for surface in msg.hydroelastic_contacts: view = applogic.getCurrentRenderView() # Keep track if any DebugData is written to. # Necessary to keep DrakeVisualizer from spewing messages to the # console when no DebugData is sent to director. has_debug_data = False # Draw the spatial force. if self.show_spatial_force: point = np.array([surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2]]) force = np.array([surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2]]) moment = np.array([surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2]]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag d.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.005, headRadius=0.01, color=[1, 0, 0]) has_debug_data = True # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag d.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.005, headRadius=0.01, color=[0, 0, 1]) has_debug_data = True # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: # Arrows and/or spheres are drawn through debug data if there # exists a quadrature point. if surface.num_quadrature_points > 0: has_debug_data = True for quad_point_data in surface.quadrature_point_data: origin = np.array([quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2]]) if self.show_traction_vectors: traction = np.array([quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2]]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2]]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale d.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: d.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) # Send everything except pressure and contact edges to director. if has_debug_data: item_name = '{}, {}'.format( surface.body1_name, surface.body2_name) cls = vis.PolyDataItem item = cls(item_name, d.getPolyData(), view) om.addToObjectModel(item, folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) # Coloring for force and moment vectors. item.colorBy('RGB255') if self.show_pressure or self.show_contact_edges: pos, pos_above, pos_below, uvs, tri_mesh, seg_mesh = \ self.process_triangles(surface) if self.show_pressure and len(tri_mesh) > 0: # Copy data to VTK objects. vtk_uvs = vnp.getVtkFromNumpy(uvs) vtk_tris_above = vtk.vtkCellArray() vtk_tris_below = vtk.vtkCellArray() vtk_tris_above.Allocate(len(tri_mesh)) vtk_tris_below.Allocate(len(tri_mesh)) for tri in tri_mesh: vtk_tris_above.InsertNextCell(3, tri) vtk_tris_below.InsertNextCell(3, tri) vtk_polydata_tris_above = vtk.vtkPolyData() vtk_polydata_tris_above.SetPoints( vnp.getVtkPointsFromNumpy(pos_above)) vtk_polydata_tris_above.SetPolys(vtk_tris_above) vtk_polydata_tris_above.GetPointData().SetTCoords(vtk_uvs) vtk_polydata_tris_below = vtk.vtkPolyData() vtk_polydata_tris_below.SetPoints( vnp.getVtkPointsFromNumpy(pos_below)) vtk_polydata_tris_below.SetPolys(vtk_tris_below) vtk_polydata_tris_below.GetPointData().SetTCoords(vtk_uvs) vtk_mapper_above = vtk.vtkPolyDataMapper() vtk_mapper_above.SetInputData(vtk_polydata_tris_above) vtk_mapper_below = vtk.vtkPolyDataMapper() vtk_mapper_below.SetInputData(vtk_polydata_tris_below) # Feed VTK objects into director. item_name = 'Pressure between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item_above = vis.PolyDataItem( item_name, vtk_polydata_tris_above, view) polydata_item_above.actor.SetMapper(vtk_mapper_above) polydata_item_above.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item_above, folder) item_name = 'Pressure between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item_below = vis.PolyDataItem( item_name, vtk_polydata_tris_below, view) polydata_item_below.actor.SetMapper(vtk_mapper_below) polydata_item_below.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item_below, folder) if self.show_contact_edges and len(seg_mesh) > 0: # Copy data to VTK objects. vtk_segs = vtk.vtkCellArray() vtk_segs.Allocate(len(seg_mesh)) for seg in seg_mesh: vtk_segs.InsertNextCell(2, seg) vtk_polydata_segs = vtk.vtkPolyData() vtk_polydata_segs.SetPoints( vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_segs.SetLines(vtk_segs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_segs) vtk_mapper.Update() # Feed VTK objects into director. item_name = 'Contact edges between {}, {}'.format( surface.body1_name, surface.body2_name) polydata_item = vis.PolyDataItem( item_name, vtk_polydata_segs, view) polydata_item.actor.SetMapper(vtk_mapper) [r, g, b] = color_map.get_contrasting_color() contrasting_color = [r*255, g*255, b*255] polydata_item.actor.GetProperty().SetColor(contrasting_color) om.addToObjectModel(polydata_item, folder)
def handle_message(self, msg): # Limits the rate of message handling, since redrawing is done in the # message handler. self._sub.setSpeedLimit(30) # Removes the folder completely. om.removeFromObjectModel(om.findObjectByName(self._folder_name)) # Recreates folder. folder = om.getOrCreateContainer(self._folder_name) # Set the color map. color_map = self.create_color_map() # The scale value attributable to auto-scale. auto_force_scale = 1.0 auto_moment_scale = 1.0 auto_traction_scale = 1.0 auto_slip_velocity_scale = 1.0 max_force = -1 max_moment = -1 max_traction = -1 max_slip_speed = -1 # Determine scaling magnitudes if autoscaling is activated. if self.magnitude_mode == ContactVisModes.kAutoScale: for surface in msg.hydroelastic_contacts: if self.show_spatial_force: force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) if force_mag > max_force: max_force = force_mag if moment_mag > max_moment: max_moment = moment_mag # Prepare scaling information for the traction vectors. if self.show_traction_vectors: for quad_point_data in surface.quadrature_point_data: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) max_traction = max(max_traction, np.linalg.norm(traction)) # Prepare scaling information for the slip velocity vectors. if self.show_slip_velocity_vectors: for quad_point_data in surface.quadrature_point_data: slip_speed = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) max_slip_speed = max(max_slip_speed, np.linalg.norm(slip_speed)) # Compute scaling factors. We don't want division by zero. # We don't want division by negative numbers. if max_force > 0: auto_force_scale = 1.0 / max_force if max_moment > 0: auto_moment_scale = 1.0 / max_moment if max_traction > 0: auto_traction_scale = 1.0 / max_traction if max_slip_speed > 0: auto_slip_velocity_scale = 1.0 / max_slip_speed # TODO(drum) Consider exiting early if no visualization options are # enabled. view = applogic.getCurrentRenderView() for surface in msg.hydroelastic_contacts: contact_data_folder = om.getOrCreateContainer( f'Contact data between {surface.body1_name} and ' f'{surface.body2_name}', folder) # Adds a collection of debug data to the console with the given # item name. def add_contact_data(data, item_name): # Exploit the fact that data.append is a vtkAppendPolyData # instance. The number of input connections on port zero is the # number of *actual* geometries added. If zero have been added, # do no work. if (data is None or data.append.GetNumberOfInputConnections(0) == 0): return item = vis.PolyDataItem(item_name, data.getPolyData(), view) om.addToObjectModel(item, contact_data_folder) item.setProperty('Visible', True) item.setProperty('Alpha', 1.0) item.colorBy('RGB255') # Draw the spatial force. if self.show_spatial_force: force_data = DebugData() point = np.array([ surface.centroid_W[0], surface.centroid_W[1], surface.centroid_W[2] ]) force = np.array([ surface.force_C_W[0], surface.force_C_W[1], surface.force_C_W[2] ]) moment = np.array([ surface.moment_C_W[0], surface.moment_C_W[1], surface.moment_C_W[2] ]) force_mag = np.linalg.norm(force) moment_mag = np.linalg.norm(moment) # Draw the force arrow if it's of sufficient magnitude. if force_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this force would be # skipped. scale /= force_mag force_data.addArrow(start=point, end=point + auto_force_scale * force * scale, tubeRadius=0.001, headRadius=0.002, color=[1, 0, 0]) # Draw the moment arrow if it's of sufficient magnitude. if moment_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode == ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this moment would be # skipped. scale /= moment_mag force_data.addArrow(start=point, end=point + auto_moment_scale * moment * scale, tubeRadius=0.001, headRadius=0.002, color=[0, 0, 1]) add_contact_data(force_data, "Spatial force") # Iterate over all quadrature points, drawing traction and slip # velocity vectors. if self.show_traction_vectors or self.show_slip_velocity_vectors: traction_data = DebugData() slip_data = DebugData() for quad_point_data in surface.quadrature_point_data: origin = np.array([ quad_point_data.p_WQ[0], quad_point_data.p_WQ[1], quad_point_data.p_WQ[2] ]) if self.show_traction_vectors: traction = np.array([ quad_point_data.traction_Aq_W[0], quad_point_data.traction_Aq_W[1], quad_point_data.traction_Aq_W[2] ]) traction_mag = np.linalg.norm(traction) # Draw the arrow only if it's of sufficient magnitude. if traction_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this traction # would be skipped. scale /= traction_mag offset = auto_traction_scale * traction * scale traction_data.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[1, 0, 1]) else: traction_data.addSphere(center=origin, radius=0.000125, color=[1, 0, 1]) if self.show_slip_velocity_vectors: slip = np.array([ quad_point_data.vt_BqAq_W[0], quad_point_data.vt_BqAq_W[1], quad_point_data.vt_BqAq_W[2] ]) slip_mag = np.linalg.norm(slip) # Draw the arrow only if it's of sufficient magnitude. if slip_mag > self.min_magnitude: scale = self.global_scale if self.magnitude_mode ==\ ContactVisModes.kFixedLength: # magnitude must be > 0 otherwise this slip # vector would be skipped. scale /= slip_mag offset = auto_slip_velocity_scale * slip * scale slip_data.addArrow(start=origin, end=origin + offset, tubeRadius=0.000125, headRadius=0.00025, color=[0, 1, 1]) else: slip_data.addSphere(center=origin, radius=0.000125, color=[0, 1, 1]) add_contact_data(traction_data, "Traction") add_contact_data(slip_data, "Slip velocity") if self.show_pressure or self.show_contact_edges: pos, uvs, tri_mesh, seg_mesh = \ self.process_triangles(surface) if self.show_pressure and len(tri_mesh) > 0: # Copy data to VTK objects. vtk_uvs = vnp.getVtkFromNumpy(uvs) vtk_tris = vtk.vtkCellArray() vtk_tris.Allocate(len(tri_mesh)) for tri in tri_mesh: vtk_tris.InsertNextCell(3, tri) vtk_polydata_tris = vtk.vtkPolyData() vtk_polydata_tris.SetPoints(vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_tris.SetPolys(vtk_tris) vtk_polydata_tris.GetPointData().SetTCoords(vtk_uvs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_tris) # Feed VTK objects into director. item_name = 'Contact surface' polydata_item = vis.PolyDataItem(item_name, vtk_polydata_tris, view) polydata_item.actor.SetMapper(vtk_mapper) polydata_item.actor.SetTexture(self.texture) om.addToObjectModel(polydata_item, contact_data_folder) if self.show_contact_edges and len(seg_mesh) > 0: # Copy data to VTK objects. vtk_segs = vtk.vtkCellArray() vtk_segs.Allocate(len(seg_mesh)) for seg in seg_mesh: vtk_segs.InsertNextCell(2, seg) vtk_polydata_segs = vtk.vtkPolyData() vtk_polydata_segs.SetPoints(vnp.getVtkPointsFromNumpy(pos)) vtk_polydata_segs.SetLines(vtk_segs) vtk_mapper = vtk.vtkPolyDataMapper() vtk_mapper.SetInputData(vtk_polydata_segs) vtk_mapper.Update() # Feed VTK objects into director. item_name = 'Mesh edges' polydata_item = vis.PolyDataItem(item_name, vtk_polydata_segs, view) polydata_item.actor.SetMapper(vtk_mapper) [r, g, b] = color_map.get_contrasting_color() contrasting_color = [r * 255, g * 255, b * 255] polydata_item.actor.GetProperty().SetColor(contrasting_color) om.addToObjectModel(polydata_item, contact_data_folder)