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