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 make_frame(vis, name, h, radius, o=1.0):
"""Add a red-green-blue triad to the Meschat visualizer.
Args:
vis (MeshCat Visualizer): the visualizer
name (string): name for this frame (should be unique)
h (float): height of frame visualization
radius (float): radius of frame visualization
o (float): opacity
"""
vis[name]['x'].set_object(
g.Cylinder(height=h, radius=radius),
g.MeshLambertMaterial(color=0xff0000, reflectivity=0.8, opacity=o))
rotate_x = mtf.rotation_matrix(np.pi / 2.0, [0, 0, 1])
rotate_x[0, 3] = h / 2
vis[name]['x'].set_transform(rotate_x)
vis[name]['y'].set_object(
g.Cylinder(height=h, radius=radius),
g.MeshLambertMaterial(color=0x00ff00, reflectivity=0.8, opacity=o))
rotate_y = mtf.rotation_matrix(np.pi / 2.0, [0, 1, 0])
rotate_y[1, 3] = h / 2
vis[name]['y'].set_transform(rotate_y)
vis[name]['z'].set_object(
g.Cylinder(height=h, radius=radius),
g.MeshLambertMaterial(color=0x0000ff, reflectivity=0.8, opacity=o))
rotate_z = mtf.rotation_matrix(np.pi / 2.0, [1, 0, 0])
rotate_z[2, 3] = h / 2
vis[name]['z'].set_transform(rotate_z)
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_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 draw_bond(v, label, p1, d, radius, color=0xffffff):
H = np.linalg.norm(d)
R = np.linalg.norm(d[:2])
e = d / H
x = np.array([0, 1, 0])
rot = -acos(e @ x)
if -1 < e @ x < 1:
axis = np.cross(e, x)
v[label].set_object(
g.Mesh(g.Cylinder(H, radius), g.MeshLambertMaterial(color=color)))
v[label].set_transform(
tf.translation_matrix(p1).dot(
tf.rotation_matrix(rot, axis).dot(
tf.translation_matrix([0, H / 2, 0]))))
else:
v[label].set_object(
g.Mesh(g.Cylinder(H, radius), g.MeshLambertMaterial(color=color)))
v[label].set_transform(tf.translation_matrix(p1 + d / 2))
def visualize(self, states, dt):
import meshcat
import meshcat.geometry as g
import meshcat.transformations as tf
import time
# import meshcat.animation as anim
# Create a new visualizer
vis = meshcat.Visualizer()
vis.open()
vis["cart"].set_object(g.Box([0.2, 0.5, 0.2]))
vis["pole"].set_object(g.Cylinder(self.length_pole, 0.01))
# animation = anim.Animation()
# for i, state in enumerate(states):
# with animation.at_frame(vis, i*10) as frame:
# print(frame)
# frame["cart"].set_transform(tf.translation_matrix([0, state[0], 0]))
# frame["pole"].set_transform(
# tf.translation_matrix([0, state[0] + self.length_pole/2, 0]).dot(
# tf.rotation_matrix(np.pi/2 + state[1], [1,0,0], [0,-self.length_pole/2,0])))
# vis.set_animation(animation, play=True, repeat=10)
# time.sleep(10)
# # anim.convert_frame_to_video()
while True:
# vis["cart"].set_transform(tf.translation_matrix([0, 0, 0]))
# vis["pole"].set_transform(
# tf.translation_matrix([0, 0 + self.length_pole/2, 0]).dot(
# tf.rotation_matrix(np.pi/2 + 0, [1,0,0], [0,-self.length_pole/2,0])))
# time.sleep(dt)
for state in states:
vis["cart"].set_transform(
tf.translation_matrix([0, state[0], 0]))
vis["pole"].set_transform(
tf.translation_matrix(
[0, state[0] + self.length_pole / 2, 0]).dot(
tf.rotation_matrix(np.pi / 2 + state[1], [1, 0, 0],
[0, -self.length_pole / 2, 0])))
time.sleep(dt)
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 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
)
surface_and_bottom = (ray_origin, ray_direction, expected)
# surface and top
ray_origin = (-2, 0.2, 0.0)
ray_direction = norm((1.0, 0.2, 0.2))
expected = (
(-0.9082895433880116, 0.41834209132239775, 0.2183420913223977),
(0.5, 0.7, 0.5)
)
surface_and_top = (ray_origin, ray_direction, expected)
tests = (touching, end_caps_only, surface_and_bottom, surface_and_top)
# 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)
ray_origin = (-2, 0.2, 0.0)
ray_direction = norm((1.0, 0.2, -0.2))
expected = ((-0.9082895433880116, 0.41834209132239775, -0.2183420913223977),
(0.5, 0.7, -0.5))
surface_and_bottom = (ray_origin, ray_direction, expected)
# surface and top
ray_origin = (-2, 0.2, 0.0)
ray_direction = norm((1.0, 0.2, 0.2))
expected = ((-0.9082895433880116, 0.41834209132239775, 0.2183420913223977),
(0.5, 0.7, 0.5))
surface_and_top = (ray_origin, ray_direction, expected)
tests = (touching, end_caps_only, surface_and_bottom, surface_and_top)
# 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):
def animate(self, chain, states, framerate=5, 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()
anim = Animation()
vertices = chain.get_vertex_coords()
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)
for i in range(len(states)):
chain.update(states[i])
with anim.at_frame(vis, framerate * i) as frame:
for i, link in enumerate(chain.linkArray):
if link.meshObj == None:
continue
boxVis = frame["link:" + link.name]
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 in range(int(vertices.shape[0] / 2)):
p1 = vertices[2 * i]
p2 = vertices[2 * i + 1]
cylinder_transform = getCylinderTransform(p1, p2)
boxVis = vis["link" + str(i)]
boxVis.set_object(g.Cylinder(1, 0.01))
boxVis.set_transform(cylinder_transform)
for i in range(len(states)):
chain.update(states[i])
with anim.at_frame(vis, framerate * i) as frame:
vertices = chain.get_vertex_coords()
for i in range(int(vertices.shape[0] / 2)):
p1 = vertices[2 * i]
p2 = vertices[2 * i + 1]
cylinder_transform = getCylinderTransform(p1, p2)
boxVis = frame["link" + str(i)]
boxVis.set_transform(cylinder_transform)
vis.set_animation(anim)
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,
})
