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 draw_transformation(self, state):
state = list(state)
origin = state[0:3]
origin[0] = state[1]
origin[1] = state[0]
origin[2] = state[2] + self.cube_dim[2] / 2.0
theta = state[4]
wheel_angle = state[6]
temp = tf.rotation_matrix(theta, [1, 0, 0]) # assume rotate about y
temp[0:3,
-1] = tf.translation_from_matrix(tf.translation_matrix(origin))
self.cube.set_transform(temp)
self.wheel.set_transform(tf.rotation_matrix(
-wheel_angle, [1, 0, 0])) # rotate the pole
def draw_transformation(self, state, dim=2.0):
nd = len(state)
state = list(state)
origin = state[0:3] # so you can edit
origin[0] = 0.0
origin[1] = state[0]
origin[2] = state[1] + self.cube_dim[2] / 2.0
theta = state[dim]
wheel_angle = state[len(state) / 2 - 1]
temp = tf.rotation_matrix(theta, [1, 0, 0]) # assume rotate about y
temp[0:3,
-1] = tf.translation_from_matrix(tf.translation_matrix(origin))
self.cube.set_transform(temp)
self.wheel.set_transform(tf.rotation_matrix(
wheel_angle, [1, 0, 0])) # rotate the pole
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 PublishAllGeometry(self):
n_bodies = self.rbtree.get_num_bodies()-1
all_meshcat_geometry = {}
for body_i in range(n_bodies):
body = self.rbtree.get_body(body_i+1)
# TODO(gizatt) Replace these body-unique indices
# with more readable body.get_model_name() or other
# model index information when an appropriate
# function gets bound in pydrake.
body_name = body.get_name() + ("(%d)" % body_i)
if self.draw_collision:
draw_elements = [self.rbtree.FindCollisionElement(k)
for k in body.get_collision_element_ids()]
else:
draw_elements = body.get_visual_elements()
for element_i, element in enumerate(draw_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
np.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
else:
print "UNSUPPORTED GEOMETRY TYPE ",\
geom.getShape(), " IGNORED"
continue
rgba = [1., 0.7, 0., 1.]
if not self.draw_collision:
rgba = element.getMaterial()
self.vis[self.prefix][body_name][str(element_i)]\
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=Rgba2Hex(rgba)))
self.vis[self.prefix][body_name][str(element_i)].\
set_transform(element_local_tf)
def PublishAllGeometry(self):
n_bodies = self.rbtree.get_num_bodies()-1
all_meshcat_geometry = {}
for body_i in range(n_bodies):
body = self.rbtree.get_body(body_i+1)
# TODO(gizatt) Replace these body-unique indices
# with more readable body.get_model_name() or other
# model index information when an appropriate
# function gets bound in pydrake.
body_name = body.get_name() + ("(%d)" % body_i)
if self.draw_collision:
draw_elements = [self.rbtree.FindCollisionElement(k)
for k in body.get_collision_element_ids()]
else:
draw_elements = body.get_visual_elements()
for element_i, element in enumerate(draw_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
else:
print "UNSUPPORTED GEOMETRY TYPE ",\
geom.getShape(), " IGNORED"
continue
rgba = [1., 0.7, 0., 1.]
if not self.draw_collision:
rgba = element.getMaterial()
self.vis[self.prefix][body_name][str(element_i)]\
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=Rgba2Hex(rgba)))
self.vis[self.prefix][body_name][str(element_i)].\
set_transform(element_local_tf)
def AlignSceneToModel(scene_points, model_points, max_distance=0.05,
num_iters=10, num_sample_points=250):
"""
Finds a good (x, y, sin(theta), cos(theta) transform between scene_points
and model_points.
Args:
@param scene_points An Nx3 numpy array of scene points.
@param model_points An Nx3 numpy array of model points.
@param max_distance float. The maximum distance in meters to consider of
nearest neighbors between scene_points and model_points.
@param num_iters int. The number of times to try the best fit optimization
from different initial guesses.
@param num_sample_points: int. The number of points to sample from the scene
to limit the time the algorithm takes to run.
Returns:
@return best_X_MS The best 4x4 homogenous transform between scene_points and
model_points.
@return best_cost float. The cost function evaluated at best_X_MS.
"""
scene_sample = scene_points[np.random.choice(scene_points.shape[0],
num_sample_points,
replace=False),
:]
homogenous_scene = np.ones((4, scene_sample.shape[0]))
homogenous_model = np.ones((4, model_points.shape[0]))
homogenous_scene[:3, :] = scene_sample.T
homogenous_model[:3, :] = model_points.T
centroid_scene = np.mean(homogenous_scene, axis=1)
centroid_model = np.mean(homogenous_model, axis=1)
best_X_MS = None
best_cost = float('inf')
for i in range(num_iters):
init_theta = np.random.uniform(0, 2*np.pi)
init_guess = reduce(np.dot,
[tf.translation_matrix((centroid_model[0],
centroid_model[1],
0.)),
tf.rotation_matrix(init_theta, (0, 0, 1)),
tf.translation_matrix((-centroid_scene[0],
-centroid_scene[1],
0.))])
X_MS, cost = FindBestFitTransform(
homogenous_scene,homogenous_model, init_guess, max_distance)
if cost < best_cost:
best_cost = cost
best_X_MS = X_MS
return best_X_MS, best_cost
def load(self):
"""
Loads `meshcat` visualization elements.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(self._scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = link.name.split("::")
for j in range(link.num_geom):
geom = link.geom[j]
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom.type == geom.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
else:
print "UNSUPPORTED GEOMETRY TYPE ", \
geom.type, " IGNORED"
continue
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)]\
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=Rgba2Hex(geom.color)))
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)].set_transform(element_local_tf)
def runTest(self):
"""
Test that we can render meshes from raw vertices and faces as
numpy arrays
"""
v = self.vis["triangular_mesh"]
v.set_transform(tf.rotation_matrix(np.pi / 2, [0., 0, 1]))
vertices = np.array([[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]])
faces = np.array([[0, 1, 2], [3, 0, 2]])
v.set_object(g.TriangularMeshGeometry(vertices, faces),
g.MeshLambertMaterial(color=0xeedd22, wireframe=True))
v = self.vis["triangular_mesh_w_vertex_coloring"]
v.set_transform(
tf.translation_matrix([1, 0, 0]).dot(
tf.rotation_matrix(np.pi / 2, [0, 0, 1])))
colors = vertices
v.set_object(g.TriangularMeshGeometry(vertices, faces, colors),
g.MeshLambertMaterial(vertexColors=True, wireframe=True))
def runTest(self):
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]))
animation = meshcat.animation.Animation()
with animation.at_frame(v, 0) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([0, 0, 0]))
with animation.at_frame(v, 30) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([2, 0, 0]).dot(tf.rotation_matrix(np.pi/2, [0, 0, 1])))
v.set_animation(animation)
def runTest(self):
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]))
animation = meshcat.animation.Animation()
with animation.at_frame(v, 0) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([0, 0, 0]))
with animation.at_frame(v, 30) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([2, 0, 0]).dot(tf.rotation_matrix(np.pi/2, [0, 0, 1])))
with animation.at_frame(v, 0) as frame_vis:
frame_vis["/Cameras/default/rotated/<object>"].set_property("zoom", "number", 1)
with animation.at_frame(v, 30) as frame_vis:
frame_vis["/Cameras/default/rotated/<object>"].set_property("zoom", "number", 0.5)
v.set_animation(animation)
def PublishAllGeometry(self):
n_bodies = self.rbtree.get_num_bodies() - 1
all_meshcat_geometry = {}
for body_i in range(n_bodies):
body = self.rbtree.get_body(body_i + 1)
body_name = body.get_name()
visual_elements = body.get_visual_elements()
this_body_patches = []
for element_i, element in enumerate(visual_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
print extra_rotation
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
else:
print "UNSUPPORTED GEOMETRY TYPE ",\
geom.getShape(), " IGNORED"
continue
self.vis[body_name][str(element_i)].set_object(
meshcat_geom)
print body_name, element_i, element_local_tf
self.vis[body_name][str(element_i)].set_transform(
element_local_tf)
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 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 PublishAllGeometry(self):
n_bodies = self.rbtree.get_num_bodies() - 1
all_meshcat_geometry = {}
for body_i in range(n_bodies):
body = self.rbtree.get_body(body_i + 1)
# TODO(gizatt) Replace these body-unique indices
# with more readable body.get_model_name() or other
# model index information when an appropriate
# function gets bound in pydrake.
body_name = body.get_name() + ("(%d)" % body_i)
visual_elements = body.get_visual_elements()
this_body_patches = []
for element_i, element in enumerate(visual_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
print extra_rotation
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
else:
print "UNSUPPORTED GEOMETRY TYPE ",\
geom.getShape(), " IGNORED"
continue
def rgba2hex(rgb):
''' Turn a list of R,G,B elements (any indexable
list of >= 3 elements will work), where each element
is specified on range [0., 1.], into the equivalent
24-bit value 0xRRGGBB. '''
val = 0.
for i in range(3):
val += (256**(2 - i)) * (255. * rgb[i])
return val
self.vis[self.prefix][body_name][str(element_i)]\
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=rgba2hex(element.getMaterial())))
self.vis[self.prefix][body_name][str(element_i)].\
set_transform(element_local_tf)
def load(self):
"""
Loads `meshcat` visualization elements.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
self.vis[self.prefix].delete()
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(self._scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = link.name.split("::")
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currenly sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom.type == geom.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
# Turn a list of R,G,B elements (any indexable list of >= 3
# elements will work), where each element is specified on range
# [0., 1.], into the equivalent 24-bit value 0xRRGGBB.
def Rgb2Hex(rgb):
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)]\
.set_object(meshcat_geom,
meshcat.geometry
.MeshLambertMaterial(
color=Rgb2Hex(geom.color)))
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)].set_transform(element_local_tf)
def load(self):
"""
Loads `meshcat` visualization elements.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
self.vis[self.prefix].delete()
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(self._scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = link.name.split("::")
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currenly sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1], geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom.type == geom.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
else:
print "UNSUPPORTED GEOMETRY TYPE ", \
geom.type, " IGNORED"
continue
# Turn a list of R,G,B elements (any indexable list of >= 3
# elements will work), where each element is specified on range
# [0., 1.], into the equivalent 24-bit value 0xRRGGBB.
def Rgb2Hex(rgb):
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
self.vis[self.prefix][source_name][str(link.robot_num)][
frame_name][str(j)]\
.set_object(meshcat_geom,
meshcat.geometry
.MeshLambertMaterial(
color=Rgb2Hex(geom.color)))
self.vis[self.prefix][source_name][str(
link.robot_num)][frame_name][str(j)].set_transform(
element_local_tf)
def _convert_mesh(geom):
# Given a LCM geometry message, forms a meshcat geometry and material
# for that geometry, as well as a local tf to the meshcat geometry
# that matches the LCM geometry.
# For MESH geometry, this function checks if a texture file exists next
# to the mesh file, and uses that to provide the material information if
# present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry
# not satisfying the above, this function uses the geom.color field to
# create a flat color for the object. In the case of other geometry types,
# both fields are returned as None.
meshcat_geom = None
material = None
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = (
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3]))
elif geom.type == geom.MESH:
meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
scale = geom.float_data[:3]
element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale))
# 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_png = geom.string_data[0:-3] + "png"
if os.path.exists(candidate_texture_path_png):
material = meshcat.geometry.MeshLambertMaterial(
map=meshcat.geometry.ImageTexture(
image=meshcat.geometry.PngImage.from_file(
candidate_texture_path_png)))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
material = meshcat.geometry.MeshLambertMaterial(
color=rgb_2_hex(geom.color[:3]),
transparent=geom.color[3] != 1.,
opacity=geom.color[3])
return meshcat_geom, material, element_local_tf
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# Frame C is located at the contact point, but with the world frame
# orientation.
if force_norm < 1e-6:
X_CGeom = tf.identity_matrix()
else:
# Rotates [0,1,0] to contact_force/force_norm.
angle_axis = np.cross(
np.array([0, 1, 0]),
contact_info.contact_force() / force_norm)
X_CGeom = tf.rotation_matrix(
np.arcsin(np.linalg.norm(angle_axis)), angle_axis)
X_WC = tf.translation_matrix(contact_info.contact_point())
cvis.set_transform(X_WC @ X_CGeom)
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
def DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
if (not self._warned_pose_bundle_input_port_connected
and self.get_input_port(0).HasValue(context)):
_warn_deprecated(
"The pose_bundle input port of MeshcatContactVisualizer is"
"deprecated; use the geometry_query inport port instead.",
date="2021-04-01")
self._warned_pose_bundle_input_port_connected = True
contact_results = self.EvalAbstractInput(context, 1).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# Frame C is located at the contact point, but with the world frame
# orientation.
if force_norm < 1e-6:
X_CGeom = tf.identity_matrix()
else:
# Rotates [0,1,0] to contact_force/force_norm.
angle_axis = np.cross(
np.array([0, 1, 0]),
contact_info.contact_force() / force_norm)
X_CGeom = tf.rotation_matrix(
np.arcsin(np.linalg.norm(angle_axis)), angle_axis)
X_WC = tf.translation_matrix(contact_info.contact_point())
cvis.set_transform(X_WC @ X_CGeom)
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
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]))
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):
vis['point_{}_{}'.format(idx, ptidx)].set_object(
g.Sphere(0.05),
red_material)
def PublishAllGeometry(self):
n_bodies = self.rbtree.get_num_bodies() - 1
all_meshcat_geometry = {}
for body_i in range(n_bodies):
body = self.rbtree.get_body(body_i + 1)
# TODO(gizatt) Replace these body-unique indices
# with more readable body.get_model_name() or other
# model index information when an appropriate
# function gets bound in pydrake.
body_name = body.get_name() + ("(%d)" % body_i)
self.body_names.append(body_name)
if self.old_mrbv is not None and \
body_name in self.old_mrbv.body_names:
continue
self.vis[self.prefix][body_name].delete()
if self.draw_collision:
draw_elements = [
self.rbtree.FindCollisionElement(k)
for k in body.get_collision_element_ids()
]
else:
draw_elements = body.get_visual_elements()
for element_i, element in enumerate(draw_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
else:
print "UNSUPPORTED GEOMETRY TYPE ",\
geom.getShape(), " IGNORED"
continue
def rgba2hex(rgb):
''' Turn a list of R,G,B elements (any indexable
list of >= 3 elements will work), where each element
is specified on range [0., 1.], into the equivalent
24-bit value 0xRRGGBB. '''
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
rgba = [1., 0.7, 0., 1.]
if not self.draw_collision:
rgba = element.getMaterial()
# Publish this object to the visualizer
# if we have been instructed to reset bodies
# with this name, or if the vis has no record
# of this body.
self.vis[self.prefix][body_name][str(element_i)]\
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=rgba2hex(rgba)))
self.vis[self.prefix][body_name][str(element_i)].\
set_transform(element_local_tf)
# Finally, nuke any bodies that used to exist that don't exist
# now.
if self.old_mrbv is not None:
for body_name in self.old_mrbv.body_names:
if body_name not in self.body_names:
self.vis[self.prefix][body_name].delete()
def _add_rbt_geometry(self, rbt, rbt_key, draw_collision=False):
# type: (RigidBodyTree, str, bool) -> None
"""
Add the geometry of a RigidBodyTree to the meshcat visualizer.
Note that later update to rbt WOULD NOT be reflected
:param rbt:
:param rbt_key: The key to index the rigid body tree
:param draw_collision:
:return:
"""
n_bodies = rbt.get_num_bodies() - 1
# all_meshcat_geometry = {}
for body_i in range(n_bodies):
# Get the body index
body = rbt.get_body(body_i + 1)
body_name = rbt_key + body.get_name() + ("(%d)" % body_i)
if draw_collision:
draw_elements = [rbt.FindCollisionElement(k) for k in body.get_collision_element_ids()]
else:
draw_elements = body.get_visual_elements()
for element_i, element in enumerate(draw_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
elif geom_type == Shape.MESH_POINTS:
# The shape of points is (3, N)
points = geom.getPoints() # type: np.ndarray
n_points = points.shape[1]
color = MeshCatVisualizer._make_meshcat_color(n_points, r=255, g=0, b=0)
meshcat_geom = meshcat_geometry.PointCloud(points, color.T, size=0.01)
self._meshcat_vis[self._prefix][body_name][str(element_i)] \
.set_object(meshcat_geom)
self._meshcat_vis[self._prefix][body_name][str(element_i)]. \
set_transform(element_local_tf)
self._rbt_geometry_element_set.add(body_name)
continue
else:
print "UNSUPPORTED GEOMETRY TYPE ", \
geom.getShape(), " IGNORED"
continue
# The color of the body
rgba = [1., 0.7, 0., 1.]
if not draw_collision:
rgba = element.getMaterial()
self._meshcat_vis[self._prefix][body_name][str(element_i)] \
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=MeshCatVisualizer.rgba2hex(rgba)))
self._meshcat_vis[self._prefix][body_name][str(element_i)]. \
set_transform(element_local_tf)
self._rbt_geometry_element_set.add(body_name)
def _convert_mesh(geom):
# Given a LCM geometry message, forms a meshcat geometry and material
# for that geometry, as well as a local tf to the meshcat geometry
# that matches the LCM geometry.
# For MESH geometry, this function checks if a texture file exists next
# to the mesh file, and uses that to provide the material information if
# present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry
# not satisfying the above, this function uses the geom.color field to
# create a flat color for the object. In the case of other geometry types,
# both fields are returned as None.
meshcat_geom = None
material = None
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi/2., [1, 0, 0])
element_local_tf[0:3, 0:3] = (
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3]))
elif geom.type == geom.MESH:
meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
scale = geom.float_data[:3]
element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale))
# 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_png = geom.string_data[0:-3] + "png"
if os.path.exists(candidate_texture_path_png):
material = meshcat.geometry.MeshLambertMaterial(
map=meshcat.geometry.ImageTexture(
image=meshcat.geometry.PngImage.from_file(
candidate_texture_path_png)))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
material = meshcat.geometry.MeshLambertMaterial(
color=rgb_2_hex(geom.color[:3]),
transparent=geom.color[3] != 1.,
opacity=geom.color[3])
return meshcat_geom, material, element_local_tf
light_attenuations = [40.0, 40.0, 40.0, 40.0]
else:
raise NotImplementedError("Rotating light pattern_name %s" % pattern_name)
return light_locations, light_attenuations
if __name__ == "__main__":
logging.basicConfig(filename='../out/info.log', filemode='w', level=logging.DEBUG)
# First connect to meshcat: this'll block if there's no server + display a helpful
# reminder for me to start a server.
print("Opening meshcat server at default url: make sure meshcat-server is running.")
vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
vis.delete()
camera_frame_vis = vis["camera_frame"]
# Makes point cloud look "upright" in meshcat vis
camera_frame_vis.set_transform(tf.rotation_matrix(np.pi/2, [1., 0., 0]))
meshcat_draw_frustrum(camera_frame_vis,
TF=get_extrinsics(),
K=get_projector_intrinsics(),
near_distance=0.025,
far_distance=1.,
w=1280,
h=768
)
modes = [
"Off",
"Depth recoloring",
"Normal recoloring",
