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)
