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
