def test_publisher(self):
lcm = DrakeLcm(lcm_url="memq://")
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_subscriber(self):
lcm = DrakeLcm(lcm_url="memq://")
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_connect_drake_visualizer(self):
# Test visualization API.
T = float
SceneGraph = mut.SceneGraph_[T]
DiagramBuilder = DiagramBuilder_[T]
Simulator = Simulator_[T]
lcm = DrakeLcm()
role = mut.Role.kIllustration
def normal(builder, scene_graph):
with catch_drake_warnings(expected_count=2):
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):
with catch_drake_warnings(expected_count=2):
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 = DrakeLcm()
dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm,
use_cpp_serializer=True)
subscriber = Subscriber(lcm, "TEST_CHANNEL", lcmt_quaternion)
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_cpp(self):
lcm = DrakeLcm()
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 test_subscriber_wait_for_message(self):
"""Ensures that `WaitForMessage` threading works in a Python workflow
that is not threaded."""
# N.B. This will fail with `threading`. See below for using
# `multithreading`.
lcm = DrakeLcm("memq://")
lcm.StartReceiveThread()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
value = AbstractValue.Make(header_t())
for i in range(3):
message = header_t()
message.utime = i
lcm.Publish("TEST_LOOP", message.encode())
sub.WaitForMessage(i, value)
self.assertEqual(value.get_value().utime, i)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("filename")
args = parser.parse_args()
# Load the model file.
tree = RigidBodyTree()
ext = os.path.splitext(args.filename)[1]
if ext == ".sdf":
AddModelInstancesFromSdfFile(args.filename, FloatingBaseType.kFixed,
None, tree)
elif ext == ".urdf":
AddModelInstanceFromUrdfFile(args.filename, FloatingBaseType.kFixed,
None, tree)
else:
parser.error("Unknown extension " + ext)
# Send drake-visualizer messages to load the model and then position it in
# its default configuration.
q = tree.getZeroConfiguration()
v = np.zeros(tree.get_num_velocities())
lcm = DrakeLcm()
visualizer = DrakeVisualizer(tree=tree, lcm=lcm)
visualizer.PublishLoadRobot()
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, BasicVector(np.concatenate([q, v])))
visualizer.Publish(context)
def test_subscriber_wait_for_message(self):
"""Checks how `WaitForMessage` works without Python threads."""
lcm = DrakeLcm()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
value = AbstractValue.Make(header_t())
for old_message_count in range(3):
message = header_t()
message.utime = old_message_count + 1
lcm.Publish("TEST_LOOP", message.encode())
for attempt in range(10):
new_count = sub.WaitForMessage(
old_message_count, value, timeout=0.02)
if new_count > old_message_count:
break
lcm.HandleSubscriptions(0)
self.assertEqual(value.get_value().utime, old_message_count + 1)
def test_all_serializers_exist(self):
"""Checks that all of Drake's Python LCM messages have a matching C++
serializer bound for use by LcmPublisherSystem.
"""
# The drake_lcm_py_library() in drake/lcmtypes/BUILD.bazel generates
# a module __init__.py that enumerates all Drake Python LCM messages.
# Fetch that module's list of message classes.
all_message_classes = [
getattr(drake_lcmtypes, name) for name in dir(drake_lcmtypes)
if any([
name.startswith("lcmt_"),
name.startswith("experimental_lcmt_")
])
]
self.assertGreater(len(all_message_classes), 1)
# Confirm that each message class is partnered with the definition of a
# C++ serializer in lcm_py_bind_cpp_serializers.cc.
lcm = DrakeLcm()
for message_class in all_message_classes:
# Check that the Python message class is a valid template value.
serializer = mut._Serializer_[message_class]
self.assertIsNotNone(serializer)
# Confirm that we can actually instantiate a publisher that takes
# the matching C++ message on its input port.
mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=message_class,
lcm=lcm,
use_cpp_serializer=True)
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=DrakeLcm())
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 test_subscriber(self):
lcm = DrakeLcm()
dut = Subscriber(lcm=lcm, channel="CHANNEL", lcm_type=lcmt_quaternion)
lcm.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)
lcm.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 test_deprecated_connect_lcm_scope(self):
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))
with catch_drake_warnings(expected_count=1):
mut.ConnectLcmScope(src=source.get_output_port(0),
channel="TEST_CHANNEL",
builder=builder,
lcm=DrakeLcm(),
publish_period=0.001)
def __init__(self, *, meshcat_port=None):
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
logging.info(f"Meldis is listening for LCM messages at {lcm_url}")
self.meshcat = Meshcat(port=meshcat_port)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
def test_lcm_scope(self):
builder = DiagramBuilder()
source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))
scope, publisher = mut.LcmScopeSystem.AddToBuilder(
builder=builder,
lcm=DrakeLcm(),
signal=source.get_output_port(0),
channel="TEST_CHANNEL",
publish_period=0.001)
self.assertIsInstance(scope, mut.LcmScopeSystem)
self.assertIsInstance(publisher, mut.LcmPublisherSystem)
class Meldis:
"""
MeshCat LCM Display Server (MeLDiS)
Offers a MeshCat vizualization server that listens for and draws Drake's
legacy LCM vizualization messages.
Refer to the pydrake.visualization.meldis module docs for details.
"""
def __init__(self, *, meshcat_port=None):
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
logging.info(f"Meldis is listening for LCM messages at {lcm_url}")
self.meshcat = Meshcat(port=meshcat_port)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
def _subscribe(self, channel, message_type, handler):
def _parse_and_handle(data):
handler(message=message_type.decode(data))
self._lcm.Subscribe(channel=channel, handler=_parse_and_handle)
def serve_forever(self):
# TODO(jwnimmer-tri) If there are no browser connections open for some
# period of time, we should probably give up and quit, rather than
# leave a zombie meldis running forever.
while True:
self._lcm.HandleSubscriptions(timeout_millis=1000)
def test_lcm(self):
dut = DrakeLcm()
self.assertIsInstance(dut, DrakeLcmInterface)
dut.get_lcm_url()
DrakeLcm(lcm_url="")
DrakeLcm(lcm_url="", defer_initialization=True)
# Test virtual function names.
dut.Publish
dut.HandleSubscriptions
def test_diagram_publisher(self):
"""Acceptance tests that a Python LeafSystem is able to output LCM
messages for LcmPublisherSystem to transmit.
"""
lcm = DrakeLcm()
builder = DiagramBuilder()
source = builder.AddSystem(TestSystemsLcm.PythonMessageSource())
publisher = builder.AddSystem(
mut.LcmPublisherSystem.Make(channel="LCMT_HEADER",
lcm_type=lcmt_header,
lcm=lcm,
publish_period=0.05))
builder.Connect(source.get_output_port(), publisher.get_input_port())
diagram = builder.Build()
diagram.Publish(diagram.CreateDefaultContext())
def __init__(self, tree):
lcm = DrakeLcm()
self.tree = tree
self.visualizer = DrakeVisualizer(tree=self.tree,
lcm=lcm,
enable_playback=True)
self.x = np.concatenate([
robot.getZeroConfiguration(),
np.zeros(tree.get_num_velocities())
])
# Workaround for the fact that PublishLoadRobot is not public.
# Ultimately that should be fixed.
sim = Simulator(self.visualizer)
context = sim.get_mutable_context()
context.FixInputPort(0, BasicVector(self.x))
sim.Initialize()
def test_apply_visualization_config(self):
"""Exercises VisualizationConfig and ApplyVisualizationConfig.
"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
lcm_buses = LcmBuses()
lcm_buses.Add("default", DrakeLcm())
config = mut.VisualizationConfig(publish_period=0.25)
self.assertIn("publish_period", repr(config))
copy.copy(config)
mut.ApplyVisualizationConfig(config=config,
plant=plant,
scene_graph=scene_graph,
lcm_buses=lcm_buses,
builder=builder)
def test_drake_visualizer_api(self):
tree = self._make_tree()
lcm = DrakeLcm()
# 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 test_subscriber(self):
lcm = DrakeLcm()
dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
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)
# Test LcmInterfaceSystem overloads
lcm_system = mut.LcmInterfaceSystem(lcm=lcm)
dut = mut.LcmSubscriberSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm_system)
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_drake_visualizer(self, T):
# Test visualization API.
SceneGraph = mut.SceneGraph_[T]
DiagramBuilder = DiagramBuilder_[T]
Simulator = Simulator_[T]
lcm = DrakeLcm()
role = mut.Role.kIllustration
params = mut.DrakeVisualizerParams(
publish_period=0.1, role=mut.Role.kIllustration,
default_color=mut.Rgba(0.1, 0.2, 0.3, 0.4))
self.assertEqual(repr(params), "".join([
"DrakeVisualizerParams("
"publish_period=0.1, "
"role=Role.kIllustration, "
"default_color=Rgba(r=0.1, g=0.2, b=0.3, a=0.4))"]))
# Add some subscribers to detect message broadcast.
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)
# There are three ways to configure DrakeVisualizer.
def by_hand(builder, scene_graph, params):
visualizer = builder.AddSystem(
mut.DrakeVisualizer_[T](lcm=lcm, params=params))
builder.Connect(scene_graph.get_query_output_port(),
visualizer.query_object_input_port())
def auto_connect_to_system(builder, scene_graph, params):
mut.DrakeVisualizer_[T].AddToBuilder(builder=builder,
scene_graph=scene_graph,
lcm=lcm, params=params)
def auto_connect_to_port(builder, scene_graph, params):
mut.DrakeVisualizer_[T].AddToBuilder(
builder=builder,
query_object_port=scene_graph.get_query_output_port(),
lcm=lcm, params=params)
for func in [by_hand, auto_connect_to_system, auto_connect_to_port]:
# Build the diagram.
builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
func(builder, scene_graph, params)
# Simulate to t = 0 to send initial load and draw messages.
diagram = builder.Build()
Simulator(diagram).AdvanceTo(0)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 1)
load_subscriber.clear()
draw_subscriber.clear()
# Ad hoc broadcasting.
scene_graph = SceneGraph()
mut.DrakeVisualizer_[T].DispatchLoadMessage(
scene_graph, lcm, params)
lcm.HandleSubscriptions(0)
self.assertEqual(load_subscriber.count, 1)
self.assertEqual(draw_subscriber.count, 0)
load_subscriber.clear()
draw_subscriber.clear()
def test_lcm_driven_loop(self):
"""Duplicates the test logic in `lcm_driven_loop_test.cc`."""
lcm_url = "udpm://239.255.76.67:7669"
t_start = 3.
t_end = 10.
def publish_loop():
# Publishes a set of messages for the driven loop. This should be
# run from a separate process.
# N.B. Because of this, care should be taken not to share C++
# objects between process boundaries.
t = t_start
while t <= t_end:
message = header_t()
message.utime = int(1e6 * t)
lcm.Publish("TEST_LOOP", message.encode())
time.sleep(0.1)
t += 1
class DummySys(LeafSystem):
# Converts message to time in seconds.
def __init__(self):
LeafSystem.__init__(self)
self.DeclareAbstractInputPort("header_t",
AbstractValue.Make(header_t))
self.DeclareVectorOutputPort(BasicVector(1), self._calc_output)
def _calc_output(self, context, output):
message = self.EvalAbstractInput(context, 0).get_value()
y = output.get_mutable_value()
y[:] = message.utime / 1e6
# Construct diagram for LcmDrivenLoop.
lcm = DrakeLcm(lcm_url)
utime = mut.PyUtimeMessageToSeconds(header_t)
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
builder = DiagramBuilder()
builder.AddSystem(sub)
dummy = builder.AddSystem(DummySys())
builder.Connect(sub.get_output_port(0), dummy.get_input_port(0))
logger = LogOutput(dummy.get_output_port(0), builder)
logger.set_forced_publish_only()
diagram = builder.Build()
dut = mut.LcmDrivenLoop(diagram, sub, None, lcm, utime)
dut.set_publish_on_every_received_message(True)
# N.B. Use `multiprocessing` instead of `threading` so that we may
# avoid issues with GIL deadlocks.
publish_proc = Process(target=publish_loop)
publish_proc.start()
# Initialize to first message.
first_msg = dut.WaitForMessage()
dut.get_mutable_context().SetTime(utime.GetTimeInSeconds(first_msg))
# Run to desired amount. (Anything more will cause interpreter to
# "freeze".)
dut.RunToSecondsAssumingInitialized(t_end)
publish_proc.join()
# Check expected values.
log_t_expected = np.array([4, 5, 6, 7, 8, 9])
log_t = logger.sample_times()
self.assertTrue(np.allclose(log_t_expected, log_t))
log_y = logger.data()
self.assertTrue(np.allclose(log_t_expected, log_y))
def test_subscriber_wait_for_message_with_timeout(self):
"""Confirms that the subscriber times out."""
lcm = DrakeLcm("memq://")
lcm.StartReceiveThread()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
sub.WaitForMessage(0, timeout=0.02)
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 _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 = {}
memq_lcm = DrakeLcm("memq://")
memq_lcm_subscriber = Subscriber(lcm=memq_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, memq_lcm)
memq_lcm.HandleSubscriptions(0)
assert memq_lcm_subscriber.count > 0
load_robot_msg = memq_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() != ".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 test_lcm(self):
self._test_lcm_interface(DrakeLcm())
class Meldis:
"""
MeshCat LCM Display Server (MeLDiS)
Offers a MeshCat vizualization server that listens for and draws Drake's
legacy LCM vizualization messages.
Refer to the pydrake.visualization.meldis module docs for details.
"""
def __init__(self, *, meshcat_port=None):
# Bookkeeping for update throtting.
self._last_update_time = time.time()
# Bookkeeping for subscriptions, keyed by LCM channel name.
self._message_types = {}
self._message_handlers = {}
self._message_pending_data = {}
self._lcm = DrakeLcm()
lcm_url = self._lcm.get_lcm_url()
_logger.info(f"Meldis is listening for LCM messages at {lcm_url}")
params = MeshcatParams(host="localhost", port=meshcat_port)
self.meshcat = Meshcat(params=params)
viewer = _ViewerApplet(meshcat=self.meshcat)
self._subscribe(channel="DRAKE_VIEWER_LOAD_ROBOT",
message_type=lcmt_viewer_load_robot,
handler=viewer.on_viewer_load)
self._subscribe(channel="DRAKE_VIEWER_DRAW",
message_type=lcmt_viewer_draw,
handler=viewer.on_viewer_draw)
contact = _ContactApplet(meshcat=self.meshcat)
self._subscribe(channel="CONTACT_RESULTS",
message_type=lcmt_contact_results_for_viz,
handler=contact.on_contact_results)
# Bookkeeping for automatic shutdown.
self._last_poll = None
self._last_active = None
def _subscribe(self, channel, message_type, handler):
"""Subscribes the handler to the given channel, using message_type to
pass in a decoded message object (not the raw bytes). The handler will
only be called at some maximum frequency. Messages on the same channel
that arrive too quickly will be discarded.
"""
# Record this channel's type and handler.
assert self._message_types.get(channel, message_type) == message_type
self._message_types[channel] = message_type
self._message_handlers.setdefault(channel, []).append(handler)
# Subscribe using an internal function that implements "last one wins".
# It's important to service the LCM queue as frequently as possible:
# https://github.com/RobotLocomotion/drake/issues/15234
# https://github.com/lcm-proj/lcm/issues/345
# However, if the sender is transmitting visualization messages at
# a high rate (e.g., if a sim is running much faster than realtime),
# then we should only pass some of them along to MeshCat to avoid
# flooding it. The hander merely records the message data; we'll
# pass it along to MeshCat using our `self._should_update()` timer.
def _on_message(data):
self._message_pending_data[channel] = data
self._lcm.Subscribe(channel=channel, handler=_on_message)
def _invoke_subscriptions(self):
"""Posts any unhandled messages to their handlers and clears the
collection of unhandled messages.
"""
for channel, data in self._message_pending_data.items():
message = self._message_types[channel].decode(data)
for function in self._message_handlers[channel]:
function(message=message)
self._message_pending_data.clear()
def serve_forever(self, *, idle_timeout=None):
"""Run indefinitely, forwarding LCM => MeshCat messages.
If provided, the optional idle_timeout must be strictly positive and
this loop will sys.exit after that many seconds without any websocket
connections.
"""
while True:
self._lcm.HandleSubscriptions(timeout_millis=1000)
if not self._should_update():
continue
self._invoke_subscriptions()
self.meshcat.Flush()
self._check_for_shutdown(idle_timeout=idle_timeout)
def _should_update(self):
"""Post LCM-driven updates to MeshCat no faster than 40 Hz."""
now = time.time()
update_period = 0.025 # 40 Hz
remaining = update_period - (now - self._last_update_time)
if remaining > 0.0:
return False
else:
self._last_update_time = now
return True
def _check_for_shutdown(self, *, idle_timeout):
# Allow the user to opt-out of the timeout feature.
if idle_timeout is None:
return
assert idle_timeout > 0.0
# One-time initialization.
now = time.time()
if self._last_active is None:
self._last_active = now
return
# Only check once every 5 seconds.
if (self._last_poll is not None) and (now < self._last_poll + 5.0):
return
self._last_poll = now
# Check to see if any browser client(s) are connected.
if self.meshcat.GetNumActiveConnections() > 0:
self._last_active = now
return
# In case we are idle for too long, exit automatically.
if now > self._last_active + idle_timeout:
_logger.info("Meldis is exiting now; no browser was connected for"
f" >{idle_timeout} seconds")
sys.exit(1)
import numpy as np
rtree = getCassieTree()
plant = RigidBodyPlant(rtree, 0.0005)
builder = DiagramBuilder()
cassie = builder.AddSystem(plant)
setDefaultContactParams(cassie)
# precomputed standing fixed point state
# q = getNominalStandingConfiguration()
# qd = [0. for i in xrange(rtree.get_num_velocities())]
x = CassieFixedPointState()
# Setup visualizer
lcm = DrakeLcm()
visualizer = builder.AddSystem(
DrakeVisualizer(tree=rtree, lcm=lcm, enable_playback=True))
builder.Connect(cassie.get_output_port(0), visualizer.get_input_port(0))
# Zero inputs -- passive forward simulation
u0 = ConstantVectorSource(np.zeros(rtree.get_num_actuators()))
null_controller = builder.AddSystem(u0)
builder.Connect(null_controller.get_output_port(0), cassie.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
simulator.set_publish_every_time_step(True)
