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 main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--target_realtime_rate",
type=float,
default=1.0,
help="Desired rate relative to real time. See documentation for "
"Simulator::set_target_realtime_rate() for details.")
parser.add_argument("--simulation_time",
type=float,
default=10.0,
help="Desired duration of the simulation in seconds.")
parser.add_argument(
"--time_step",
type=float,
default=0.,
help="If greater than zero, the plant is modeled as a system with "
"discrete updates and period equal to this time_step. "
"If 0, the plant is modeled as a continuous system.")
args = parser.parse_args()
file_name = FindResourceOrThrow(
"drake/examples/multibody/cart_pole/cart_pole.sdf")
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
AddModelFromSdfFile(file_name=file_name,
plant=cart_pole,
scene_graph=scene_graph)
cart_pole.AddForceElement(UniformGravityFieldElement([0, 0, -9.81]))
cart_pole.Finalize(scene_graph)
assert cart_pole.geometry_source_is_registered()
builder.Connect(
cart_pole.get_geometry_poses_output_port(),
scene_graph.get_source_pose_port(cart_pole.get_source_id()))
lcm = DrakeLcm()
ConnectVisualization(scene_graph=scene_graph, builder=builder, lcm=lcm)
diagram = builder.Build()
DispatchLoadMessage(scene_graph=scene_graph, lcm=lcm)
diagram_context = diagram.CreateDefaultContext()
cart_pole_context = diagram.GetMutableSubsystemContext(
cart_pole, diagram_context)
cart_pole_context.FixInputPort(
cart_pole.get_actuation_input_port().get_index(), [0])
cart_slider = cart_pole.GetJointByName("CartSlider")
pole_pin = cart_pole.GetJointByName("PolePin")
cart_slider.set_translation(context=cart_pole_context, translation=0.)
pole_pin.set_angle(context=cart_pole_context, angle=2.)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(args.target_realtime_rate)
simulator.Initialize()
simulator.StepTo(args.simulation_time)
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()
mock_lcm_subscriber = Subscriber(
lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# 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)
# Draw the frames in self.frames_to_draw.
if "::" in frame_name:
robot_name, link_name = self._parse_name(frame_name)
else:
robot_name = "world"
link_name = frame_name
if (robot_name in self.frames_to_draw.keys() and
link_name in self.frames_to_draw[robot_name]):
prefix = (self.prefix + '/' + source_name + '/' +
str(link.robot_num) + '/' + frame_name)
AddTriad(
self.vis,
name="frame",
prefix=prefix,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
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 _build_body_patches(self, use_random_colors,
substitute_collocated_mesh_files):
"""
Generates body patches. self._patch_Blist stores a list of patches for
each body (starting at body id 1). A body patch is a list of all 3D
vertices of a piece of visual geometry.
"""
self._patch_Blist = {}
self._patch_Blist_colors = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
# TODO(SeanCurtis-TRI): Use SceneGraph inspection instead of mocking
# LCM and inspecting the generated message.
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# 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
X_BG = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)), geom.position)
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Draw a bounding box.
patch_G = 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]
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch_G = np.vstack([
np.sin(lati) * np.cos(longi),
np.sin(lati) * np.sin(longi),
np.cos(lati)
])
patch_G *= 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
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch_G = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif geom.type == geom.MESH:
filename = geom.string_data
base, ext = os.path.splitext(filename)
if (ext.lower() is not ".obj") and \
substitute_collocated_mesh_files:
# Check for a co-located .obj file (case insensitive).
for f in glob.glob(base + '.*'):
if f[-4:].lower() == '.obj':
filename = f
break
if filename[-4:].lower() != '.obj':
raise RuntimeError("The given file " + filename +
" is not "
"supported and no alternate " +
base + ".obj could be found.")
if not os.path.exists(filename):
raise FileNotFoundError(errno.ENOENT,
os.strerror(errno.ENOENT),
filename)
mesh = ReadObjToSurfaceMesh(filename)
patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
# Compute pose in body.
patch_B = X_BG @ patch_G
# Close path if not closed.
if (patch_B[:, -1] != patch_B[:, 0]).any():
patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T))
this_body_patches.append(patch_B)
if use_random_colors:
this_body_colors.append(this_color)
else:
this_body_colors.append(geom.color)
self._patch_Blist[link.name] = this_body_patches
self._patch_Blist_colors[link.name] = this_body_colors
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 all 3D vertices of a piece of visual geometry. '''
self.viewPatches = {}
self.viewPatchColors = {}
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# 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]
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch = np.vstack([
np.sin(longi) * np.cos(lati),
np.sin(longi) * np.sin(lati),
np.cos(lati)
])
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
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif geom.type == geom.MESH:
# TODO(gizatt): Remove trimesh and shapely dependency when
# vertex information is accessible from the SceneGraph
# interface.
mesh = trimesh.load(geom.string_data)
patch = mesh.vertices.T
# Apply scaling
for i in range(3):
patch[i, :] *= geom.float_data[i]
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)
# 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
def add_drake_visualizer(scene_graph, builder):
lcm = DrakeLcm()
ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph, lcm=lcm)
DispatchLoadMessage(scene_graph,
lcm) # TODO: only update viewer after a plan is found
return lcm # Important that variable is saved
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 load1(self):
"""
Loads all visualization elements in the Blender server.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
self.bsi.send_remote_call("initialize_scene")
# Keeps track of registered bounding boxes, to keep us from
# having to register + delete brand new objects every cycle.
# If more than this number is needed in a given cycle,
# more are registered and this number is increased.
# If less are needed, the unused ones are made invisible.
# The attributes of all bounding boxes are updated every cycle.
self.num_registered_bounding_boxes = 0
self.num_visible_bounding_boxes = 0
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
# warnings.filterwarnings("ignore", message="(Advanced) Explicitly dispatches an LCM load message")
warnings.simplefilter("ignore", DrakeDeprecationWarning)
DispatchLoadMessage(self._scene_graph, mock_lcm)
# SendLoadMessage() TODO
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Load all the elements over on the Blender side.
self.num_link_geometries_by_link_name = {}
self.link_subgeometry_local_tfs_by_link_name = {}
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = self._parse_name(link.name)
print('frame_name', frame_name)
print('link.name', link.name)
self.num_link_geometries_by_link_name[link.name] = link.num_geom
tfs = []
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
geom_name = self._format_geom_name(source_name, frame_name, j)
tfs.append(
RigidTransform(RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4())
# It will have a material with this key.
# We will set it up after deciding whether it's
# a mesh with a texture or not...
material_key = "material_" + geom_name
material_key_assigned = False
# Check overrides
for override in self.material_overrides:
if override[0].match(geom_name):
print("Using override ", override[0].pattern,
" on name ", geom_name, " with applied args ",
override[1])
self.bsi.send_remote_call("register_material",
name=material_key,
**(override[1]))
material_key_assigned = True
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Blender cubes are 2x2x2 by default
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="cube",
scale=[x * 0.5 for x in geom.float_data[:3]],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="sphere",
scale=geom.float_data[0],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
# Blender cylinders are r=1, h=2 by default
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="cylinder",
scale=[
geom.float_data[0], geom.float_data[0], 0.5 * geom.
float_data[1]
],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.MESH:
print('geom_name', geom_name)
print('path', geom.string_data[0:-3])
print('scale', geom.float_data[:3])
print('loc', geom.position)
print('quat', geom.quaternion)
print('material_key', material_key)
do_load_geom = lambda: self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="obj",
path=geom.string_data[0:-3] + "obj",
scale=geom.float_data[:3],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
# Attempt to find a texture for the object by looking for an
# identically-named *.png next to the model.
# 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 not material_key_assigned and os.path.exists(
candidate_texture_path_png):
material_key_assigned = self.bsi.send_remote_call(
"register_material",
name=material_key,
material_type="color_texture",
path=candidate_texture_path_png)
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
if not material_key_assigned:
material_key_assigned = self.bsi.send_remote_call(
"register_material",
name=material_key,
material_type="color",
color=geom.color[:4])
print('color', geom.color[:4])
# Finally actually load the geometry now that the material
# is registered.
do_load_geom()
self.link_subgeometry_local_tfs_by_link_name[link.name] = tfs
for i, camera_tf in enumerate(self.camera_tfs):
camera_tf_post = self.global_transform.multiply(camera_tf)
self.bsi.send_remote_call(
"register_camera",
name="cam_%d" % i,
location=camera_tf_post.translation().tolist(),
quaternion=camera_tf_post.quaternion().wxyz().tolist(),
angle=np.pi / 2.)
self.bsi.send_remote_call("configure_rendering",
camera_name='cam_%d' % i,
resolution=[640, 480],
file_format="JPEG",
taa_render_samples=20,
cycles=True)
if self.env_map_path:
self.bsi.send_remote_call("set_environment_map",
path=self.env_map_path)
def load(self):
"""
Loads all visualization elements in the Blender server.
@pre The `scene_graph` used to construct this object must be part of a
fully constructed diagram (e.g. via `DiagramBuilder.Build()`).
"""
self.bsi.send_remote_call("initialize_scene")
# Intercept load message via mock LCM.
mock_lcm = DrakeMockLcm()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# Load all the elements over on the Blender side.
self.num_link_geometries_by_link_name = {}
self.link_subgeometry_local_tfs_by_link_name = {}
self.geom_name_to_color_map = {}
num_allocated_labels = 0
max_num_objs = sum([
load_robot_msg.link[i].num_geom
for i in range(load_robot_msg.num_links)
])
cmap = plt.cm.get_cmap("hsv", max_num_objs + 1)
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[source_name, frame_name] = self._parse_name(link.name)
self.num_link_geometries_by_link_name[link.name] = link.num_geom
tfs = []
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
geom_name = self._format_geom_name(source_name, frame_name,
link.robot_num, j)
tfs.append(
RigidTransform(RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4())
# It will have a material with this key.
material_key = "material_" + geom_name
label_num = num_allocated_labels
self.geom_name_to_color_map[geom_name] = cmap(label_num)
num_allocated_labels += 1
material_key_assigned = self.bsi.send_remote_call(
"register_material",
name=material_key,
material_type="emission",
color=cmap(label_num))
if geom.type == geom.BOX:
assert geom.num_float_data == 3
# Blender cubes are 2x2x2 by default
self.bsi.send_remote_call(
"register_object",
name="obj_" + geom_name,
type="cube",
scale=[x * 0.5 for x in geom.float_data[:3]],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
self.bsi.send_remote_call("register_object",
name="obj_" + geom_name,
type="sphere",
scale=geom.float_data[0],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
# Blender cylinders are r=1, h=2 by default
self.bsi.send_remote_call("register_object",
name="obj_" + geom_name,
type="cylinder",
scale=[
geom.float_data[0],
geom.float_data[0],
0.5 * geom.float_data[1]
],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
elif geom.type == geom.MESH:
self.bsi.send_remote_call("register_object",
name="obj_" + geom_name,
type="obj",
path=geom.string_data[0:-3] +
"obj",
scale=geom.float_data[:3],
location=geom.position,
quaternion=geom.quaternion,
material=material_key)
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
continue
self.link_subgeometry_local_tfs_by_link_name[link.name] = tfs
for i, camera_tf in enumerate(self.camera_tfs):
camera_tf_post = self.global_transform.multiply(camera_tf)
self.bsi.send_remote_call(
"register_camera",
name="cam_%d" % i,
location=camera_tf_post.translation().tolist(),
quaternion=camera_tf_post.rotation().ToQuaternion().wxyz(
).tolist(),
angle=np.pi / 2.)
self.bsi.send_remote_call("configure_rendering",
camera_name='cam_%d' % i,
resolution=[640, 480],
file_format="BMP",
configure_for_masks=True,
taa_render_samples=10,
cycles=False)
self.bsi.send_remote_call("set_environment_map", path=None)
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()``).
"""
if self._delete_prefix_on_load:
self.vis[self.prefix].delete()
# Intercept load message via memq LCM.
memq_lcm = DrakeLcm("memq://")
memq_lcm_subscriber = Subscriber(lcm=memq_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, memq_lcm)
memq_lcm.HandleSubscriptions(0)
assert memq_lcm_subscriber.count > 0
load_robot_msg = memq_lcm_subscriber.message
# Translate elements to `meshcat`.
for i in range(load_robot_msg.num_links):
link = load_robot_msg.link[i]
[_, 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][str(
link.robot_num)][frame_name][str(j)])
# 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.
if isinstance(meshcat_geom, meshcat.geometry.MeshGeometry):
meshcat_geom.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
meshcat_geom.contents))
material.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
meshcat_geom.contents + "material"))
mesh = meshcat.geometry.Mesh(meshcat_geom, material)
mesh.uuid = str(
uuid.uuid5(uuid.NAMESPACE_X500,
meshcat_geom.contents + "mesh"))
cur_vis.set_object(mesh)
else:
cur_vis.set_object(meshcat_geom, material)
cur_vis.set_transform(element_local_tf)
# Draw the frames in self.frames_to_draw.
if "::" in frame_name:
robot_name, link_name = self._parse_name(frame_name)
else:
robot_name = "world"
link_name = frame_name
if (robot_name in self.frames_to_draw.keys()
and link_name in self.frames_to_draw[robot_name]):
prefix = (self.prefix + '/' + str(link.robot_num) + '/' +
frame_name)
AddTriad(self.vis,
name="frame",
prefix=prefix,
length=self.axis_length,
radius=self.axis_radius,
opacity=self.frames_opacity)
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()
mock_lcm_subscriber = Subscriber(lcm=mock_lcm,
channel="DRAKE_VIEWER_LOAD_ROBOT",
lcm_type=lcmt_viewer_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
assert mock_lcm_subscriber.count > 0
load_robot_msg = mock_lcm_subscriber.message
# 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
