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 test_mock_lcm(self):
dut = DrakeMockLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
# Create a simple ground-truth message.
msg = quaternion_t()
wxyz = (1, 2, 3, 4)
msg.w, msg.x, msg.y, msg.z = wxyz
dut.Publish(channel="TEST_CHANNEL", buffer=msg.encode())
raw = dut.get_last_published_message("TEST_CHANNEL")
self.assertEqual(raw, msg.encode())
# Use an array so that we can mutate the contained value.
called = [False]
def handler(raw):
msg = quaternion_t.decode(raw)
self.assertTupleEqual((msg.w, msg.x, msg.y, msg.z), wxyz)
called[0] = True
dut.Subscribe(channel="TEST_CHANNEL", handler=handler)
dut.InduceSubscriberCallback(
channel="TEST_CHANNEL", buffer=msg.encode())
self.assertTrue(called[0])
def test_publisher(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
publish_period=0.1)
model_message = self._model_message()
self._fix_and_publish(dut, AbstractValue.Make(model_message))
raw = lcm.get_last_published_message("TEST_CHANNEL")
actual_message = quaternion_t.decode(raw)
self.assert_lcm_equal(actual_message, model_message)
def test_subscriber_cpp(self):
lcm = DrakeMockLcm()
dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=quaternion_t,
lcm=lcm,
use_cpp_serializer=True)
model_message = self._model_message()
lcm.InduceSubscriberCallback(channel="TEST_CHANNEL",
buffer=model_message.encode())
actual_message = self._cpp_value_to_py_message(self._calc_output(dut))
self.assert_lcm_equal(actual_message, model_message)
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_publisher_cpp(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
use_cpp_serializer=True)
model_message = self._model_message()
model_value = self._model_value_cpp()
self._fix_and_publish(dut, model_value)
raw = lcm.get_last_published_message("TEST_CHANNEL")
actual_message = quaternion_t.decode(raw)
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)
model_message = self._model_message()
self._fix_and_publish(dut, AbstractValue.Make(model_message))
raw = lcm.get_last_published_message("TEST_CHANNEL")
actual_message = quaternion_t.decode(raw)
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)
model = self._model_message()
dut.set_publish_period(period=0.1)
context = dut.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(model))
dut.Publish(context)
raw = lcm.get_last_published_message("TEST_CHANNEL")
value = quaternion_t.decode(raw)
self.assert_lcm_equal(value, model)
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_publisher(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm)
model = self._model_message()
dut.set_publish_period(period=0.1)
context = dut.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(model))
dut.Publish(context)
raw = lcm.get_last_published_message("TEST_CHANNEL")
value = quaternion_t.decode(raw)
self.assert_lcm_equal(value, model)
def test_publisher_cpp(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=quaternion_t,
lcm=lcm,
use_cpp_serializer=True)
model_message = self._model_message()
model_value = self._model_value_cpp()
self._fix_and_publish(dut, model_value)
raw = lcm.get_last_published_message("TEST_CHANNEL")
actual_message = quaternion_t.decode(raw)
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 = self._model_message()
lcm.InduceSubscriberCallback(
channel="TEST_CHANNEL", buffer=model.encode())
context = dut.CreateDefaultContext()
output = dut.AllocateOutput()
dut.CalcOutput(context, output)
actual = output.get_data(0).get_value()
self.assert_lcm_equal(actual, model)
def test_subscriber_cpp(self):
lcm = DrakeMockLcm()
dut = mut.LcmSubscriberSystem.Make(
channel="TEST_CHANNEL", lcm_type=quaternion_t, lcm=lcm,
use_cpp_serializer=True)
model_message = self._model_message()
lcm.Publish(channel="TEST_CHANNEL", buffer=model_message.encode())
lcm.HandleSubscriptions(0)
context = self._process_event(dut)
abstract = dut.get_output_port(0).EvalAbstract(context)
actual_message = self._cpp_value_to_py_message(abstract)
self.assert_lcm_equal(actual_message, model_message)
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.
robot_name, link_name = self._parse_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_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]
lcm = DrakeMockLcm()
for role in [mut.Role.kProximity, mut.Role.kIllustration]:
for i in range(2):
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
if i == 1:
mut.ConnectDrakeVisualizer(builder=builder,
scene_graph=scene_graph,
lcm=lcm,
role=role)
else:
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)
def test_scene_graph_api(self):
scene_graph = mut.SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
self.assertIsInstance(
scene_graph.get_source_pose_port(global_source), InputPort)
self.assertIsInstance(
scene_graph.get_pose_bundle_output_port(), OutputPort)
self.assertIsInstance(
scene_graph.get_query_output_port(), OutputPort)
# Test visualization API.
# Use a mockable so that we can make a smoke test without side effects.
lcm = DrakeMockLcm()
for i in range(2):
builder = DiagramBuilder()
scene_graph = builder.AddSystem(mut.SceneGraph())
if i == 1:
mut.ConnectDrakeVisualizer(
builder=builder, scene_graph=scene_graph)
else:
mut.ConnectDrakeVisualizer(
builder=builder, scene_graph=scene_graph,
pose_bundle_output_port=(
scene_graph.get_pose_bundle_output_port()))
mut.DispatchLoadMessage(
scene_graph=scene_graph, lcm=lcm)
def test_connect_lcm_scope(self):
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))
mut.ConnectLcmScope(src=source.get_output_port(0),
channel="TEST_CHANNEL",
builder=builder,
lcm=DrakeMockLcm())
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 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 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 test_drake_visualizer_api(self):
tree = self._make_tree()
lcm = DrakeMockLcm()
# Test for existence.
viz = mut.DrakeVisualizer(tree=tree, lcm=lcm, enable_playback=True)
viz.set_publish_period(period=0.01)
# - Force publish to ensure we have enough knot points.
context = viz.CreateDefaultContext()
x0 = np.zeros(tree.get_num_positions() + tree.get_num_velocities())
context.FixInputPort(0, BasicVector(x0))
context.set_time(0.)
viz.Publish(context)
# Use a small time period, since it uses realtime playback.
context.set_time(0.01)
viz.Publish(context)
viz.ReplayCachedSimulation()
def test_drake_visualizer_api(self):
tree = self._make_tree()
lcm = DrakeMockLcm()
# Test for existence.
viz = mut.DrakeVisualizer(tree=tree, lcm=lcm, enable_playback=True)
viz.set_publish_period(period=0.01)
# - Force publish to ensure we have enough knot points.
context = viz.CreateDefaultContext()
x0 = np.zeros(tree.get_num_positions() + tree.get_num_velocities())
viz.get_input_port(0).FixValue(context, x0)
context.SetTime(0.)
viz.Publish(context)
# Use a small time period, since it uses realtime playback.
context.SetTime(0.01)
viz.Publish(context)
viz.ReplayCachedSimulation()
# - Check that PublishLoadRobot can be called.
viz.PublishLoadRobot()
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 test_mock_lcm_induce_subscriber_callback(self):
dut = DrakeMockLcm()
dut.Subscribe(channel="TEST_CHANNEL", handler=self._handler)
dut.InduceSubscriberCallback(channel="TEST_CHANNEL",
buffer=self.quat.encode())
self.assertEqual(self.count, 1)
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 test_mock_lcm_get_last_published_message_deprecated(self):
dut = DrakeMockLcm()
dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode())
with catch_drake_warnings(expected_count=1):
raw = dut.get_last_published_message("TEST_CHANNEL")
self.assertEqual(raw, self.quat.encode())
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 test_mock_lcm_get_last_published_message_deprecated(self):
dut = DrakeMockLcm()
dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode())
with catch_drake_warnings(expected_count=1):
raw = dut.get_last_published_message("TEST_CHANNEL")
self.assertEqual(raw, self.quat.encode())
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 test_mock_lcm(self):
self._test_lcm_interface(DrakeMockLcm())
def test_mock_lcm_get_last_published_message(self):
dut = DrakeMockLcm()
dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode())
raw = dut.get_last_published_message("TEST_CHANNEL")
self.assertEqual(raw, self.quat.encode())
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 test_mock_lcm(self):
dut = DrakeMockLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
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 test_mock_lcm_get_last_published_message(self):
dut = DrakeMockLcm()
dut.Publish(channel="TEST_CHANNEL", buffer=self.quat.encode())
raw = dut.get_last_published_message("TEST_CHANNEL")
self.assertEqual(raw, self.quat.encode())
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
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 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()`).
"""
# TODO(russt): Declare an initialization event to publish this
# pending resolution of #9842.
# 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=Rgb2Hex(geom.color)))
self.vis[self.prefix][source_name][str(
link.robot_num)][frame_name][str(j)].set_transform(
element_local_tf)
