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)