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