From 2b44dd1325e42bf9b51b139dd2594bf93672d629 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sun, 9 Aug 2020 20:36:49 -0400 Subject: [PATCH 1/4] Pass kwargs through MeshcatVisualizer to meshcat.Visualizer (#13848) --- bindings/pydrake/systems/meshcat_visualizer.py | 7 +++++-- bindings/pydrake/systems/test/meshcat_visualizer_test.py | 5 ++++- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/bindings/pydrake/systems/meshcat_visualizer.py b/bindings/pydrake/systems/meshcat_visualizer.py index 81b8b35d8073..9f0ce4daa204 100644 --- a/bindings/pydrake/systems/meshcat_visualizer.py +++ b/bindings/pydrake/systems/meshcat_visualizer.py @@ -213,7 +213,8 @@ def __init__(self, frames_to_draw={}, frames_opacity=1., axis_length=0.15, - axis_radius=0.006): + axis_radius=0.006, + **kwargs): """ Args: scene_graph: A SceneGraph object. @@ -241,6 +242,8 @@ def __init__(self, {"1": {"A", "B"}}. frames_opacity, axis_length and axis_radius are the opacity, length and radius of the coordinate axes to be drawn. + + Additional kwargs will be passed to the MeshcatVisualizer constructor. Note: This call will not return until it connects to the ``meshcat-server``. @@ -273,7 +276,7 @@ def __init__(self, self.prefix = prefix if zmq_url is not None: print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...") - self.vis = meshcat.Visualizer(zmq_url=zmq_url) + self.vis = meshcat.Visualizer(zmq_url=zmq_url, **kwargs) print("Connected to meshcat-server.") self._scene_graph = scene_graph diff --git a/bindings/pydrake/systems/test/meshcat_visualizer_test.py b/bindings/pydrake/systems/test/meshcat_visualizer_test.py index 0da14178de80..d2d76f7f237b 100644 --- a/bindings/pydrake/systems/test/meshcat_visualizer_test.py +++ b/bindings/pydrake/systems/test/meshcat_visualizer_test.py @@ -63,9 +63,12 @@ def test_cart_pole(self): cart_pole.Finalize() assert cart_pole.geometry_source_is_registered() + # Note: pass window=None argument to confirm kwargs are passed + # through to meshcat.Visualizer. visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph, zmq_url=ZMQ_URL, - open_browser=False)) + open_browser=False, + window=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0)) From 491e2f30a628066e1f48059311a4d64d56cc1462 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Thu, 30 Jul 2020 21:43:33 -0400 Subject: [PATCH 2/4] Provides support for using ipywidgets with the systems framework, and PoseSliders as the first useful widget system. --- bindings/pydrake/systems/BUILD.bazel | 29 +++ bindings/pydrake/systems/jupyter_widgets.py | 228 ++++++++++++++++++ .../systems/jupyter_widgets_examples.ipynb | 130 ++++++++++ 3 files changed, 387 insertions(+) create mode 100644 bindings/pydrake/systems/jupyter_widgets.py create mode 100644 bindings/pydrake/systems/jupyter_widgets_examples.ipynb diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 770c7077622b..17f0d2f4ab83 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -18,6 +18,7 @@ load( "drake_py_library", "drake_py_unittest", ) +load("//tools/jupyter:jupyter_py.bzl", "drake_jupyter_py_binary") load("//bindings/pydrake:pydrake.bzl", "add_lint_tests_pydrake") load("//tools/skylark:test_tags.bzl", "vtk_test_tags") @@ -257,6 +258,17 @@ drake_py_library( ], ) +drake_py_library( + name = "jupyter_widgets_py", + srcs = ["jupyter_widgets.py"], + imports = PACKAGE_INFO.py_imports, + deps = [ + ":framework_py", + ":module_py", + "//bindings/pydrake:math_py", + ], +) + drake_py_library( name = "perception_py", srcs = ["perception.py"], @@ -315,6 +327,7 @@ PY_LIBRARIES_WITH_INSTALL = [ PY_LIBRARIES = [ ":drawing_py", ":meshcat_visualizer_py", + ":jupyter_widgets_py", ":scalar_conversion_py", ":module_py", ":perception_py", @@ -373,6 +386,22 @@ drake_py_binary( ], ) +drake_jupyter_py_binary( + name = "jupyter_widgets_examples", + add_test_rule = 1, + data = [ + "//manipulation/models/ycb:models", + ], + deps = [ + ":analysis_py", + ":framework_py", + ":jupyter_widgets_py", + "//bindings/pydrake:geometry_py", + "//bindings/pydrake/multibody:parsing_py", + "//bindings/pydrake/multibody:plant_py", + ], +) + drake_pybind_library( name = "test_util_py", testonly = 1, diff --git a/bindings/pydrake/systems/jupyter_widgets.py b/bindings/pydrake/systems/jupyter_widgets.py new file mode 100644 index 000000000000..dd199a77b65d --- /dev/null +++ b/bindings/pydrake/systems/jupyter_widgets.py @@ -0,0 +1,228 @@ +""" +Provides support for using ipywidgets with the systems framework, and a number +of useful widget systems. +""" + +import asyncio +import numpy as np +import sys +from warnings import warn +from ipywidgets import FloatSlider, Layout +from IPython import get_ipython +from warnings import warn + +from pydrake.common.value import AbstractValue +from pydrake.systems.framework import LeafSystem, BasicVector +from pydrake.math import RigidTransform, RollPitchYaw + + +class PoseSliders(LeafSystem): + """ + Provides a set of ipywidget sliders (to be used in a Jupyter notebook) with + on one slider for each of roll, pitch, yaw, x, y, and z. This can be used, + for instance, as an interface to teleoperate the end-effector of a robot. + + System YAML + name: PoseSliders + output_ports: + - pose + """ + def __init__(self, visible=[True] * 6): + """ + Args: + visible: A six element boolean list that specifies whether the + slider corresponding slider (ordered by roll, pitch, yaw, + x, y, z) is displayed or not. + + See SetRpyRange, and SetXyzRange to set the slider limits, and SetPose + or SetRpy and SetXyz to set their value. + """ + LeafSystem.__init__(self) + self.DeclareAbstractOutputPort( + "pose", lambda: AbstractValue.Make(RigidTransform()), + self.DoCalcOutput) + + # Note: This timing affects the keyboard teleop performance. A larger + # time step causes more lag in the response. + self.DeclarePeriodicPublish(0.01, 0.0) + + self.roll = FloatSlider(min=-np.pi, + max=np.pi, + value=0.0, + step=0.01, + continuous_update=True, + description="roll", + layout=Layout(width='90%')) + self.pitch = FloatSlider(min=-np.pi, + max=np.pi, + value=0.0, + step=0.01, + continuous_update=True, + description="pitch", + layout=Layout(width='90%')) + self.yaw = FloatSlider(min=-np.pi, + max=np.pi, + value=0.0, + step=0.01, + continuous_update=True, + description="yaw", + layout=Layout(width='90%')) + self.x = FloatSlider(min=-1.0, + max=1.0, + value=0., + step=0.01, + continuous_update=True, + description="x", + orient='horizontal', + layout=Layout(width='90%')) + self.y = FloatSlider(min=-1.0, + max=1.0, + value=0., + step=0.01, + continuous_update=True, + description="y", + layout=Layout(width='90%')) + self.z = FloatSlider(min=-1.0, + max=1.0, + value=0., + step=0.01, + continuous_update=True, + description="z", + layout=Layout(width='90%')) + if visible[0]: + display(self.roll) + if visible[1]: + display(self.pitch) + if visible[2]: + display(self.yaw) + if visible[3]: + display(self.x) + if visible[4]: + display(self.y) + if visible[5]: + display(self.z) + + def SetPose(self, pose): + """ + Sets the current value of the sliders. + Args: + pose: Any viable argument for the RigidTransform + constructor. + """ + tf = RigidTransform(pose) + self.SetRpy(RollPitchYaw(tf.rotation())) + self.SetXyz(tf.translation()) + + def SetRpy(self, rpy): + """ + Sets the current value of the sliders for roll, pitch, and yaw. + Args: + rpy: An instance of drake.math.RollPitchYaw + """ + self.roll.value = rpy.roll_angle() + self.pitch.value = rpy.pitch_angle() + self.yaw.value = rpy.yaw_angle() + + def SetRpyRange(self, min, max): + """ + Sets the minimum and maximum values of the sliders for roll, pitch, and + yaw. + Args: + min: An instance of drake.math.RollPitchYaw for the lower limit. + max: An instance of drake.math.RollPitchYaw for the upper limit. + """ + self.roll.min = min.roll_angle() + self.pitch.min = min.pitch_angle() + self.yaw.min = min.yaw_angle() + self.roll.max = max.roll_angle() + self.pitch.max = max.pitch_angle() + self.yaw.max = max.yaw_angle() + + def SetXyz(self, xyz): + """ + Sets the current value of the sliders for x, y, and z. + Args: + xyz: A 3 element iterable object with x, y, z. + """ + self.x.value = xyz[0] + self.y.value = xyz[1] + self.z.value = xyz[2] + + def SetXyzRange(self, min, max): + """ + Sets the minimum and maximum values of the sliders for x, y, and z. + Args: + min: 3 element iterable object representing the (elementwise) + x,y,z, minimum values. + max: 3 element iterable object representing the (elementwise) + x,y,z, maximum values. + """ + self.x.min = min[0] + self.y.min = min[1] + self.z.min = min[2] + self.x.max = max[0] + self.y.max = max[1] + self.z.max = max[2] + + def DoPublish(self, context, event): + """ + Allow the ipython kernel to process the event queue. + """ + update_widgets() + + def DoCalcOutput(self, context, output): + """ + Constructs the output values from the widget elements. + """ + output.set_value(RigidTransform( + RollPitchYaw(self.roll.value, self.pitch.value, self.yaw.value), + [self.x.value, self.y.value, self.z.value])) + + +# Note: The implementation below was inspired by +# https://github.com/Kirill888/jupyter-ui-poll , though I suspect it can be +# optimized. +# +# For reference, +# https://ipywidgets.readthedocs.io/en/latest/examples/Widget%20Asynchronous.html # noqa +# describes the problem but does not offer a solution. +def update_widgets(num_ui_events_to_process=1): + """ + Allows the kernel to process GUI events. This is required in order to + process ipywidget updates inside a simulation loop. + """ + + shell = get_ipython() + # Ok to do nothing if running from console + if shell is None: + return + kernel = shell.kernel + events = [] + kernel.shell_handlers['execute_request'] = lambda *e: events.append(e) + current_parent = (kernel._parent_ident, kernel._parent_header) + + for _ in range(num_ui_events_to_process): + # ensure stdout still happens in the same cell + kernel.set_parent(*current_parent) + kernel.do_one_iteration() + kernel.set_parent(*current_parent) + + kernel.shell_handlers['execute_request'] = kernel.execute_request + + def _replay_events(shell, events): + kernel = shell.kernel + sys.stdout.flush() + sys.stderr.flush() + for stream, ident, parent in events: + kernel.set_parent(ident, parent) + if kernel._aborting: + kernel._send_abort_reply(stream, parent, ident) + else: + kernel.execute_request(stream, ident, parent) + + loop = asyncio.get_event_loop() + if loop.is_running(): + loop.call_soon(lambda: _replay_events(shell, events)) + else: + warn('Automatic execution of scheduled cells only works with ' + 'asyncio-based ipython') diff --git a/bindings/pydrake/systems/jupyter_widgets_examples.ipynb b/bindings/pydrake/systems/jupyter_widgets_examples.ipynb new file mode 100644 index 000000000000..373635b72940 --- /dev/null +++ b/bindings/pydrake/systems/jupyter_widgets_examples.ipynb @@ -0,0 +1,130 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from IPython import get_ipython\n", + "\n", + "# Must check immediately if we are running as a notebook or as a test.\n", + "running_as_notebook = (get_ipython() is not None)\n", + "\n", + "running_as_notebook = False # my test isn't working yet." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# PoseSliders\n", + "\n", + "Running this cell should allow you to control the 6 DOF pose of a mustard bottle in drake visualizer." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from IPython.display import display\n", + "from ipywidgets import ToggleButton\n", + "import numpy as np\n", + "\n", + "from pydrake.common import FindResourceOrThrow\n", + "from pydrake.common.value import AbstractValue\n", + "from pydrake.geometry import ConnectDrakeVisualizer, FramePoseVector, SceneGraph\n", + "from pydrake.math import RigidTransform\n", + "from pydrake.multibody.plant import MultibodyPlant\n", + "from pydrake.multibody.parsing import Parser\n", + "from pydrake.multibody.tree import BodyIndex\n", + "from pydrake.systems.analysis import Simulator \n", + "from pydrake.systems.framework import DiagramBuilder, LeafSystem\n", + " \n", + "from pydrake.systems.jupyter_widgets import PoseSliders\n", + "\n", + "\n", + "class PoseToFramePoseVector(LeafSystem):\n", + " \"\"\"\n", + " Wraps a single pose input into a FramePoseVector.\n", + " \"\"\"\n", + " \n", + " def __init__(self, frame_id):\n", + " LeafSystem.__init__(self)\n", + " self.frame_id = frame_id\n", + " self.DeclareAbstractInputPort(\n", + " \"pose\", AbstractValue.Make(RigidTransform.Identity()))\n", + " self.DeclareAbstractOutputPort(\n", + " \"vector\", lambda: AbstractValue.Make(FramePoseVector()), self.CalcOutput)\n", + "\n", + " def CalcOutput(self, context, output):\n", + " pose = self.EvalAbstractInput(context, 0).get_value()\n", + " output.get_value().set_value(id=frame_id, value=pose)\n", + " \n", + "\n", + "builder = DiagramBuilder()\n", + "scene_graph = builder.AddSystem(SceneGraph())\n", + "plant = MultibodyPlant(time_step=0.0)\n", + "plant.RegisterAsSourceForSceneGraph(scene_graph)\n", + "Parser(plant, scene_graph).AddModelFromFile(FindResourceOrThrow(\n", + " \"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf\"))\n", + "plant.Finalize()\n", + "\n", + "frame_id = plant.GetBodyFrameIdOrThrow(BodyIndex(1)) \n", + " \n", + "# Add pose sliders.\n", + "sliders = builder.AddSystem(PoseSliders(visible=[True] * 6))\n", + "to_vector = builder.AddSystem(PoseToFramePoseVector(frame_id))\n", + "builder.Connect(sliders.get_output_port(0), to_vector.get_input_port(0))\n", + "builder.Connect(\n", + " to_vector.get_output_port(0),\n", + " scene_graph.get_source_pose_port(plant.get_source_id()))\n", + "\n", + "vis = ConnectDrakeVisualizer(builder, scene_graph)\n", + "\n", + "diagram = builder.Build()\n", + "simulator = Simulator(diagram)\n", + "\n", + "if running_as_notebook: \n", + " simulator.set_target_realtime_rate(1.0)\n", + " stop_button = ToggleButton(value=False, description='Stop Simulation')\n", + " display(stop_button)\n", + " while not stop_button.value:\n", + " simulator.AdvanceTo(simulator.get_context().get_time() + 5.0)\n", + " stop_button.value = False\n", + "else: # running as a test.\n", + " simulator.AdvanceTo(0.1)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.9" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} From e3beced2025277411b456e11450316d07a146f5a Mon Sep 17 00:00:00 2001 From: Andres Valenzuela Date: Mon, 10 Aug 2020 09:35:32 -0400 Subject: [PATCH 3/4] pydrake: Respect mesh scaling in PlanarSceneGraphVisualizer (#13836) * pydrake: Respect mesh scaling in PlanarSceneGraphVisualizer --- .../pydrake/systems/planar_scenegraph_visualizer.py | 4 +++- .../systems/test/planar_scenegraph_visualizer_test.py | 11 +++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/bindings/pydrake/systems/planar_scenegraph_visualizer.py b/bindings/pydrake/systems/planar_scenegraph_visualizer.py index 122ce48285fa..9a7131715819 100644 --- a/bindings/pydrake/systems/planar_scenegraph_visualizer.py +++ b/bindings/pydrake/systems/planar_scenegraph_visualizer.py @@ -267,7 +267,9 @@ def _build_body_patches(self, use_random_colors, if not os.path.exists(filename): raise FileNotFoundError(errno.ENOENT, os.strerror( errno.ENOENT), filename) - mesh = ReadObjToSurfaceMesh(filename) + # Get mesh scaling. + scale = geom.float_data[0] + mesh = ReadObjToSurfaceMesh(filename, scale) patch_G = np.vstack([v.r_MV() for v in mesh.vertices()]).T else: diff --git a/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py b/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py index b915096e6020..9212ad5574ec 100644 --- a/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py +++ b/bindings/pydrake/systems/test/planar_scenegraph_visualizer_test.py @@ -139,12 +139,12 @@ def test_mesh_file_parsing(self): This test ensures we can load obj files or provide a reasonable error message. """ - def scene_graph_with_mesh(filename): + def scene_graph_with_mesh(filename, scale=1.0): builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) world_body = mbp.world_body() - mesh_shape = Mesh(filename) + mesh_shape = Mesh(filename, scale=scale) mesh_body = mbp.AddRigidBody("mesh", SpatialInertia( mass=1.0, p_PScm_E=np.array([0., 0., 0.]), G_SP_E=UnitInertia(1.0, 1.0, 1.0))) @@ -185,6 +185,13 @@ def scene_graph_with_mesh(filename): with self.assertRaises(RuntimeError): PlanarSceneGraphVisualizer(scene_graph) + # This should load correctly and yield a very large patch. + scene_graph = scene_graph_with_mesh(mesh_name, 1e3) + visualizer = PlanarSceneGraphVisualizer(scene_graph) + _, _, width, height = visualizer.ax.dataLim.bounds + self.assertTrue(width > 10.0) + self.assertTrue(height > 10.0) + def testConnectPlanarSceneGraphVisualizer(self): """Cart-Pole with simple geometry.""" file_name = FindResourceOrThrow( From 92441c27aedbb60ee5885fd813258832e9a0d50a Mon Sep 17 00:00:00 2001 From: Eric Cousineau Date: Mon, 10 Aug 2020 12:55:07 -0400 Subject: [PATCH 4/4] remove need for "is interactive" check --- .../systems/jupyter_widgets_examples.ipynb | 90 +++++++++---------- 1 file changed, 43 insertions(+), 47 deletions(-) diff --git a/bindings/pydrake/systems/jupyter_widgets_examples.ipynb b/bindings/pydrake/systems/jupyter_widgets_examples.ipynb index 373635b72940..2d7da415a149 100644 --- a/bindings/pydrake/systems/jupyter_widgets_examples.ipynb +++ b/bindings/pydrake/systems/jupyter_widgets_examples.ipynb @@ -1,19 +1,5 @@ { "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "from IPython import get_ipython\n", - "\n", - "# Must check immediately if we are running as a notebook or as a test.\n", - "running_as_notebook = (get_ipython() is not None)\n", - "\n", - "running_as_notebook = False # my test isn't working yet." - ] - }, { "cell_type": "markdown", "metadata": {}, @@ -30,7 +16,7 @@ "outputs": [], "source": [ "from IPython.display import display\n", - "from ipywidgets import ToggleButton\n", + "import ipywidgets as widgets\n", "import numpy as np\n", "\n", "from pydrake.common import FindResourceOrThrow\n", @@ -40,62 +26,72 @@ "from pydrake.multibody.plant import MultibodyPlant\n", "from pydrake.multibody.parsing import Parser\n", "from pydrake.multibody.tree import BodyIndex\n", - "from pydrake.systems.analysis import Simulator \n", + "from pydrake.systems.analysis import Simulator\n", "from pydrake.systems.framework import DiagramBuilder, LeafSystem\n", - " \n", + "\n", "from pydrake.systems.jupyter_widgets import PoseSliders\n", "\n", "\n", "class PoseToFramePoseVector(LeafSystem):\n", - " \"\"\"\n", - " Wraps a single pose input into a FramePoseVector.\n", - " \"\"\"\n", - " \n", - " def __init__(self, frame_id):\n", - " LeafSystem.__init__(self)\n", - " self.frame_id = frame_id\n", - " self.DeclareAbstractInputPort(\n", - " \"pose\", AbstractValue.Make(RigidTransform.Identity()))\n", - " self.DeclareAbstractOutputPort(\n", - " \"vector\", lambda: AbstractValue.Make(FramePoseVector()), self.CalcOutput)\n", - "\n", - " def CalcOutput(self, context, output):\n", - " pose = self.EvalAbstractInput(context, 0).get_value()\n", - " output.get_value().set_value(id=frame_id, value=pose)\n", - " \n", + " \"\"\"\n", + " Wraps a single pose input into a FramePoseVector.\n", + " \"\"\"\n", + "\n", + " def __init__(self, frame_id):\n", + " LeafSystem.__init__(self)\n", + " self.frame_id = frame_id\n", + " self.DeclareAbstractInputPort(\n", + " \"pose\", AbstractValue.Make(RigidTransform.Identity()))\n", + " self.DeclareAbstractOutputPort(\n", + " \"vector\", lambda: AbstractValue.Make(FramePoseVector()), self.CalcOutput)\n", + "\n", + " def CalcOutput(self, context, output):\n", + " pose = self.EvalAbstractInput(context, 0).get_value()\n", + " output.get_value().set_value(id=frame_id, value=pose)\n", + "\n", "\n", "builder = DiagramBuilder()\n", "scene_graph = builder.AddSystem(SceneGraph())\n", "plant = MultibodyPlant(time_step=0.0)\n", "plant.RegisterAsSourceForSceneGraph(scene_graph)\n", "Parser(plant, scene_graph).AddModelFromFile(FindResourceOrThrow(\n", - " \"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf\"))\n", + " \"drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf\"))\n", "plant.Finalize()\n", "\n", - "frame_id = plant.GetBodyFrameIdOrThrow(BodyIndex(1)) \n", - " \n", + "frame_id = plant.GetBodyFrameIdOrThrow(\n", + " plant.GetBodyByName(\"base_link_mustard\").index())\n", + "\n", "# Add pose sliders.\n", "sliders = builder.AddSystem(PoseSliders(visible=[True] * 6))\n", "to_vector = builder.AddSystem(PoseToFramePoseVector(frame_id))\n", "builder.Connect(sliders.get_output_port(0), to_vector.get_input_port(0))\n", "builder.Connect(\n", - " to_vector.get_output_port(0),\n", - " scene_graph.get_source_pose_port(plant.get_source_id()))\n", + " to_vector.get_output_port(0),\n", + " scene_graph.get_source_pose_port(plant.get_source_id()))\n", "\n", "vis = ConnectDrakeVisualizer(builder, scene_graph)\n", "\n", "diagram = builder.Build()\n", "simulator = Simulator(diagram)\n", + "context = simulator.get_context()\n", + "\n", + "# Change this to `np.inf` if you want the sim to run forever.\n", + "t_max = 5.\n", + "\n", + "simulator.set_target_realtime_rate(1.0)\n", + "stop_button = widgets.Button(description=\"Stop Simulation\")\n", + "done = False\n", + "\n", + "def on_stop(_):\n", + " global done\n", + " done = True\n", + "\n", + "stop_button.on_click(on_stop)\n", + "display(stop_button)\n", "\n", - "if running_as_notebook: \n", - " simulator.set_target_realtime_rate(1.0)\n", - " stop_button = ToggleButton(value=False, description='Stop Simulation')\n", - " display(stop_button)\n", - " while not stop_button.value:\n", - " simulator.AdvanceTo(simulator.get_context().get_time() + 5.0)\n", - " stop_button.value = False\n", - "else: # running as a test.\n", - " simulator.AdvanceTo(0.1)\n" + "while not done and context.get_time() < t_max:\n", + " simulator.AdvanceTo(context.get_time() + 0.1)\n", + "print(\"Simulation Done\")" ] }, {