def add_frame(self, T_frame, frame_name='frame_0', frame_scale=1.0):
# type: (np.ndarray, str, float) -> None
"""
:param T_frame: 4x4 np.ndarray that is the frame expressed in world frame
:param frame_name:
:param frame_scale: The length of the frame line
:return:
"""
# Build the vertices
vertices_x = np.zeros(shape=(3, 2))
vertices_x[:, 0] = T_frame[0:3, 3]
vertices_x[:, 1] = vertices_x[:, 0] + frame_scale * T_frame[0:3, 0]
vertices_y = np.zeros(shape=(3, 2))
vertices_y[:, 0] = T_frame[0:3, 3]
vertices_y[:, 1] = vertices_y[:, 0] + frame_scale * T_frame[0:3, 1]
vertices_z = np.zeros(shape=(3, 2))
vertices_z[:, 0] = T_frame[0:3, 3]
vertices_z[:, 1] = vertices_z[:, 0] + frame_scale * T_frame[0:3, 2]
# Send to meshcat
line_x_name = frame_name + 'x_axis'
line_y_name = frame_name + 'y_axis'
line_z_name = frame_name + 'z_axis'
self._meshcat_vis[self._prefix][line_x_name].set_object(geometry=Line(
meshcat_geometry.PointsGeometry(vertices_x), meshcat_geometry.MeshBasicMaterial(color=0xff0000)
))
self._meshcat_vis[self._prefix][line_y_name].set_object(geometry=Line(
meshcat_geometry.PointsGeometry(vertices_y), meshcat_geometry.MeshBasicMaterial(color=0x00ff00)
))
self._meshcat_vis[self._prefix][line_z_name].set_object(geometry=Line(
meshcat_geometry.PointsGeometry(vertices_z), meshcat_geometry.MeshBasicMaterial(color=0x0000ff)
))
def viewer_draw_frame(viewer, frame, id=None):
if id == None:
id = str(uuid.uuid1())
vertices = np.array([list(frame.point), list(frame.point + frame.xaxis)]).T
viewer['%s_xaxis' % id].set_object(mcg.Line(mcg.PointsGeometry(vertices), mcg.MeshBasicMaterial(color=0xff0000)))
vertices = np.array([list(frame.point), list(frame.point + frame.yaxis)]).T
viewer['%s_yaxis' % id].set_object(mcg.Line(mcg.PointsGeometry(vertices), mcg.MeshBasicMaterial(color=0x00ff00)))
vertices = np.array([list(frame.point), list(frame.point + frame.zaxis)]).T
viewer['%s_zaxis' % id].set_object(mcg.Line(mcg.PointsGeometry(vertices), mcg.MeshBasicMaterial(color=0x0000ff)))
return ['%s_xaxis' % id, '%s_yaxis' % id, '%s_zaxis' % id]
def meshcat_draw_frustrum(vis, TF, K, near_distance, far_distance, w, h):
# TODO(gizatt): This obviously isn't right -- the projected
# light doesn't match the drawn view frustrum.
# Not dealing with, for now; I think the issue is a combination
# of bad intrinsics and bugs related to flipped image coordinates
# somewhere along the pipeline.
image_bbox_verts = np.array([
[0., w, w, 0.],
[0., 0., h, h]
])
TF_inv = np.eye(4)
TF_inv[:3, :3] = TF[:3, :3].T
TF_inv[:3, 3] = -TF_inv[:3, :3].dot(TF[:3, 3])
TF = TF_inv
N = image_bbox_verts.shape[1]
Kinv = np.linalg.inv(K)
def project_bbox_verts(dist):
homog = np.concatenate(
[image_bbox_verts*dist, dist*np.ones((1, N))],
axis=0
)
pts = np.dot(Kinv, homog)
return ((TF[:3, :3].dot(pts)).T + TF[:3, 3]).T
near_pts = project_bbox_verts(near_distance)
far_pts= project_bbox_verts(far_distance)
near_colors = np.zeros((3, N))
near_colors[1, :] = 1.
far_colors = np.zeros((3, N))
far_colors[2, :] = 1.
vis['frustrum']['near'].set_object(g.LineLoop(
g.PointsGeometry(near_pts, color=near_colors),
g.MeshBasicMaterial(vertexColors=True, linewidth=0.1)))
vis['frustrum']['far'].set_object(g.LineLoop(
g.PointsGeometry(far_pts, color=far_colors),
g.MeshBasicMaterial(vertexColors=True, linewidth=0.1)))
connecting = np.zeros((3, N*2))
connecting[:, ::2] = near_pts
connecting[:, 1::2] = far_pts
connecting_colors = np.zeros((3, N*2))
connecting_colors[:, ::2] = near_colors
connecting_colors[:, 1::2] = far_colors
vis['frustrum']['connecting'].set_object(g.LineSegments(
g.PointsGeometry(connecting, color=connecting_colors),
g.MeshBasicMaterial(vertexColors=True, linewidth=1.)
))
# Draw a little box for the projector :)
vis['projector'].set_object(
g.Box([0.1, 0.1, 0.1]),
g.MeshLambertMaterial(
color=0xaaffaa))
def meshcat_visualize_deformed(meshcat_vis,
beam_disp,
orig_shape,
disc=10,
scale=1.0,
time_step=1):
red = np.array([1.0, 0.0, 0.0])
blue = np.array([0.0, 0.0, 1.0])
white = np.array([1.0, 1.0, 1.0])
pink = np.array([255.0, 20.0, 147.0]) / 255
black = [.0, .0, .0]
n_row, _ = beam_disp.shape
n_row_orig, _ = orig_shape.shape
ref_pt = np.array([0, 0, 0]) #beam_disp[0,:]
e = np.ones(disc + 1)
ref_trans = np.outer(e, ref_pt)
assert (int(n_row / disc) == int(n_row_orig / disc))
# print('-----------')
for k in range(int(n_row / (disc + 1))):
beam_pts = beam_disp[k * (disc + 1):(k + 1) * (disc + 1), :]
beam_pts = ref_trans + (beam_pts - ref_trans) * scale
orig_beam_pts = orig_shape[k * (disc + 1):(k + 1) * (disc + 1), :]
# print(orig_beam_pts)
scaled_orig_beam_pts = ref_trans + (orig_beam_pts - ref_trans) * scale
delta = np.abs(np.subtract(beam_pts, scaled_orig_beam_pts))
pt_delta = np.apply_along_axis(np.linalg.norm, 1, delta)
if np.max(pt_delta) > 1e-30:
pt_delta /= np.max(pt_delta)
color = np.outer(white, e - pt_delta) + np.outer(pink, pt_delta)
mc_key = 'deformed_' + str(k)
for i in range(disc):
mc_key_k = mc_key + str(i)
meshcat_vis[mc_key_k].set_object(
g.Line(g.PointsGeometry(beam_pts[i:i + 2, :].T),
g.MeshBasicMaterial(rgb_to_hex(color[:, i]))))
# meshcat_vis[mc_key_k].set_object(g.Line(g.PointsGeometry(beam_pts[i:i+2,:].T), g.MeshBasicMaterial(rgb_to_hex(black),opacity=0.6)))
or_key = 'original_' + str(k)
meshcat_vis[or_key].set_object(
g.Line(g.PointsGeometry(scaled_orig_beam_pts.T),
g.MeshBasicMaterial(rgb_to_hex(black), opacity=0.6)))
# time.sleep(time_step)
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 add_path(self, vertices: Tuple[Tuple[float, float, float]], colour=0xffffff) -> str:
""" Add a line to the scene and return the identifier. The line is made from
multiple line segments. The line will be drawn with a single colour.
Parameters
----------
vertices : tuple of tuple of float
The starting point of the line as (x, y, z) coordinates.
colour : int (optional)
An optional colour specified as a hex integer. The default colour is
white.
See also
--------
add_ray_path : Draws the line using individual line segments. Use this
method when each line segment needs to be drawn with a different colour.
Returns
-------
identifier : str
The string identifier used to add the line to the scene.
"""
vis = self.vis
self._will_add_expendable_to_scene(vertices)
vertices = np.array(vertices)
assert vertices.shape[0] == 3 # easy to get this wrong
identifier = self.get_next_identifer()
vis[identifier].set_object(
g.Line(g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=colour, transparency=True, opacity=0.5))
)
self._did_add_expendable_to_scene(identifier)
return identifier
def add_line_segment(self,
start: Tuple[float, float, float],
end: Tuple[float, float, float],
colour=0xffffff) -> str:
""" Add a line segment to the scene and return the identifier.
Parameters
----------
start : tuple
The starting point of the line as (x, y, z) coordinates.
end : tuple
The ending point of the line as (x, y, z) coordinates.
colour : int (optional)
An optional colour specified as a hex integer. The default colour is
white.
Returns
-------
identifier : str
The string identifier used to add the line to the scene.
"""
vis = self.vis
line = (start, end)
self._will_add_expendable_to_scene(line)
vertices = np.column_stack(line)
assert vertices.shape[0] == 3 # easy to get this wrong
identifier = self.get_next_identifer()
vis[identifier].set_object(
g.Line(
g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=colour,
transparency=False,
opacity=1)))
self._did_add_expendable_to_scene(identifier)
return identifier
def meshcat_draw_pointcloud(vis, verts, colors):
color_vis = colors[:, :, ::-1].astype(np.float32) / 255.
verts_vis = verts.reshape(-1, 3).T
color_vis = color_vis.reshape(-1, 3).T
vis.set_object(g.Points(
g.PointsGeometry(verts_vis, color=color_vis),
g.PointsMaterial(size=0.01)
))
def viewer_draw_lines(viewer, lines, color=None, id=None):
if color == None:
color = 0x777777
if id == None:
id = str(uuid.uuid1())
for i, line in enumerate(lines):
vertices = np.array([list(line['start']), list(line['end'])]).T
viewer["%s_%d" % (id, i)].set_object(mcg.Line(mcg.PointsGeometry(vertices), mcg.MeshBasicMaterial(color=color)))
return ["%s_%d" % (id, i) for i, line in enumerate(lines)]
def draw_scene_tree_meshcat(scene_tree,
zmq_url=None,
alpha=1.0,
node_sphere_size=0.1):
pruned_tree = scene_tree.get_tree_without_production_rules()
# Do actual drawing in meshcat, starting from root of tree
# So first find the root...
root_node = get_tree_root(pruned_tree)
vis = meshcat.Visualizer(zmq_url=zmq_url or "tcp://127.0.0.1:6000")
vis["scene_tree"].delete()
node_queue = [root_node]
# Assign functionally random colors to each new node
# type we discover.
node_class_to_color_dict = {}
cmap = plt.cm.get_cmap('jet')
cmap_counter = 0.
k = 0
while len(node_queue) > 0:
node = node_queue.pop(0)
children = list(pruned_tree.successors(node))
node_queue += children
# Draw this node
node_type_string = node.__class__.__name__
if node_type_string in node_class_to_color_dict.keys():
color = node_class_to_color_dict[node_type_string]
else:
color = rgb_2_hex(cmap(cmap_counter))
node_class_to_color_dict[node_type_string] = color
cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.)
vis["scene_tree"][node.name + "%d" % k].set_object(
meshcat_geom.Sphere(node_sphere_size),
meshcat_geom.MeshToonMaterial(color=color,
opacity=alpha,
transparent=(alpha != 1.)))
tf = node.tf.cpu().detach().numpy()
vis["scene_tree"][node.name + "%d" % k].set_transform(tf)
# Draw connections to children
verts = []
for child in children:
verts.append(node.tf[:3, 3].cpu().detach().numpy())
verts.append(child.tf[:3, 3].cpu().detach().numpy())
if len(verts) > 0:
verts = np.vstack(verts).T
vis["scene_tree"][node.name + "%d" % k +
"_child_connections"].set_object(
meshcat_geom.Line(
meshcat_geom.PointsGeometry(verts),
meshcat_geom.LineBasicMaterial(
linewidth=10, color=color)))
k += 1
def show(self, chain, showMeshes=False):
if 'google.colab' in sys.modules:
server_args = ['--ngrok_http_tunnel']
# Start a single meshcat server instance to use for the remainder of this notebook.
from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
proc, zmq_url, web_url = start_zmq_server_as_subprocess(
server_args=server_args)
vis = meshcat.Visualizer(zmq_url=zmq_url)
else:
vis = meshcat.Visualizer().open()
if showMeshes:
for i, link in enumerate(chain.linkArray):
if link.meshObj == None:
print("No mesh: " + link.name)
continue
boxVis = vis["link:" + link.name]
boxVis.set_object(
link.meshObj,
g.MeshLambertMaterial(color=0xffffff, reflectivity=0.8))
rotationMatrix = np.pad(link.absoluteOrientation, [(0, 1),
(0, 1)],
mode='constant')
rotationMatrix[-1][-1] = 1
boxVis.set_transform(
tf.translation_matrix(link.absoluteBase) @ rotationMatrix)
else:
for i, link in enumerate(chain.linkArray):
boxVis = vis["link:" + link.name]
if link.primitiveObj != None:
if isinstance(link.primitiveObj, primitives.Box):
box = meshcat.geometry.Box(link.primitiveObj.size)
boxVis.set_object(box)
if isinstance(link.primitiveObj, primitives.Cylinder):
cylinder = meshcat.geometry.Cylinder(
link.primitiveObj.length, link.primitiveObj.radius)
boxVis.set_object(cylinder)
if isinstance(link.primitiveObj, primitives.Sphere):
sphere = meshcat.geometry.Sphere(
link.primitiveObj.radius)
boxVis.set_object(cylinder)
rotationMatrix = np.pad(link.absoluteOrientation, [(0, 1),
(0, 1)],
mode='constant')
rotationMatrix[-1][-1] = 1
boxVis.set_transform(
tf.translation_matrix(link.absoluteBase)
@ rotationMatrix)
boxVis = vis["skeleton"]
boxVis.set_object(
g.Line(g.PointsGeometry(chain.get_vertex_coords().T)))
def draw_open3d_point_cloud(meshcat, pcd, normals_scale=0.0, size=0.001):
pts = np.asarray(pcd.points)
meshcat.set_object(g.PointCloud(pts.T, np.asarray(pcd.colors).T, size=size))
if pcd.has_normals() and normals_scale > 0.0:
normals = np.asarray(pcd.normals)
vertices = np.hstack(
(pts, pts + normals_scale * normals)).reshape(-1, 3).T
meshcat["normals"].set_object(
g.LineSegments(g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=0x000000)))
def meshcat_visualize_csp_log(meshcat_vis, assembly_network, assign_history, stiffness_checker=None, scale=1.0, time_step=1):
# EE direction color
color = [1, 0, 0]
remain_color = [1, 1, 0]
ref_pt = np.zeros(3)
ref_pt, _ = assembly_network.get_end_points(0)
full_e_ids = set(range(assembly_network.get_size_of_elements()))
for k in assign_history.keys():
e_ids = assign_history[k]
vertices = np.zeros([3, 2*len(e_ids)])
for i, e in enumerate(e_ids):
p1, p2 = assembly_network.get_end_points(e)
p1 = ref_pt + (p1 - ref_pt) * scale
p2 = ref_pt + (p2 - ref_pt) * scale
vertices[:, 2*i] = np.array(p1)
vertices[:, 2*i+1] = np.array(p2)
if int(k) > 0:
meshcat_vis['assign_history_' + str(k-1)].delete()
meshcat_vis['assign_history_' + str(k-1) + '_remain'].delete()
mc_dir_key = 'assign_history_' + str(k)
meshcat_vis[mc_dir_key].set_object(
g.LineSegments(g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=rgb_to_hex(color))))
remain_e_ids = list(full_e_ids.difference(e_ids))
vertices = np.zeros([3, 2*len(remain_e_ids)])
for i, e in enumerate(remain_e_ids):
p1, p2 = assembly_network.get_end_points(e)
p1 = ref_pt + (p1 - ref_pt) * scale
p2 = ref_pt + (p2 - ref_pt) * scale
vertices[:, 2*i] = np.array(p1)
vertices[:, 2*i+1] = np.array(p2)
meshcat_vis[mc_dir_key+'_remain'].set_object(
g.LineSegments(g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=rgb_to_hex(remain_color), opacity=0.3)))
time.sleep(time_step)
def meshcat_visualize_deformed(meshcat_vis, beam_disp, orig_shape, draw_orig=True, disc=10, scale=1.0, opacity=0.6):
n_row, _ = beam_disp.shape
n_row_orig, _ = orig_shape.shape
ref_pt = np.array([0,0,0]) #beam_disp[0,:]
e = np.ones(disc+1)
ref_trans = np.outer(e, ref_pt)
assert(n_row / disc == n_row_orig / disc)
for k in range(n_row / (disc+1)):
beam_pts = beam_disp[k*(disc+1):(k+1)*(disc+1),:]
beam_pts = ref_trans + (beam_pts - ref_trans) * scale
orig_beam_pts = orig_shape[k*(disc+1):(k+1)*(disc+1),:]
orig_beam_pts = ref_trans + (orig_beam_pts - ref_trans) * scale
delta = np.abs(np.subtract(beam_pts, orig_beam_pts))
pt_delta = np.apply_along_axis(np.linalg.norm, 1, delta)
# print("max delta {0}".format(np.max(pt_delta)))
if np.max(pt_delta) > 1e-30:
pt_delta /= np.max(pt_delta)
color = np.outer(white, e - pt_delta) + np.outer(pink, pt_delta)
# print("color {0}".format(color))
mc_key = 'deformed_' + str(k)
for i in range(disc):
mc_key_k = mc_key + str(i)
meshcat_vis[mc_key_k].set_object(
g.Line(g.PointsGeometry(beam_pts[i:i+2,:].T),
g.MeshBasicMaterial(rgb_to_hex(color[:,i]))))
if draw_orig:
or_key = 'original_' + str(k)
meshcat_vis[or_key].set_object(
g.Line(g.PointsGeometry(orig_beam_pts.T),
g.MeshBasicMaterial(rgb_to_hex(black), opacity=opacity)))
def meshcat_draw_face_detection(vis, verts, points):
# Select the appropriate verts
pts = np.array(points)
selected_verts = verts[pts[:, 0], pts[:, 1], :].reshape(-1, 3).T
color = np.zeros(selected_verts.shape)
color[0, :] = 1.
#logging.info("Triangles shape: ", canonical_face_mesh.triangles)
#mesh = g.TriangularMeshGeometry(
# vertices=selected_verts,
# faces=canonical_face_mesh.faces
#)
#vis["face_mesh"].set_object(mesh)
vis["face_points"].set_object(g.Points(
g.PointsGeometry(selected_verts, color=color),
g.PointsMaterial(size=0.01)
))
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 visualize_points(vis,
name,
pts, # [N,3]
color=None, # [3,] array of float in [0,1]
size=0.001, # size of the points
T=None, # T_world_pts transform
):
if color is not None:
N, _ = pts.shape
color = 1.0 * np.ones([N, 3]) * np.array(color)
color = color.transpose()
geom = g.Points(
g.PointsGeometry(pts.transpose(), color=color),
g.PointsMaterial(size=size)
)
vis[name].set_object(geom)
if T is not None:
vis[name].set_transform(T)
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
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["box"].set_object(g.Box([1.0, 0.2, 0.3]))
v["box"].delete()
v["box"].set_object(g.Box([0.1, 0.2, 0.3]))
v["box"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v["transparent_ellipsoid"].set_object(g.Mesh(
g.Ellipsoid([0.3, 0.1, 0.1]),
g.MeshLambertMaterial(color=0xffffff,
opacity=0.5)))
v["transparent_ellipsoid"].set_transform(tf.translation_matrix([0, 2.0, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["meshes/convex"]
v["obj"].set_object(g.Mesh(g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.obj"))))
v["stl_ascii"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_ascii"))))
v["stl_ascii"].set_transform(tf.translation_matrix([0, -0.5, 0]))
v["stl_binary"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_binary"))))
v["stl_binary"].set_transform(tf.translation_matrix([0, -1, 0]))
v["dae"].set_object(g.Mesh(g.DaeMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.dae"))))
v["dae"].set_transform(tf.translation_matrix([0, -1.5, 0]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([0, 2, 0]))
verts = np.random.rand(3, 1000000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
v = self.vis["lines"]
v.set_transform(tf.translation_matrix(([-2, -3, 0])))
vertices = np.random.random((3, 10)).astype(np.float32)
v["line_segments"].set_object(g.LineSegments(g.PointsGeometry(vertices)))
v["line"].set_object(g.Line(g.PointsGeometry(vertices)))
v["line"].set_transform(tf.translation_matrix([0, 1, 0]))
v["line_loop"].set_object(g.LineLoop(g.PointsGeometry(vertices)))
v["line_loop"].set_transform(tf.translation_matrix([0, 2, 0]))
v["line_loop_with_material"].set_object(g.LineLoop(g.PointsGeometry(vertices), g.LineBasicMaterial(color=0xff0000)))
v["line_loop_with_material"].set_transform(tf.translation_matrix([0, 3, 0]))
colors = vertices # Color each line by treating its xyz coordinates as RGB colors
v["line_with_vertex_colors"].set_object(g.Line(g.PointsGeometry(vertices, colors), g.LineBasicMaterial(vertexColors=True)))
v["line_with_vertex_colors"].set_transform(tf.translation_matrix([0, 4, 0]))
v["triad"].set_object(g.LineSegments(
g.PointsGeometry(position=np.array([
[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T,
color=np.array([
[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
),
g.LineBasicMaterial(vertexColors=True)))
v["triad"].set_transform(tf.translation_matrix(([0, 5, 0])))
v["triad_function"].set_object(g.triad(0.5))
v["triad_function"].set_transform(tf.translation_matrix([0, 6, 0]))
from __future__ import absolute_import, division, print_function
import time
import numpy as np
import meshcat
import meshcat.geometry as g
verts = np.random.random((3, 100000)).astype(np.float32)
vis = meshcat.Visualizer().open()
vis.set_object(
g.Points(g.PointsGeometry(verts, color=verts), g.PointsMaterial()))
def meshcat_visualize_assembly_sequence(meshcat_vis, assembly_network, element_id_sequence, seq_poses,
stiffness_checker=None, viz_pose=False, viz_deform=False,
scale=1.0, time_step=1, direction_len=0.01, exagg=1.0):
# EE direction color
dir_color = [0, 1, 0]
disc = 10 # disc for deformed beam
ref_pt, _ = assembly_network.get_end_points(0)
existing_e_ids = []
if stiffness_checker:
t_tol, r_tol = stiffness_checker.get_nodal_deformation_tol()
for k in element_id_sequence.keys():
e_id = element_id_sequence[k]
existing_e_ids.append(e_id)
if not viz_deform and stiffness_checker:
print(existing_e_ids)
assert(stiffness_checker.solve(existing_e_ids))
max_t, max_r = stiffness_checker.get_max_nodal_deformation()
print("max_t: {0} / {1}, max_r: {2} / {3}".format(max_t, t_tol, max_r, r_tol))
orig_shape = stiffness_checker.get_original_shape(disc=disc, draw_full_shape=False)
beam_disp = stiffness_checker.get_deformed_shape(exagg_ratio=exagg, disc=disc)
meshcat_visualize_deformed(meshcat_vis, beam_disp, orig_shape, disc, scale=scale)
else:
p1, p2 = assembly_network.get_end_points(e_id)
p1 = ref_pt + (p1 - ref_pt) * scale
p2 = ref_pt + (p2 - ref_pt) * scale
e_mid = (np.array(p1) + np.array(p2)) / 2
seq_ratio = float(k)/(len(element_id_sequence)-1)
color = np.array([0, 0, 1])*(1-seq_ratio) + np.array([1, 0, 0])*seq_ratio
# TODO: add_text(str(k), position=e_mid, text_size=text_size)
# print('color {0} -> {1}'.format(color, rgb_to_hex(color)))
vertices = np.vstack((p1, p2)).T
mc_key = 'ase_seq_' + str(k)
meshcat_vis[mc_key].set_object(g.LineSegments(g.PointsGeometry(vertices), g.MeshBasicMaterial(color=rgb_to_hex(color))))
if seq_poses is not None and viz_pose:
assert(k in seq_poses)
dir_vertices = np.zeros([3, 2*len(seq_poses[k])])
for i, ee_dir in enumerate(seq_poses[k]):
assert(isinstance(ee_dir, EEDirection))
cmap_pose = multiply(Pose(point=e_mid), make_print_pose(ee_dir.phi, ee_dir.theta))
origin_world = e_mid
axis = np.zeros(3)
axis[2] = 1
axis_world = tform_point(cmap_pose, direction_len*axis)
dir_vertices[:, 2*i] = np.array(origin_world)
dir_vertices[:, 2*i+1] = np.array(axis_world)
mc_dir_key = 'as_dir_' + str(k)
meshcat_vis[mc_dir_key + 'line'].set_object(
g.LineSegments(g.PointsGeometry(dir_vertices),
g.MeshBasicMaterial(color=rgb_to_hex(dir_color), opacity=0.1)))
# meshcat_vis[mc_dir_key].set_object(g.Points(
# g.PointsGeometry(dir_vertices),
# g.PointsMaterial(size=0.01)))
time.sleep(time_step)
def draw_tree(tree, vis, prefix="", draw_regions=False):
# Given a scene tree (nx.DiGraph), draw it in the
# specified meshcat visualizer.
# Draw the scene geometry flat, to keep TFs easy.
name_prefix = prefix + "scene"
vis[name_prefix].delete()
k = 0
for node in tree.nodes:
name = name_prefix + "/%s_%03d" % (node.__class__.__name__, k)
if node.geometry is not None:
color = node.geometry_color
alpha = 1.0
vis[name].set_object(
node.geometry,
meshcat_geom.MeshLambertMaterial(color=color, opacity=alpha, transparent=(alpha != 1.))
)
tf = node.tf.GetAsMatrix4()
geom_tf = node.geometry_tf.GetAsMatrix4()
tf = tf.dot(geom_tf)
tf[:3, :3] = tf[:3, :3].dot(np.diag(node.geometry_scale))
print(tf)
vis[name].set_transform(tf)
k += 1
# Draw the tree structure.
tree_prefix = prefix + "tree"
vis[tree_prefix].delete()
k = 0
for node in tree.nodes:
name = tree_prefix + "/" + node.__class__.__name__ + "_%03d" % k
k += 1
# Draw node as randomly colored sphere
color = random.randint(0, 0xFFFFFF)
alpha = 0.5
vis[name]["triad"].set_object(
meshcat_geom.triad(scale=0.1)
)
vis[name]["sphere"].set_object(
meshcat_geom.Sphere(0.01),
meshcat_geom.MeshToonMaterial(color=color, opacity=alpha, transparent=(alpha != 1.))
)
vis[name].set_transform(node.tf.GetAsMatrix4())
# Draw children
verts = []
for child in tree.successors(node):
# Draw link to child
verts.append(node.tf.translation()),
verts.append(child.tf.translation())
if len(verts) > 0:
verts = np.vstack(verts).T
# Don't want this as a direct child or it'll inherit the transform
vis[name + "_child_connections"].set_object(
meshcat_geom.Line(meshcat_geom.PointsGeometry(verts),
meshcat_geom.LineBasicMaterial(linewidth=50, color=color)))
if draw_regions:
# Draw the child regions for each child
if isinstance(node, (AndNode, OrNode, RepeatingSetNode)):
for info_k, child_info in enumerate(node.child_infos):
region_name = "child_region_%03d" % info_k
lb = child_info.child_xyz_bounds.xyz_min
ub = child_info.child_xyz_bounds.xyz_max
vis[name][region_name].set_object(
meshcat_geom.Box(ub - lb),
meshcat_geom.MeshToonMaterial(color=0x111111, opacity=0.1, transparent=True)
)
tf = RigidTransform(p=(ub+lb)/2)
vis[name][region_name].set_transform(tf.GetAsMatrix4())
vis['cyl'].set_object(
g.Cylinder(length, radius),
material)
vis['cyl'].set_transform(
tf.translation_matrix([0.0, 0.0, 0.0]).dot(
tf.rotation_matrix(np.radians(-90), [1, 0, 0]))
)
# Visualise test rays
for idx, (ray_origin, ray_direction, expected) in enumerate(tests):
ray_inf = np.array(ray_origin) + 5.0 * np.array(ray_direction)
vertices = np.column_stack((ray_origin, ray_inf))
red_material = g.MeshLambertMaterial(
reflectivity=1.0, sides=0, color=0xff0000)
vis['line_{}'.format(idx)].set_object(g.Line(g.PointsGeometry(vertices)))
points = ray_z_cylinder(length, radius, ray_origin, ray_direction)
for ptidx, pt in enumerate(points):
vis['point_{}_{}'.format(idx, ptidx)].set_object(
g.Sphere(0.05),
red_material)
vis['point_{}_{}'.format(idx, ptidx)].set_transform(
tf.translation_matrix(pt)
)
if np.allclose(points, expected, atol=1e-15):
print(points)
print("OK!")
vis.jupyter_cell()
# In[ ]:
def draw_scene_tree_structure_meshcat(scene_tree,
prefix="scene_tree",
zmq_url=None,
alpha=0.775,
node_sphere_size=0.05,
linewidth=2,
with_triad=True,
quiet=True,
color_by_score=None,
delete=True):
# Color by score can be a tuple of min, max score. It'll go from red at min score
# to blue at max score.
# Do actual drawing in meshcat.
if quiet:
with open(os.devnull, 'w') as devnull:
with contextlib.redirect_stdout(devnull):
vis = meshcat.Visualizer(
zmq_url=zmq_url or "tcp://127.0.0.1:6000")
else:
vis = meshcat.Visualizer(zmq_url=zmq_url or "tcp://127.0.0.1:6000")
if delete:
vis[prefix].delete()
# Assign functionally random colors to each new node
# type we discover, or color my their scores.
node_class_to_color_dict = {}
cmap = plt.cm.get_cmap('jet')
cmap_counter = 0.
k = 0
for node in scene_tree.nodes:
children, rules = scene_tree.get_children_and_rules(node)
#
if color_by_score is not None:
assert len(color_by_score
) == 2, "Color by score should be a tuple of (min, max)"
score = node.score_child_set(children)
print("Node score: ", score)
score = (score - color_by_score[0]) / (color_by_score[1] -
color_by_score[0])
score = 1. - np.clip(score.item(), 0., 1.)
color = rgb_2_hex(cmap(score))
#color = 0x555555
else:
# Draw this node
node_type_string = node.__class__.__name__
if node_type_string in node_class_to_color_dict.keys():
color = node_class_to_color_dict[node_type_string]
else:
color = rgb_2_hex(cmap(cmap_counter))
node_class_to_color_dict[node_type_string] = color
cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.)
vis[prefix][node.name + "%d/sphere" % k].set_object(
meshcat_geom.Sphere(node_sphere_size),
meshcat_geom.MeshToonMaterial(color=color,
opacity=alpha,
transparent=(alpha != 1.),
depthTest=False))
if with_triad:
vis[prefix][node.name + "%d/triad" % k].set_object(
meshcat_geom.triad(scale=node_sphere_size * 5.))
tf = node.tf.cpu().detach().numpy()
vis[prefix][node.name + "%d" % k].set_transform(tf)
# Draw connections to each child
for child, rule in zip(children, rules):
verts = []
verts.append(node.tf[:3, 3].cpu().detach().numpy())
verts.append(child.tf[:3, 3].cpu().detach().numpy())
verts = np.vstack(verts).T
if color_by_score is not None:
score = rule.score_child(node, child)
print("Rule score: ", score)
score = (score - color_by_score[0]) / (color_by_score[1] -
color_by_score[0])
score = 1. - np.clip(score.item(), 0., 1.)
color = rgb_2_hex(cmap(score))
vis[prefix][node.name + "_to_" + child.name].set_object(
meshcat_geom.Line(
meshcat_geom.PointsGeometry(verts),
meshcat_geom.LineBasicMaterial(linewidth=linewidth,
color=color,
depthTest=False)))
k += 1
