def _get_shape_geometry(self, shape): visual_mesh = shape.get_user_data().get('visual_mesh', None) if shape.get_user_data() is not None else None if visual_mesh is not None: return g.TriangularMeshGeometry(vertices=visual_mesh.vertices, faces=visual_mesh.faces) elif shape.get_geometry_type() == GeometryType.CONVEXMESH: data = shape.get_shape_data() # N x 9 - i.e. 3 triangles faces = np.arange(0, data.shape[0] * 3, 1, dtype=np.int).reshape(-1, 3) return g.TriangularMeshGeometry(vertices=data.reshape(-1, 3), faces=faces) elif shape.get_geometry_type() == GeometryType.SPHERE: return g.Sphere(radius=shape.get_sphere_radius()) elif shape.get_geometry_type() == GeometryType.BOX: return g.Box((2 * shape.get_box_half_extents()).tolist()) else: raise NotImplementedError("Not supported geometry type.")
def plot_surface(meshcat, X, Y, Z, color=0xdd9999, wireframe=False): (rows, cols) = Z.shape vertices = np.empty((rows * cols, 3), dtype=np.float32) vertices[:, 0] = X.reshape((-1)) vertices[:, 1] = Y.reshape((-1)) vertices[:, 2] = Z.reshape((-1)) # Vectorized faces code from https://stackoverflow.com/questions/44934631/making-grid-triangular-mesh-quickly-with-numpy # noqa faces = np.empty((rows - 1, cols - 1, 2, 3), dtype=np.uint32) r = np.arange(rows * cols).reshape(rows, cols) faces[:, :, 0, 0] = r[:-1, :-1] faces[:, :, 1, 0] = r[:-1, 1:] faces[:, :, 0, 1] = r[:-1, 1:] faces[:, :, 1, 1] = r[1:, 1:] faces[:, :, :, 2] = r[1:, :-1, None] faces.shape = (-1, 3) if isinstance(color, int): meshcat.set_object( g.TriangularMeshGeometry(vertices, faces), g.MeshLambertMaterial(color=color, wireframe=wireframe)) else: meshcat.set_object( TriangularMeshGeometry(vertices, faces, color), g.MeshLambertMaterial(vertexColors=True, wireframe=wireframe))
def loadBVH(bvh): import meshcat.geometry as mg num_vertices = bvh.num_vertices num_tris = bvh.num_tris vertices = np.empty((num_vertices,3)) faces = np.empty((num_tris,3),dtype=int) for k in range(num_tris): tri = bvh.tri_indices(k) faces[k] = [tri[i] for i in range(3)] for k in range(num_vertices): vert = bvh.vertices(k) vertices[k] = vert vertices = vertices.astype(np.float32) if num_tris > 0: mesh = mg.TriangularMeshGeometry(vertices, faces) else: mesh = mg.Points( mg.PointsGeometry(vertices.T, color=np.repeat(np.ones((3,1)),num_vertices,axis=1)), mg.PointsMaterial(size=0.002)) return mesh
def runTest(self): """ Test that we can render meshes from raw vertices and faces as numpy arrays """ v = self.vis["triangular_mesh"] v.set_transform(tf.rotation_matrix(np.pi / 2, [0., 0, 1])) vertices = np.array([[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]]) faces = np.array([[0, 1, 2], [3, 0, 2]]) v.set_object(g.TriangularMeshGeometry(vertices, faces), g.MeshLambertMaterial(color=0xeedd22, wireframe=True)) v = self.vis["triangular_mesh_w_vertex_coloring"] v.set_transform( tf.translation_matrix([1, 0, 0]).dot( tf.rotation_matrix(np.pi / 2, [0, 0, 1]))) colors = vertices v.set_object(g.TriangularMeshGeometry(vertices, faces, colors), g.MeshLambertMaterial(vertexColors=True, wireframe=True))
def loadMesh(mesh): import meshcat.geometry as mg if isinstance(mesh, hppfcl.BVHModelBase): num_vertices = mesh.num_vertices num_tris = mesh.num_tris call_triangles = mesh.tri_indices call_vertices = mesh.vertices elif isinstance(mesh, hppfcl.Convex): num_vertices = mesh.num_points num_tris = mesh.num_polygons call_triangles = mesh.polygons call_vertices = mesh.points faces = np.empty((num_tris, 3), dtype=int) for k in range(num_tris): tri = call_triangles(k) faces[k] = [tri[i] for i in range(3)] if LooseVersion(hppfcl.__version__) >= LooseVersion("1.7.7"): vertices = call_vertices() else: vertices = np.empty((num_vertices, 3)) for k in range(num_vertices): vertices[k] = call_vertices(k) vertices = vertices.astype(np.float32) if num_tris > 0: mesh = mg.TriangularMeshGeometry(vertices, faces) else: mesh = mg.Points( mg.PointsGeometry(vertices.T, color=np.repeat(np.ones((3, 1)), num_vertices, axis=1)), mg.PointsMaterial(size=0.002)) return mesh
def _get_shape_geometry(self, shape): visual_mesh = shape.get_user_data().get( 'visual_mesh', None) if shape.get_user_data() is not None else None if visual_mesh is not None: try: exp_obj = trimesh.exchange.obj.export_obj(visual_mesh) except ValueError: exp_obj = trimesh.exchange.obj.export_obj( visual_mesh, include_texture=False) return g.ObjMeshGeometry.from_stream( trimesh.util.wrap_as_stream(exp_obj)) elif shape.get_geometry_type() == GeometryType.CONVEXMESH: data = shape.get_shape_data() # N x 9 - i.e. 3 triangles faces = np.arange(0, data.shape[0] * 3, 1, dtype=np.int).reshape(-1, 3) return g.TriangularMeshGeometry(vertices=data.reshape(-1, 3), faces=faces) elif shape.get_geometry_type() == GeometryType.SPHERE: return g.Sphere(radius=shape.get_sphere_radius()) elif shape.get_geometry_type() == GeometryType.BOX: return g.Box((2 * shape.get_box_half_extents()).tolist()) else: raise NotImplementedError("Not supported geometry type.")
def loadMesh(mesh): import meshcat.geometry as mg if isinstance(mesh, hppfcl.HeightFieldOBBRSS): heights = mesh.getHeights() x_grid = mesh.getXGrid() y_grid = mesh.getYGrid() min_height = mesh.getMinHeight() X, Y = np.meshgrid(x_grid, y_grid) nx = len(x_grid) - 1 ny = len(y_grid) - 1 num_cells = (nx) * (ny) * 2 + (nx + ny) * 4 + 2 num_vertices = X.size num_tris = num_cells faces = np.empty((num_tris, 3), dtype=int) vertices = np.vstack( (np.stack((X.reshape(num_vertices), Y.reshape(num_vertices), heights.reshape(num_vertices)), axis=1), np.stack((X.reshape(num_vertices), Y.reshape(num_vertices), np.full(num_vertices, min_height)), axis=1))) face_id = 0 for y_id in range(ny): for x_id in range(nx): p0 = x_id + y_id * (nx + 1) p1 = p0 + 1 p2 = p1 + nx + 1 p3 = p2 - 1 faces[face_id] = np.array([p0, p3, p1]) face_id += 1 faces[face_id] = np.array([p3, p2, p1]) face_id += 1 if y_id == 0: p0_low = p0 + num_vertices p1_low = p1 + num_vertices faces[face_id] = np.array([p0, p1_low, p0_low]) face_id += 1 faces[face_id] = np.array([p0, p1, p1_low]) face_id += 1 if y_id == ny - 1: p2_low = p2 + num_vertices p3_low = p3 + num_vertices faces[face_id] = np.array([p3, p3_low, p2_low]) face_id += 1 faces[face_id] = np.array([p3, p2_low, p2]) face_id += 1 if x_id == 0: p0_low = p0 + num_vertices p3_low = p3 + num_vertices faces[face_id] = np.array([p0, p3_low, p3]) face_id += 1 faces[face_id] = np.array([p0, p0_low, p3_low]) face_id += 1 if x_id == nx - 1: p1_low = p1 + num_vertices p2_low = p2 + num_vertices faces[face_id] = np.array([p1, p2_low, p2]) face_id += 1 faces[face_id] = np.array([p1, p1_low, p2_low]) face_id += 1 # Last face p0 = num_vertices p1 = p0 + nx p2 = 2 * num_vertices - 1 p3 = p2 - nx faces[face_id] = np.array([p0, p1, p2]) face_id += 1 faces[face_id] = np.array([p0, p2, p3]) face_id += 1 elif isinstance(mesh, (hppfcl.Convex, hppfcl.BVHModelBase)): if isinstance(mesh, hppfcl.BVHModelBase): num_vertices = mesh.num_vertices num_tris = mesh.num_tris call_triangles = mesh.tri_indices call_vertices = mesh.vertices elif isinstance(mesh, hppfcl.Convex): num_vertices = mesh.num_points num_tris = mesh.num_polygons call_triangles = mesh.polygons call_vertices = mesh.points faces = np.empty((num_tris, 3), dtype=int) for k in range(num_tris): tri = call_triangles(k) faces[k] = [tri[i] for i in range(3)] if LooseVersion(hppfcl.__version__) >= LooseVersion("1.7.7"): vertices = call_vertices() else: vertices = np.empty((num_vertices, 3)) for k in range(num_vertices): vertices[k] = call_vertices(k) vertices = vertices.astype(np.float32) if num_tris > 0: mesh = mg.TriangularMeshGeometry(vertices, faces) else: mesh = mg.Points( mg.PointsGeometry(vertices.T, color=np.repeat(np.ones((3, 1)), num_vertices, axis=1)), mg.PointsMaterial(size=0.002)) return mesh