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_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 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 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 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 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 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]))
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
