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 add_geometry(self, geometry, pathname, transform): vis = self.vis material = g.MeshBasicMaterial(reflectivity=self.reflectivity, sides=0, wireframe=self.wireframe) material.transparency = self.transparency material.opacity = self.opacity if isinstance(geometry, Sphere): sphere = geometry vis[pathname].set_object(g.Sphere(sphere.radius), material) vis[pathname].set_transform(transform) elif isinstance(geometry, Cylinder): cyl = geometry vis[pathname].set_object(g.Cylinder(cyl.length, cyl.radius), material) # meshcat cylinder is aligned along y-axis. Align along z then apply the # node's transform as normal. transform = np.copy(transform) # Change basic XYZ -> XZY transform[:, [1, 2]] = transform[:, [2, 1]] vis[pathname].set_transform(transform) elif isinstance(geometry, Mesh): obj = meshcat.geometry.StlMeshGeometry.from_stream( io.BytesIO(trimesh.exchange.stl.export_stl(geometry.trimesh))) vis[pathname].set_object(obj, material) vis[pathname].set_transform(transform) else: raise NotImplementedError("Cannot yet add {} to visualiser".format( type(geometry)))
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_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 add_point(self, point_in_world, point_name='point_0', radius=0.02): # type: (np.ndarray, str, float) -> None """ Add a point to the world, represent by sphere :param point_in_world: (3,) np.ndarray represent the point :param point_name: the key used to index the point :param radius: the radius in METER (by default 5 cm) :return: """ meshcat_sphere_geom = meshcat.geometry.Sphere(radius) self._meshcat_vis[self._prefix][point_name].set_object( meshcat_sphere_geom, meshcat_geometry.MeshBasicMaterial(color=0x0000ff) ) point_in_world_tf = np.eye(4) point_in_world_tf[0:3, 3] = point_in_world self._meshcat_vis[self._prefix][point_name].set_transform(point_in_world_tf)
def add_history( self, history: Tuple, baubles: bool = True, world_segment: str = "short", short_length: float = 1.0, bauble_radius: float = 0.01, ): """ Similar to `add_ray_path` but with improved visualisation options. Parameters ---------- history: tuple Tuple of rays and events as returned from `photon_tracer.follow` baubles: bool (optional) Default is True. Draws baubles at exit location. world_segment: str (optional) Opt-out (`'exclude'`) or draw short (`'short`) path segments to the world node. short_length: float The length of the final path segment when `world_segment='short'`. bauble_radius: float The bauble radius when `baubles=True`. """ vis = self.vis if not world_segment in {"exclude", "short"}: raise ValueError( "`world_segment` should be either `'exclude'` or `'short'`.") if world_segment == "exclude": rays, events = zip(*history) try: idx = events.index(Event.EXIT) history = history[0:idx] if len(history) < 2: # nothing left to render return except ValueError: pass if len(history) < 2: raise AppError("Need at least two points to render a line.") ids = [] rays, events = zip(*history) for (start_part, end_part) in zip(history[:-1], history[1:]): start_ray, end_ray = start_part[0], end_part[0] nanometers = start_ray.wavelength start = start_ray.position end = end_ray.position if world_segment == "short": if end_ray == history[-1][0]: end = (np.array(start_ray.position) + np.array(start_ray.direction) * short_length) colour = wavelength_to_hex_int(nanometers) ids.append(self.add_line_segment(start, end, colour=colour)) if baubles: event = start_part[1] if event in {Event.TRANSMIT}: baubid = self.get_next_identifer() vis[f"exit/{baubid}"].set_object( g.Sphere(bauble_radius), g.MeshBasicMaterial(color=colour, transparency=False, opacity=1), ) vis[f"exit/{baubid}"].set_transform( tf.translation_matrix(start)) ids.append(baubid) return ids
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)