def load(self):
"""
Loads ``meshcat`` visualization elements.
Precondition:
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] = self._parse_name(link.name)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
meshcat_geom, material, element_local_tf = _convert_mesh(geom)
if meshcat_geom is not None:
cur_vis = (self.vis[self.prefix][source_name][str(
link.robot_num)][frame_name][str(j)])
cur_vis.set_object(meshcat_geom, material)
cur_vis.set_transform(element_local_tf)
def load(self):
"""
Loads ``meshcat`` visualization elements.
Precondition:
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] = self._parse_name(link.name)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently sets alpha=0 to make collision
# geometry "invisible". Ignore those geometries here.
if geom.color[3] == 0:
continue
meshcat_geom, material, element_local_tf = _convert_mesh(geom)
if meshcat_geom is not None:
cur_vis = (
self.vis[self.prefix][source_name][str(link.robot_num)]
[frame_name][str(j)])
cur_vis.set_object(meshcat_geom, material)
cur_vis.set_transform(element_local_tf)
def get_box_from_geom(scene_graph, visual_only=True):
# TODO: GeometryInstance, InternalGeometry, & GeometryContext to get the shape of objects
# TODO: get_contact_results_output_port
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
# https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
# 'link', 'num_links'
#builder.Connect(scene_graph.get_pose_bundle_output_port(),
# viz.get_input_port(0))
# TODO: hash bodies instead
box_from_geom = {}
for body_index in range(load_robot_msg.num_links):
# 'geom', 'name', 'num_geom', 'robot_num'
link = load_robot_msg.link[body_index]
[source_name, frame_name] = link.name.split("::")
# source_name = 'Source_1'
model_index = link.robot_num
visual_index = 0
for geom in sorted(link.geom, key=lambda g: g.position[::-1]): # sort by z, y, x
# 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type'
# string_data is empty...
if visual_only and (geom.color[3] == 0):
continue
# TODO: sort by lowest point on the bounding box?
# TODO: maybe just return the set of bodies in order and let the user decide what to with them
visual_index += 1 # TODO: affected by transparent visual
if geom.type == geom.BOX:
assert geom.num_float_data == 3
[width, length, height] = geom.float_data
extent = np.array([width, length, height]) / 2.
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
[radius] = geom.float_data
extent = np.array([radius, radius, radius])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
[radius, height] = geom.float_data
extent = np.array([radius, radius, height/2.])
#meshcat_geom = meshcat.geometry.Cylinder(
# geom.float_data[1], geom.float_data[0])
# In Drake, cylinders are along +z
#elif geom.type == geom.MESH:
# meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
# geom.string_data[0:-3] + "obj")
else:
#print("Robot {}, link {}, geometry {}: UNSUPPORTED GEOMETRY TYPE {} WAS IGNORED".format(
# model_index, frame_name, visual_index-1, geom.type))
continue
link_from_box = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position).GetAsIsometry3() #.GetAsMatrix4()
box_from_geom[model_index, frame_name, visual_index-1] = \
(BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom))
return box_from_geom
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 get_box_from_geom(scene_graph, visual_only=True):
# https://github.com/RussTedrake/underactuated/blob/master/src/underactuated/meshcat_visualizer.py
# https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_viewer_draw.lcm
mock_lcm = DrakeMockLcm()
DispatchLoadMessage(scene_graph, mock_lcm)
load_robot_msg = lcmt_viewer_load_robot.decode(
mock_lcm.get_last_published_message("DRAKE_VIEWER_LOAD_ROBOT"))
box_from_geom = {}
for body_index in range(load_robot_msg.num_links):
# 'geom', 'name', 'num_geom', 'robot_num'
link = load_robot_msg.link[body_index]
[source_name, frame_name] = link.name.split("::")
model_index = link.robot_num
visual_index = 0
for geom in sorted(link.geom,
key=lambda g: g.position[::-1]): # sort by z, y, x
# 'color', 'float_data', 'num_float_data', 'position', 'quaternion', 'string_data', 'type'
if visual_only and (geom.color[3] == 0):
continue
visual_index += 1
if geom.type == geom.BOX:
assert geom.num_float_data == 3
[width, length, height] = geom.float_data
extent = np.array([width, length, height]) / 2.
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
[radius] = geom.float_data
extent = np.array([radius, radius, radius])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
[radius, height] = geom.float_data
extent = np.array([radius, radius, height / 2.])
# In Drake, cylinders are along +z
else:
continue
link_from_box = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsIsometry3()
box_from_geom[model_index, frame_name, visual_index-1] = \
(BoundingBox(np.zeros(3), extent), link_from_box, get_geom_name(geom))
return box_from_geom
def rewrite_lcmlog_geom_string(fin, fout, pattern, replace):
"""
Look through the log, find all messages on the DRAKE_VIEWER_LOAD_ROBOT channel, and run a regex on all of the link_data.geom_data.string_data values contained in those messages. All other messages are untouched. The resulting data is written to a new lcmlog file [fout].
"""
while True:
header_bytes = fin.read(28)
if len(header_bytes) < 28:
break
sync, evno, tstamp, clen, dlen = struct.unpack('>LqqLL', header_bytes)
chan = fin.read(clen)
data = fin.read(dlen)
if str(chan) == 'DRAKE_VIEWER_LOAD_ROBOT':
msg = lcmt_viewer_load_robot.decode(data)
msg = replace_goem_string(msg, pattern, replace)
data = msg.encode()
dlen = len(data)
header_bytes = struct.pack('>LqqLL', sync, evno, tstamp, clen, dlen)
fout.write(header_bytes)
fout.write(chan)
fout.write(data)
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 handle_load_robot(raw):
load_robot_msgs.append(lcmt_viewer_load_robot.decode(raw))
def buildViewPatches(self, use_random_colors):
''' Generates view patches. self.viewPatches stores a list of
viewPatches for each body (starting at body id 1). A viewPatch is a
list of 2D coordinates in counterclockwise order forming the boundary
of a filled polygon representing a piece of visual geometry. '''
self.viewPatches = {}
self.viewPatchColors = {}
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"))
# Spawn a random color generator, in case we need to pick
# random colors for some bodies. Each body will be given
# a unique color when using this random generator, with
# each visual element of the body colored the same.
color = iter(plt.cm.rainbow(np.linspace(0, 1,
load_robot_msg.num_links)))
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
this_body_patches = []
this_body_colors = []
this_color = next(color)
for j in range(link.num_geom):
geom = link.geom[j]
# MultibodyPlant currently 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)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch = np.vstack((
geom.float_data[0]/2.*np.array([1, 1, 1, 1,
-1, -1, -1, -1]),
geom.float_data[1]/2.*np.array([1, 1, 1, 1,
-1, -1, -1, -1]),
geom.float_data[2]/2.*np.array([1, 1, -1, -1,
-1, -1, 1, 1])))
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
radius = geom.float_data[0]
sample_pts = np.arange(0., 2.*math.pi, 0.25)
patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3]
+ math.sin(pt)*self.Tview[1, 0:3]
for pt in sample_pts])
patch = np.transpose(patch)
patch *= radius
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
radius = geom.float_data[0]
length = geom.float_data[1]
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# I don't have access to the body to world transform
# yet; decide between drawing a box and circle assuming the
# T_body_to_world is will not rotate us out of the
# viewing plane.
z_axis = np.matmul(self.Tview[0:2, 0:3],
element_local_tf.multiply([0, 0, 1]))
if np.linalg.norm(z_axis) < 0.01:
# Draw a circle.
sample_pts = np.arange(0., 2.*math.pi, 0.25)
patch = np.vstack([math.cos(pt)*self.Tview[0, 0:3]
+ math.sin(pt)*self.Tview[1, 0:3]
for pt in sample_pts])
patch = np.transpose(patch)
patch *= radius
else:
# Draw a bounding box.
patch = np.vstack((
radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]),
radius*np.array([1, 1, 1, 1, -1, -1, -1, -1]),
(length/2)*np.array([1, 1, -1, -1, -1, -1, 1, 1])))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
patch = np.vstack((patch, np.ones((1, patch.shape[1]))))
patch = np.dot(element_local_tf.GetAsMatrix4(), patch)
# Project into 2D
patch = np.dot(self.Tview, patch)
# Close path if not closed
if (patch[:, -1] != patch[:, 0]).any():
patch = np.hstack((patch, patch[:, 0][np.newaxis].T))
this_body_patches.append(patch)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self.viewPatches[link.name] = this_body_patches
self.viewPatchColors[link.name] = this_body_colors