def test_connect_drake_visualizer(self):
# Test visualization API.
# Use a mockable so that we can make a smoke test without side
# effects.
T = float
SceneGraph = mut.SceneGraph_[T]
DiagramBuilder = DiagramBuilder_[T]
Simulator = Simulator_[T]
lcm = DrakeMockLcm()
role = mut.Role.kIllustration
test_prefix = "TEST_PREFIX_"
def normal(builder, scene_graph):
mut.ConnectDrakeVisualizer(builder=builder,
scene_graph=scene_graph,
lcm=lcm,
role=role)
mut.DispatchLoadMessage(scene_graph=scene_graph,
lcm=lcm,
role=role)
def port(builder, scene_graph):
mut.ConnectDrakeVisualizer(
builder=builder,
scene_graph=scene_graph,
pose_bundle_output_port=(
scene_graph.get_pose_bundle_output_port()),
lcm=lcm,
role=role)
mut.DispatchLoadMessage(scene_graph=scene_graph,
lcm=lcm,
role=role)
for func in [normal, port]:
# Create subscribers.
load_channel = "DRAKE_VIEWER_LOAD_ROBOT"
draw_channel = "DRAKE_VIEWER_DRAW"
load_subscriber = Subscriber(lcm, load_channel,
lcmt_viewer_load_robot)
draw_subscriber = Subscriber(lcm, draw_channel, lcmt_viewer_draw)
# Test sequence.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
# Only load will be published by `DispatchLoadMessage`.
func(builder, scene_graph)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 0)
diagram = builder.Build()
# Load and draw will be published.
Simulator(diagram).Initialize()
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 2)
self.assertEqual(draw_subscriber.count, 1)
def test_mock_lcm_roundtrip(self):
dut = DrakeMockLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
dut.Subscribe(channel="TEST_CHANNEL", handler=self._handler)
dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode())
self.assertEqual(self.count, 0)
dut.HandleSubscriptions(0)
self.assertEqual(self.count, 1)
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 test_subscriber(self):
lcm = DrakeMockLcm()
dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=quaternion_t,
lcm=lcm)
model_message = self._model_message()
lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode())
lcm.HandleSubscriptions(0)
actual_message = self._calc_output(dut).get_value()
self.assert_lcm_equal(actual_message, model_message)
def test_subscriber(self):
lcm = DrakeMockLcm()
dut = mut.LcmSubscriberSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm)
model_message = self._model_message()
lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode())
lcm.HandleSubscriptions(0)
context = self._process_event(dut)
actual_message = dut.get_output_port(0).Eval(context)
self.assert_lcm_equal(actual_message, model_message)
def test_publisher(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
publish_period=0.1)
subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t)
model_message = self._model_message()
self._fix_and_publish(dut, AbstractValue.Make(model_message))
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.message, model_message)
def test_publisher_cpp(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
use_cpp_serializer=True)
subscriber = Subscriber(lcm, "TEST_CHANNEL", quaternion_t)
model_message = self._model_message()
model_value = self._model_value_cpp()
self._fix_and_publish(dut, model_value)
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.message, model_message)
def test_subscriber(self):
mock = DrakeMockLcm()
dut = Subscriber(lcm=mock, channel="CHANNEL", lcm_type=quaternion_t)
mock.Publish(channel="CHANNEL", buffer=self.quat.encode())
self.assertEqual(dut.count, 0)
self.assertEqual(len(dut.raw), 0)
self.assertEqual(dut.message.w, 0)
self.assertEqual(dut.message.x, 0)
self.assertEqual(dut.message.y, 0)
self.assertEqual(dut.message.z, 0)
mock.HandleSubscriptions(0)
self.assertEqual(dut.count, 1)
self.assertEqual(dut.raw, self.quat.encode())
self.assertEqual(dut.message.w, self.quat.w)
self.assertEqual(dut.message.x, self.quat.x)
self.assertEqual(dut.message.y, self.quat.y)
self.assertEqual(dut.message.z, self.quat.z)
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.
def handle_load_robot(raw):
load_robot_msgs.append(lcmt_viewer_load_robot.decode(raw))
load_robot_msgs = []
mock_lcm = DrakeMockLcm()
mock_lcm.Subscribe(
channel="DRAKE_VIEWER_LOAD_ROBOT",
handler=handle_load_robot)
DispatchLoadMessage(self._scene_graph, mock_lcm)
mock_lcm.HandleSubscriptions(0)
load_robot_msg = load_robot_msgs[0]
# 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 _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 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 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