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