From de40731e3336296208d976608a0975f4d6ad1e96 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Thu, 30 Jul 2020 21:43:33 -0400 Subject: [PATCH] Provides support for using ipywidgets with the systems framework, and PoseSliders as the first useful widget system. --- bindings/pydrake/common/BUILD.bazel | 7 + bindings/pydrake/common/jupyter.py | 69 ++++++++ bindings/pydrake/systems/BUILD.bazel | 30 ++++ bindings/pydrake/systems/jupyter_widgets.py | 158 ++++++++++++++++++ .../systems/jupyter_widgets_examples.ipynb | 128 ++++++++++++++ 5 files changed, 392 insertions(+) create mode 100644 bindings/pydrake/common/jupyter.py create mode 100644 bindings/pydrake/systems/jupyter_widgets.py create mode 100644 bindings/pydrake/systems/jupyter_widgets_examples.ipynb diff --git a/bindings/pydrake/common/BUILD.bazel b/bindings/pydrake/common/BUILD.bazel index 9e473a982372..5bde1d300a94 100644 --- a/bindings/pydrake/common/BUILD.bazel +++ b/bindings/pydrake/common/BUILD.bazel @@ -140,6 +140,12 @@ drake_cc_library( ], ) +drake_py_library( + name = "jupyter_py", + srcs = ["jupyter.py"], + imports = PACKAGE_INFO.py_imports, +) + drake_pybind_library( name = "value_py", cc_deps = [ @@ -328,6 +334,7 @@ PY_LIBRARIES = [ ":cpp_param_py", ":cpp_template_py", ":deprecation_py", + ":jupyter_py", ":module_py", ":pybind11_version_py", ] diff --git a/bindings/pydrake/common/jupyter.py b/bindings/pydrake/common/jupyter.py new file mode 100644 index 000000000000..186ab1bf6dac --- /dev/null +++ b/bindings/pydrake/common/jupyter.py @@ -0,0 +1,69 @@ +""" +Provides support for using ipywidgets with the systems framework, and a number +of useful widget systems. +""" + +import asyncio +import sys +from IPython import get_ipython +from warnings import warn + + +# 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 it only offers the solution of using separate +# threads for execution; a workflow that we do not wish to impose on users. + +def process_ipywidget_events(num_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_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) + + if len(events) > 0: + loop = asyncio.get_event_loop() + if loop.is_running(): + loop.call_soon(lambda: _replay_events(shell, events)) + else: + warn("One of your components is attempting to use pydrake's " + "process_ipywidget_events function. However, this IPython " + "kernel is not asyncio-based. This means the following:\n" + " (1) Once your block cell is done executing, future cells " + "will *not* execute, but it may appear like they are still " + "executing ([*]).\n" + " (2) Your Jupyter UI may break. If you find your UI to be " "unresponsive, you may need to restart the UI itself.\n" + "To avoid this behavior, avoid requesting execution of future " + "cells before or during execution of this cell." + ) diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 770c7077622b..589643868514 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,18 @@ 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", + "//bindings/pydrake/common:jupyter_py", + ], +) + drake_py_library( name = "perception_py", srcs = ["perception.py"], @@ -315,6 +328,7 @@ PY_LIBRARIES_WITH_INSTALL = [ PY_LIBRARIES = [ ":drawing_py", ":meshcat_visualizer_py", + ":jupyter_widgets_py", ":scalar_conversion_py", ":module_py", ":perception_py", @@ -373,6 +387,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..0b5be8dff156 --- /dev/null +++ b/bindings/pydrake/systems/jupyter_widgets.py @@ -0,0 +1,158 @@ +""" +Provides support for using ipywidgets with the systems framework, and a number +of useful widget systems. +""" + +import numpy as np +from collections import namedtuple +from ipywidgets import FloatSlider, Layout + +from pydrake.common.jupyter import process_ipywidget_events +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 + """ + # TODO(russt): Use namedtuple defaults parameter once we are Python >= 3.7. + Visible = namedtuple("Visible", ("roll", "pitch", "yaw", "x", "y", "z")) + Visible.__new__.__defaults__ = (True, True, True, True, True, True) + MinRange = namedtuple("Visible", ("roll", "pitch", "yaw", "x", "y", "z")) + MinRange.__new__.__defaults__ = (-np.pi, -np.pi, -np.pi, -1.0, -1.0, -1.0) + MaxRange = namedtuple("Visible", ("roll", "pitch", "yaw", "x", "y", "z")) + MaxRange.__new__.__defaults__ = (np.pi, np.pi, np.pi, 1.0, 1.0, 1.0) + Value = namedtuple("Visible", ("roll", "pitch", "yaw", "x", "y", "z")) + Value.__new__.__defaults__ = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + def __init__(self, visible=Visible(), min_range=MinRange(), + max_range=MaxRange(), value=Value()): + """ + Args: + visible: An object with boolean elements for 'roll', 'pitch', + 'yaw', 'x', 'y', 'z'; the intention is for this to be the + PoseSliders.Visible() namedtuple. Defaults to all true. + min_range, max_range, value: Objects with float values for 'roll', + 'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the + caller to use the PoseSliders.MinRange, MaxRange, and + Value namedtuples. See those tuples for default values. + """ + 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=min_range.roll, + max=max_range.roll, + value=value.roll, + step=0.01, + continuous_update=True, + description="roll", + layout=Layout(width='90%')) + self._pitch = FloatSlider(min=min_range.pitch, + max=max_range.pitch, + value=value.pitch, + step=0.01, + continuous_update=True, + description="pitch", + layout=Layout(width='90%')) + self._yaw = FloatSlider(min=min_range.yaw, + max=max_range.yaw, + value=value.yaw, + step=0.01, + continuous_update=True, + description="yaw", + layout=Layout(width='90%')) + self._x = FloatSlider(min=min_range.x, + max=max_range.x, + value=value.x, + step=0.01, + continuous_update=True, + description="x", + orient='horizontal', + layout=Layout(width='90%')) + self._y = FloatSlider(min=min_range.y, + max=max_range.y, + value=value.y, + step=0.01, + continuous_update=True, + description="y", + layout=Layout(width='90%')) + self._z = FloatSlider(min=min_range.z, + max=max_range.z, + value=value.z, + step=0.01, + continuous_update=True, + description="z", + layout=Layout(width='90%')) + + if visible.roll: + display(self._roll) + if visible.pitch: + display(self._pitch) + if visible.yaw: + display(self._yaw) + if visible.x: + display(self._x) + if visible.y: + display(self._y) + if visible.z: + 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 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 DoPublish(self, context, event): + """ + Allow the ipython kernel to process the event queue. + """ + process_ipywidget_events() + + 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])) diff --git a/bindings/pydrake/systems/jupyter_widgets_examples.ipynb b/bindings/pydrake/systems/jupyter_widgets_examples.ipynb new file mode 100644 index 000000000000..9843e86ad75f --- /dev/null +++ b/bindings/pydrake/systems/jupyter_widgets_examples.ipynb @@ -0,0 +1,128 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "running_as_notebook = False # Manually set this to True if you are a human (see #13862)." + ] + }, + { + "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", + "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(\n", + " visible=PoseSliders.Visible(),\n", + " min_range=PoseSliders.MinRange(), \n", + " max_range=PoseSliders.MaxRange(),\n", + " value=PoseSliders.Value()))\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.8.5" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +}