Provides support for using ipywidgets with the systems framework, and…

… PoseSliders as the first useful widget system.

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",
 ]

bindings/pydrake/common/jupyter.py

@@ -0,0 +1,74 @@
+"""
+Provides support for using ipywidgets with the systems framework, and a number
+of useful widget systems.
+
+This is gui code; to test changes, please manually run
+//bindings/pydrake/systems/jupyter_widgets_examples.ipynb.
+"""
+
+import asyncio
+import sys
+from warnings import warn
+
+from IPython import get_ipython
+
+
+# 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 = []
+    old_handler = kernel.shell_handlers['execute_request']
+    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'] = old_handler
+
+    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.")

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"],
@@ -314,6 +327,7 @@ PY_LIBRARIES_WITH_INSTALL = [
 
 PY_LIBRARIES = [
     ":drawing_py",
+    ":jupyter_widgets_py",
     ":meshcat_visualizer_py",
     ":scalar_conversion_py",
     ":module_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,

bindings/pydrake/systems/jupyter_widgets.py

@@ -0,0 +1,162 @@
+"""
+Provides support for using ipywidgets with the systems framework, and a number
+of useful widget systems.
+
+This is gui code; to test changes, please manually run
+//bindings/pydrake/systems/jupyter_widgets_examples.ipynb.
+"""
+
+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("MinRange", ("roll", "pitch", "yaw", "x", "y", "z"))
+    MinRange.__new__.__defaults__ = (-np.pi, -np.pi, -np.pi, -1.0, -1.0, -1.0)
+    MaxRange = namedtuple("MaxRange", ("roll", "pitch", "yaw", "x", "y", "z"))
+    MaxRange.__new__.__defaults__ = (np.pi, np.pi, np.pi, 1.0, 1.0, 1.0)
+    Value = namedtuple("Value", ("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):
+        """
+        Allows 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]))