def PlotTrajectoryMeshcat(x, t, vis, wpts_list = None):
# initialize
vis.delete()
# plot waypoints
if not(wpts_list is None):
for i, wpts in enumerate(wpts_list):
vis["wpt_%d" % i].set_object(geometry.Sphere(0.03),
geometry.MeshLambertMaterial(color=0xffff00))
T_wp = tf.translation_matrix(wpts)
vis["wpt_%d" % i].set_transform(T_wp)
d_prop = 0.10 # propeller diameter
vis["quad"]["CG"].set_object(geometry.Sphere(0.03),
geometry.MeshLambertMaterial(color=0x00ffff))
vis["quad"]["body"].set_object(geometry.Box([0.2, 0.1, 0.1]),
geometry.MeshLambertMaterial(color=0x404040))
vis["quad"]["prop0"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0x00ff00))
vis["quad"]["prop1"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0xff0000))
vis["quad"]["prop2"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0xffffff))
vis["quad"]["prop3"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0xffffff))
Rx_prop = CalcRx(np.pi/2)
TB = tf.translation_matrix([0,0,-0.05])
T0 = tf.translation_matrix([l, -l, 0])
T1 = tf.translation_matrix([l, l, 0])
T2 = tf.translation_matrix([-l, l, 0])
T3 = tf.translation_matrix([-l, -l, 0])
T0[0:3,0:3] = Rx_prop
T1[0:3,0:3] = Rx_prop
T2[0:3,0:3] = Rx_prop
T3[0:3,0:3] = Rx_prop
vis["quad"]["body"].set_transform(TB)
vis["quad"]["prop0"].set_transform(T0)
vis["quad"]["prop1"].set_transform(T1)
vis["quad"]["prop2"].set_transform(T2)
vis["quad"]["prop3"].set_transform(T3)
# visualize trajectory
time.sleep(1.0)
N = len(x)
if not (t is None):
assert N == len(t)
for i, xi in enumerate(x):
xyz = xi[0:3]
rpy = xi[3:6]
R_WB = CalcR_WB(rpy)
T = tf.translation_matrix(xyz)
T[0:3,0:3] = R_WB
vis["quad"].set_transform(T)
if i < N-1 and not(t is None):
dt = t[i+1] - t[i]
time.sleep(dt)
def add_geometry(self, geometry, pathname, transform):
vis = self.vis
material = g.MeshLambertMaterial(reflectivity=1.0, sides=0)
material.transparency = True
material.opacity = 0.5
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.
vis[pathname].set_transform(
transform.dot(
tf.rotation_matrix(np.radians(-90), [1, 0, 0])
)
)
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 runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
v["cube"].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 = 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["points"]
v.set_transform(tf.translation_matrix([-1, 0, 0]))
verts = np.random.rand(3, 100000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
def convert_link_visuals(link, link_index, material, vis, uid, b2vis):
print("convert_link_visuals:: num_visuals=", len(link.urdf_visual_shapes))
print("link.urdf_visual_shapes=", link.urdf_visual_shapes)
for v in link.urdf_visual_shapes:
print("v.geom_type=", v.geometry.geom_type)
vis_name = link.link_name + str(uid)
b2v = VisualLinkInfo()
b2v.vis_name = vis_name
b2v.link_index = link_index
b2v.origin_rpy = v.origin_rpy
b2v.origin_xyz = v.origin_xyz
b2v.inertia_xyz = link.urdf_inertial.origin_xyz
b2v.inertia_rpy = link.urdf_inertial.origin_rpy
if v.geometry.geom_type == dp.SPHERE_TYPE:
print("v.geom_radius=", v.geometry.sphere.radius)
if (v.geometry.sphere.radius > 0.):
print("created sphere!")
vis[vis_name].set_object(g.Sphere(v.geometry.sphere.radius), material)
b2vis[uid] = b2v
uid += 1
if v.geometry.geom_type == dp.MESH_TYPE:
print("mesh filename=", v.geometry.mesh.file_name)
print("geom_meshscale=", v.geometry.mesh.scale)
vis_name = link.link_name + str(uid)
vis[vis_name].set_object(
g.ObjMeshGeometry.from_file(v.geometry.mesh.file_name), material)
b2v.uid = uid
b2vis[uid] = b2v
uid += 1
return b2vis, uid
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 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 viewer_draw_sphere(viewer, sphere, color=None, id=None):
import meshcat.transformations as tf
if color == None:
color = 0x777777
if id == None:
id = str(uuid.uuid1())
s = mcg.Sphere(sphere.radius)
viewer[id].set_object(s), mcg.MeshLambertMaterial(color=color)
viewer[id].set_transform(tf.translation_matrix(list(sphere.point)))
return id
def meshcat_draw_lights(vis, light_locations, light_attenuations):
N = light_locations.shape[1]
colors = np.zeros((3, N))
colors[2, :] = light_attenuations
for k in range(N):
vis["lights"]["%d" % k].set_object(
g.Sphere(radius=0.05),
g.MeshLambertMaterial(
color=0xffffff,
reflectivity=0.8))
vis["lights"]["%d" % k].set_transform(
tf.translation_matrix(light_locations[:, k]))
def _get_shape_geometry(self, shape):
visual_mesh = shape.get_user_data().get('visual_mesh', None) if shape.get_user_data() is not None else None
if visual_mesh is not None:
return g.TriangularMeshGeometry(vertices=visual_mesh.vertices, faces=visual_mesh.faces)
elif shape.get_geometry_type() == GeometryType.CONVEXMESH:
data = shape.get_shape_data() # N x 9 - i.e. 3 triangles
faces = np.arange(0, data.shape[0] * 3, 1, dtype=np.int).reshape(-1, 3)
return g.TriangularMeshGeometry(vertices=data.reshape(-1, 3), faces=faces)
elif shape.get_geometry_type() == GeometryType.SPHERE:
return g.Sphere(radius=shape.get_sphere_radius())
elif shape.get_geometry_type() == GeometryType.BOX:
return g.Box((2 * shape.get_box_half_extents()).tolist())
else:
raise NotImplementedError("Not supported geometry type.")
def visualize(self,states,dt): import meshcat import meshcat.geometry as g import meshcat.transformations as tf import time # Create a new visualizer vis = meshcat.Visualizer() vis.open() for i in range(self.n_agents): vis["agent"+str(i)].set_object(g.Sphere(self.r_agent)) while True: for state in states: for i in range(self.n_agents): idx = self.agent_idx_to_state_idx(i) + np.arange(0,2) pos = state[idx] vis["agent" + str(i)].set_transform(tf.translation_matrix([pos[0], pos[1], 0])) time.sleep(dt)
def _get_shape_geometry(self, shape):
visual_mesh = shape.get_user_data().get(
'visual_mesh', None) if shape.get_user_data() is not None else None
if visual_mesh is not None:
try:
exp_obj = trimesh.exchange.obj.export_obj(visual_mesh)
except ValueError:
exp_obj = trimesh.exchange.obj.export_obj(
visual_mesh, include_texture=False)
return g.ObjMeshGeometry.from_stream(
trimesh.util.wrap_as_stream(exp_obj))
elif shape.get_geometry_type() == GeometryType.CONVEXMESH:
data = shape.get_shape_data() # N x 9 - i.e. 3 triangles
faces = np.arange(0, data.shape[0] * 3, 1,
dtype=np.int).reshape(-1, 3)
return g.TriangularMeshGeometry(vertices=data.reshape(-1, 3),
faces=faces)
elif shape.get_geometry_type() == GeometryType.SPHERE:
return g.Sphere(radius=shape.get_sphere_radius())
elif shape.get_geometry_type() == GeometryType.BOX:
return g.Box((2 * shape.get_box_half_extents()).tolist())
else:
raise NotImplementedError("Not supported geometry type.")
def plot_mathematical_program(meshcat, prog, X, Y, result=None):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = meshcat["constraint"]
for binding in prog.GetAllConstraints():
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
# Special case linear constraints
if False: # isinstance(c, LinearConstraint):
# TODO: take these as (optional) arguments to avoid computing them
# inefficiently.
xmin = np.min(X.reshape(-1))
xmax = np.max(X.reshape(-1))
ymin = np.min(Y.reshape(-1))
ymax = np.max(Y.reshape(-1))
A = c.A()
lower = c.lower_bound()
upper = c.upper_bound()
# find line / box intersections
# https://gist.github.com/ChickenProp/3194723
else:
v = cv[str(binding)]
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(v,
X,
Y,
Zc.reshape((X.shape[1], X.shape[0])),
color=0x9999dd)
if costs:
plot_surface(meshcat["objective"],
X,
Y,
Z.reshape(X.shape[1], X.shape[0]),
wireframe=True)
if result:
v = meshcat["solution"]
v.set_object(g.Sphere(0.1), g.MeshLambertMaterial(color=0x99ff99))
x_solution = result.get_x_val()
v.set_transform(
tf.translation_matrix(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
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 plot_mathematical_program(meshcat,
prog,
X,
Y,
result=None,
point_size=0.05):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = meshcat["constraints"]
for binding in prog.GetAllConstraints():
if isinstance(
binding.evaluator(),
pydrake.solvers.mathematicalprogram.BoundingBoxConstraint):
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
v = cv[type(c).__name__]
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(v,
X,
Y,
Zc.reshape((X.shape[1], X.shape[0])),
color=0xff3333,
wireframe=True)
else:
Zc = prog.EvalBindingVectorized(binding, values)
evaluator = binding.evaluator()
low = evaluator.lower_bound()
up = evaluator.upper_bound()
cvb = cv[type(evaluator).__name__]
for index in range(Zc.shape[0]):
color = np.repeat([[0.3], [0.3], [1.0]], N, axis=1)
infeasible = np.logical_or(Zc[index, :] < low[index],
Zc[index, :] > up[index])
color[0, infeasible] = 1.0
color[2, infeasible] = 0.3
plot_surface(cvb[str(index)],
X,
Y,
Zc[index, :].reshape(X.shape[1], X.shape[0]),
color=color,
wireframe=True)
if costs:
plot_surface(meshcat["objective"],
X,
Y,
Z.reshape(X.shape[1], X.shape[0]),
color=0x77cc77,
wireframe=True)
if result:
v = meshcat["solution"]
v.set_object(g.Sphere(point_size),
g.MeshLambertMaterial(color=0x55ff55))
x_solution = result.get_x_val()
v.set_transform(
tf.translation_matrix(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
def __init__(self,
mbp,
sg,
all_manipulable_body_ids=[],
zmq_url="default"):
LeafSystem.__init__(self)
self.all_manipulable_body_ids = all_manipulable_body_ids
self.set_name('HydraInteractionLeafSystem')
# Pose bundle (from SceneGraph) input port.
#default_sg_context = sg.CreateDefaultContext()
#print("Default sg context: ", default_sg_context)
#query_object = sg.get_query_output_port().Eval(default_sg_context)
#print("Query object: ", query_object)
#self.DeclareAbstractInputPort("query_object",
# AbstractValue.Make(query_object))
self.pose_bundle_input_port = self.DeclareAbstractInputPort(
"pose_bundle", AbstractValue.Make(PoseBundle(0)))
self.robot_state_input_port = self.DeclareVectorInputPort(
"robot_state",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
self.spatial_forces_output_port = self.DeclareAbstractOutputPort(
"spatial_forces_vector",
lambda: AbstractValue.Make(VectorExternallyAppliedSpatialForced()),
self.DoCalcAbstractOutput)
self.DeclarePeriodicPublish(0.01, 0.0)
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
fwd_pt_in_hydra_frame = RigidTransform(p=[0.0, 0.0, 0.0])
self.vis["hydra_origin"]["hand"].set_object(
meshcat_geom.ObjMeshGeometry.from_file(
os.path.join(os.getcwd(), "hand-regularfinal-scaled-1.obj")))
self.vis["hydra_origin"]["hand"].set_transform(
meshcat_tf.compose_matrix(scale=[0.001, 0.001, 0.001],
angles=[np.pi / 2, 0., np.pi / 2],
translate=[-0.25, 0., 0.]))
#self.vis["hydra_origin"]["center"].set_object(meshcat_geom.Sphere(0.02))
#self.vis["hydra_origin"]["center"].set_transform(meshcat_tf.translation_matrix([-0.025, 0., 0.]))
#self.vis["hydra_origin"]["mid"].set_object(meshcat_geom.Sphere(0.015))
#self.vis["hydra_origin"]["mid"].set_transform(meshcat_tf.translation_matrix([0.0, 0., 0.]))
#self.vis["hydra_origin"]["fwd"].set_object(meshcat_geom.Sphere(0.01))
#self.vis["hydra_origin"]["fwd"].set_transform(fwd_pt_in_hydra_frame.matrix())
#self.vis["hydra_grab"].set_object(meshcat_geom.Sphere(0.01),
# meshcat_geom.MeshLambertMaterial(
# color=0xff22dd,
# alphaMap=0.1))
self.vis["hydra_grab"]["grab_point"].set_object(
meshcat_geom.Sphere(0.01),
meshcat_geom.MeshLambertMaterial(color=0xff22dd, alphaMap=0.1))
# Hide it sketchily
self.vis["hydra_grab"].set_transform(
meshcat_tf.translation_matrix([0., 0., -1000.]))
# State for selecting objects
self.grab_needs_update = False
self.grab_in_progress = False
self.grab_update_hydra_pose = None
self.selected_body = None
self.selected_pose_in_body_frame = None
self.desired_pose_in_world_frame = None
self.stop = False
self.freeze_rotation = False
self.previously_freezing_rotation = False
# Set up subscription to Razer Hydra
self.mbp = mbp
self.mbp_context = mbp.CreateDefaultContext()
self.sg = sg
self.hasNewMessage = False
self.lastMsg = None
self.hydra_origin = RigidTransform(p=[1.0, 0., -0.1],
rpy=RollPitchYaw([0., 0., 0.]))
self.hydra_prescale = 3.0
self.callback_lock = Lock()
self.hydraSubscriber = rospy.Subscriber("/hydra_calib",
razer_hydra.msg.Hydra,
self.callback,
queue_size=1)
print("Waiting for hydra startup...")
while not self.hasNewMessage and not rospy.is_shutdown():
rospy.sleep(0.01)
print("Got hydra.")
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
def draw_atom(v, label, atom, radius, color=0xffffff):
v[label].set_object(
g.Mesh(g.Sphere(radius), g.MeshLambertMaterial(color=color)))
v[label].set_transform(tf.translation_matrix(atom))
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 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())
def load(self, context=None):
"""
Loads ``meshcat`` visualization elements.
Precondition:
Either the context is a valid Context for this system with the
geometry_query port connected or the ``scene_graph`` passed in the
constructor must be a valid SceneGraph.
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
if context and self.get_geometry_query_input_port().HasValue(context):
inspector = self.get_geometry_query_input_port().Eval(
context).inspector()
elif self._scene_graph:
inspector = self._scene_graph.model_inspector()
else:
raise RuntimeError(
"You must provide a valid Context for this system with the "
"geometry_query port connected or the ``scene_graph`` passed "
"in the constructor must be a valid SceneGraph.")
vis = self.vis[self.prefix]
# Make a fixed-seed generator for random colors for bodies.
color_generator = np.random.RandomState(seed=42)
for frame_id in inspector.GetAllFrameIds():
count = inspector.NumGeometriesForFrameWithRole(
frame_id, self._role)
if count == 0:
continue
if frame_id == inspector.world_frame_id():
name = "world"
else:
# Note: MBP declares frames with SceneGraph using `::`, we
# replace those with `/` here to expose the full tree to
# meshcat.
name = (inspector.GetOwningSourceName(frame_id) + "/" +
inspector.GetName(frame_id).replace("::", "/"))
frame_vis = vis[name]
for g_id in inspector.GetGeometries(frame_id, self._role):
color = 0xe5e5e5 # default color
alpha = 1.0
hydro_mesh = None
if self._role == Role.kIllustration:
props = inspector.GetIllustrationProperties(g_id)
if props and props.HasProperty("phong", "diffuse"):
rgba = props.GetProperty("phong", "diffuse")
# Convert Rgba from [0-1] to hex 0xRRGGBB.
color = int(255 * rgba.r()) * 256**2
color += int(255 * rgba.g()) * 256
color += int(255 * rgba.b())
alpha = rgba.a()
elif self._role == Role.kProximity:
# Pick a random color to make collision geometry
# visually distinguishable.
color = color_generator.randint(2**(24))
if self._prefer_hydro:
hydro_mesh = inspector. \
maybe_get_hydroelastic_mesh(g_id)
material = g.MeshLambertMaterial(color=color,
transparent=alpha != 1.,
opacity=alpha)
shape = inspector.GetShape(g_id)
X_FG = inspector.GetPoseInFrame(g_id).GetAsMatrix4()
if hydro_mesh is not None:
# We've got a hydroelastic mesh to load.
surface_mesh = hydro_mesh
if isinstance(hydro_mesh, VolumeMesh):
surface_mesh = ConvertVolumeToSurfaceMesh(hydro_mesh)
v_count = len(surface_mesh.triangles()) * 3
vertices = np.empty((v_count, 3), dtype=float)
normals = np.empty((v_count, 3), dtype=float)
mesh_verts = surface_mesh.vertices()
v = 0
for face in surface_mesh.triangles():
p_MA = mesh_verts[int(face.vertex(0))]
p_MB = mesh_verts[int(face.vertex(1))]
p_MC = mesh_verts[int(face.vertex(2))]
vertices[v, :] = tuple(p_MA)
vertices[v + 1, :] = tuple(p_MB)
vertices[v + 2, :] = tuple(p_MC)
p_AB_M = p_MB - p_MA
p_AC_M = p_MC - p_MA
n_M = np.cross(p_AB_M, p_AC_M)
nhat_M = n_M / np.sqrt(n_M.dot(n_M))
normals[v, :] = nhat_M
normals[v + 1, :] = nhat_M
normals[v + 2, :] = nhat_M
v += 3
geom = HydroTriSurface(vertices, normals)
elif isinstance(shape, Box):
geom = g.Box(
[shape.width(),
shape.depth(),
shape.height()])
elif isinstance(shape, Sphere):
geom = g.Sphere(shape.radius())
elif isinstance(shape, Cylinder):
geom = g.Cylinder(shape.length(), shape.radius())
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
R_GC = RotationMatrix.MakeXRotation(np.pi / 2.0).matrix()
X_FG[0:3, 0:3] = X_FG[0:3, 0:3].dot(R_GC)
elif isinstance(shape, (Mesh, Convex)):
geom = g.ObjMeshGeometry.from_file(shape.filename()[0:-3] +
"obj")
# Attempt to find a texture for the object by looking for
# an identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png,
# since they're both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material
# information should be gleaned from the SceneGraph
# constituents themselves, so that we visualize what the
# simulation is *actually* reasoning about rather than what
# files happen to be present.
candidate_texture_path = shape.filename()[0:-3] + "png"
if os.path.exists(candidate_texture_path):
material = g.MeshLambertMaterial(map=g.ImageTexture(
image=g.PngImage.from_file(
candidate_texture_path)))
# Make the uuid's deterministic for mesh geometry, to
# support caching at the zmqserver. This means that
# multiple (identical) geometries may have the same UUID,
# but testing suggests that meshcat + three.js are ok with
# it.
geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "mesh"))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "material"))
X_FG = X_FG.dot(tf.scale_matrix(shape.scale()))
else:
warnings.warn(f"Unsupported shape {shape} ignored")
continue
geometry_vis = frame_vis[str(g_id.get_value())]
geometry_vis.set_object(geom, material)
geometry_vis.set_transform(X_FG)
if frame_id in self.frames_to_draw:
AddTriad(self.vis,
name=name,
prefix=self.prefix + "/" + name,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
self.frames_to_draw.remove(frame_id)
if frame_id != inspector.world_frame_id():
self._dynamic_frames.append({
"id": frame_id,
"name": name,
})
# Loop through the input frames_to_draw list and warn the user if the
# frame_id does not exist in the scene graph.
for frame_id in self.frames_to_draw:
warnings.warn(f"Non-existent frame {frame_id} ignored")
continue
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[ ]:
dt = np.diff(data[:,0]) for i in range(num_agents): v = np.sqrt(data[:,i*4+3]**2 + data[:,i*4+4]**2) a = np.diff(v) / dt ax2.plot(data[0:-1,0],a) plt.show() if args.animate: import meshcat import meshcat.geometry as g import meshcat.transformations as tf # Create a new visualizer vis = meshcat.Visualizer() vis.open() for i in range(num_agents): vis["agent"+str(i)].set_object(g.Sphere(0.2)) for i, o in enumerate(map_data["map"]["obstacles"]): vis["obstacles"+str(i)].set_object(g.Box([1.0, 1.0, 0.2])) print(o) vis["obstacles"+str(i)].set_transform(tf.translation_matrix(np.array([o[0]+0.5, o[1]+0.5, 0]))) while True: for k in np.arange(0,data.shape[0],10): t = data[k,0] for i in range(num_agents): state = data[k,i*4+1:i*4+5] vis["agent" + str(i)].set_transform(tf.translation_matrix([state[0], state[1], 0])) time.sleep(0.01)
# print(data.dtype)
# # store in binary format
# with open("orca.npy", "wb") as f:
# np.save(f, data, allow_pickle=False)
num_agents = int((data.shape[1] - 1) / 4)
print(num_agents)
fig, ax = plt.subplots()
for i in range(num_agents):
ax.plot(data[:, i * 4 + 1], data[:, i * 4 + 2])
plt.show()
if args.animate:
# Create a new visualizer
vis = meshcat.Visualizer()
vis.open()
for i in range(num_agents):
vis["agent" + str(i)].set_object(g.Sphere(1.5))
while True:
for row in data:
t = row[0]
for i in range(num_agents):
state = row[i * 4 + 1:i * 4 + 5]
vis["agent" + str(i)].set_transform(
tf.translation_matrix([state[0], state[1], 0]))
time.sleep(0.01)
def load(self, context=None):
"""
Loads ``meshcat`` visualization elements.
Precondition:
Either the context is a valid Context for this system with the
geometry_query port connected or the ``scene_graph`` passed in the
constructor must be a valid SceneGraph.
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
if context and self.get_geometry_query_input_port().HasValue(context):
inspector = self.get_geometry_query_input_port().Eval(
context).inspector()
elif self._scene_graph:
inspector = self._scene_graph.model_inspector()
else:
raise RuntimeError(
"You must provide a valid Context for this system with the "
"geometry_query port connected or the ``scene_graph`` passed "
"in the constructor must be a valid SceneGraph.")
vis = self.vis[self.prefix]
for frame_id in inspector.all_frame_ids():
count = inspector.NumGeometriesForFrameWithRole(
frame_id, Role.kIllustration)
if count == 0:
continue
if frame_id == inspector.world_frame_id():
name = "world"
else:
# Note: MBP declares frames with SceneGraph using `::`, we
# replace those with `/` here to expose the full tree to
# meshcat.
name = (inspector.GetOwningSourceName(frame_id) + "/" +
inspector.GetName(frame_id).replace("::", "/"))
frame_vis = vis[name]
for g_id in inspector.GetGeometries(frame_id, Role.kIllustration):
color = 0xe5e5e5 # default color
alpha = 1.0
props = inspector.GetIllustrationProperties(g_id)
if props and props.HasProperty("phong", "diffuse"):
rgba = props.GetProperty("phong", "diffuse")
# Convert Rgba from [0-1] to hex 0xRRGGBB.
color = int(255 * rgba.r()) * 256**2
color += int(255 * rgba.g()) * 256
color += int(255 * rgba.b())
alpha = rgba.a()
material = g.MeshLambertMaterial(color=color,
transparent=alpha != 1.,
opacity=alpha)
shape = inspector.GetShape(g_id)
X_FG = inspector.GetPoseInFrame(g_id).GetAsMatrix4()
if isinstance(shape, Box):
geom = g.Box(
[shape.width(),
shape.depth(),
shape.height()])
elif isinstance(shape, Sphere):
geom = g.Sphere(shape.radius())
elif isinstance(shape, Cylinder):
geom = g.Cylinder(shape.length(), shape.radius())
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
R_GC = RotationMatrix.MakeXRotation(np.pi / 2.0).matrix()
X_FG[0:3, 0:3] = X_FG[0:3, 0:3].dot(R_GC)
elif isinstance(shape, Mesh):
geom = g.ObjMeshGeometry.from_file(shape.filename()[0:-3] +
"obj")
# Attempt to find a texture for the object by looking for
# an identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png,
# since they're both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material
# information should be gleaned from the SceneGraph
# constituents themselves, so that we visualize what the
# simulation is *actually* reasoning about rather than what
# files happen to be present.
candidate_texture_path = shape.filename()[0:-3] + "png"
if os.path.exists(candidate_texture_path):
material = g.MeshLambertMaterial(map=g.ImageTexture(
image=g.PngImage.from_file(
candidate_texture_path)))
# Make the uuid's deterministic for mesh geometry, to
# support caching at the zmqserver. This means that
# multiple (identical) geometries may have the same UUID,
# but testing suggests that meshcat + three.js are ok with
# it.
geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "mesh"))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
geom.contents + "material"))
X_FG = X_FG.dot(tf.scale_matrix(shape.scale()))
else:
warnings.warn(f"Unsupported shape {shape} ignored")
continue
geometry_vis = frame_vis[str(g_id.get_value())]
geometry_vis.set_object(geom, material)
geometry_vis.set_transform(X_FG)
if frame_id != inspector.world_frame_id():
self._dynamic_frames.append({
"id": frame_id,
"name": name,
})
# Place cylinder in scene
material = g.MeshLambertMaterial(reflectivity=1.0, sides=0)
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[ ]: