feat(swift): support teach() teach panel for Swift-backed robots

examples/teach_swift.py

@@ -1,58 +1,96 @@
-# #!/usr/bin/env python
-# """
-# @author Jesse Haviland
-# """
+#!/usr/bin/env python
+"""
+Hand-rolled Swift teach panel: one named slider per joint, driving the
+robot via a single per-step callback on the handle env.add_robot()
+returns -- the same mechanism roboticstoolbox's own robot.teach(q,
+backend="swift") now uses internally (see roboticstoolbox.backends.swift.
+Swift._add_teach_panel). Useful as a template for a custom panel beyond
+what teach() offers (e.g. extra UI elements alongside the sliders);
+for the common case, robot.teach(q, backend="swift") does this in one
+call.
 
-import swift
-import roboticstoolbox as rtb
+Named sliders push their value into env.values; the handle's callback
+reads from there and returns the new q each step -- there's no explicit
+per-slider setter function, and no direct robot.q/handle.q mutation in
+the loop, env.step() drives everything.
+"""
 import numpy as np
-import time
+import roboticstoolbox as rtb
+from swift import Swift, Slider, Label
 
 # Launch the simulator Swift
-env = swift.Swift()
-env.launch()
-
-# Make a Panda robot and add it to Swift
-panda = rtb.models.UR5()
-panda.q = panda.qr
-env.add(panda)
-
-
-# This is our callback funciton from the sliders in Swift which set
-# the joint angles of our robot to the value of the sliders
-def set_joint(j, value):
-    panda.q[j] = np.deg2rad(float(value))
-
-
-# Loop through each link in the Panda and if it is a variable joint,
-# add a slider to Swift to control it
-j = 0
-for link in panda.links:
-    if link.isjoint:
-        # We use a lambda as the callback function from Swift
-        # j=j is used to set the value of j rather than the variable j
-        # We use the HTML unicode format for the degree sign in the unit arg
-        env.add(
-            swift.Slider(
-                lambda x, j=j: set_joint(j, x),
-                min=np.round(np.rad2deg(link.qlim[0]), 2),
-                max=np.round(np.rad2deg(link.qlim[1]), 2),
-                step=1,
-                value=np.round(np.rad2deg(panda.q[j]), 2),
-                desc="Panda Joint " + str(j),
-                unit="°",
-            )
-        )
-
-        j += 1
-
-
-while True:
-    # Process the event queue from Swift, this invokes the callback functions
-    # from the sliders if the slider value was changed
-    # env.process_events()
-
-    # Update the environment with the new robot pose
-    env.step(0)
-
-    time.sleep(0.01)
+env = Swift()
+env.launch(ground_opacity=0.3)
+
+# Make a robot and add it to Swift
+# robot = rtb.models.UR5()
+robot = rtb.models.Panda()
+
+handle = env.add_robot(robot)
+handle.q = robot.qr
+
+# compact=True keeps six stacked Labels from taking up excessive
+# sidebar space -- Label's default styling is sized for an occasional
+# standalone heading, not several stacked close together.
+pose_labels = [Label("", compact=True) for _ in range(6)]
+for label in pose_labels:
+    env.add(label)
+
+
+def update_pose_labels(q):
+    T = robot.fkine(q)
+    t = np.round(T.t, 3)
+    r = np.round(T.rpy(unit="deg"), 3)
+    pose_labels[0].label = f"x: {t[0]}"
+    pose_labels[1].label = f"y: {t[1]}"
+    pose_labels[2].label = f"z: {t[2]}"
+    pose_labels[3].label = f"r: {r[0]}°"
+    pose_labels[4].label = f"p: {r[1]}°"
+    pose_labels[5].label = f"y: {r[2]}°"
+
+
+def teach_update(t, values):
+    # Sliders display revolute joints in degrees, prismatic in native
+    # units (metres) -- toradians() converts the whole vector back in
+    # one call, only touching revolute entries.
+    q_display = np.array([values[f"q{j}"] for j in range(robot.n)])
+    q_new = robot.toradians(q_display)
+    update_pose_labels(q_new)
+    return q_new
+
+
+handle.callback = teach_update
+
+# Loop through each joint and add a slider to Swift to control it
+qlim = robot.qlim
+for j in range(robot.n):
+    lo, hi = qlim[0, j], qlim[1, j]
+    if robot.isrevolute(j):
+        lo_disp, hi_disp, val_disp = np.degrees(lo), np.degrees(hi), np.degrees(handle.q[j])
+        step = 1.0
+        unit = "°"
+    else:
+        lo_disp, hi_disp, val_disp = lo, hi, handle.q[j]
+        step = (hi - lo) / 100
+        unit = "m"
+
+    env.add(
+        Slider(
+            lambda x: None,
+            # min/max/value stay full precision -- precision= below only
+            # rounds the *displayed* text, so the slider's actual driven
+            # value doesn't lose precision to display rounding.
+            min=float(lo_disp),
+            max=float(hi_disp),
+            step=step,
+            value=float(val_disp),
+            label=f"{robot.name} joint {j}",
+            unit=unit,
+            precision=2,
+        ),
+        name=f"q{j}",
+    )
+
+update_pose_labels(handle.q)
+
+env.run()

src/roboticstoolbox/backends/PyPlot/PyPlot.py

@@ -586,7 +586,14 @@ def _set_axes_equal(self):
         self.ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
         self.ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
 
-    def _add_teach_panel(self, robot, q):
+    def _add_teach_panel(self, robot, q, handle=None, block=True):
+        # handle, block: unused here -- PyPlot has no AssemblyHandle
+        # concept (it drives the panel by mutating robot.q directly
+        # below), and matplotlib's own GUI mainloop (entered via this
+        # backend's env.hold()) already processes slider events on its
+        # own, unlike Swift's hold() which needs an active step() loop.
+        # Both params only meaningful for the Swift backend's own
+        # _add_teach_panel().
 
         if _isnotebook():
             raise RuntimeError("cannot use teach panel under Jupyter")

src/roboticstoolbox/backends/PyPlot/PyPlot2.py

@@ -357,14 +357,23 @@ def _push_inline_frame(self):
     # def _plot_handler(self, sig, frame):
     #     plt.pause(0.001)
 
-    def _add_teach_panel(self, robot, q):
+    def _add_teach_panel(self, robot, q, handle=None, block=True):
         """
         Add a teach panel
 
         :param robot: Robot being taught
         :type robot: ERobot class
         :param q: inital joint angles in radians
         :type q: array_like(n)
+        :param handle: unused here -- PyPlot2 has no AssemblyHandle
+            concept, it drives the panel by mutating robot.q directly
+            below. Only meaningful for the Swift backend's own
+            _add_teach_panel().
+        :param block: unused here -- matplotlib's own GUI mainloop
+            (entered via this backend's env.hold()) already processes
+            slider events on its own, unlike Swift's hold() which needs
+            an active step() loop. Only meaningful for the Swift
+            backend's own _add_teach_panel().
         """
         fig = self.fig
 

src/roboticstoolbox/backends/swift/__init__.py

@@ -1,3 +1,5 @@
+import numpy as np
+
 from swift.SwiftRoute import SwiftServer, SwiftSocket, start_servers
 from swift.Elements import (
     SwiftElement,
@@ -14,9 +16,139 @@
 class Swift(_SwiftBase):
     """Swift backend with RTB capability flags."""
 
-    supports_teach: bool = False
+    supports_teach: bool = True
     supports_ellipse: bool = False
 
+    def _add_teach_panel(self, robot, q, handle, block):
+        """
+        Add a joint-slider teach panel plus a live end-effector pose
+        readout, and wire the sliders to drive ``handle`` (the
+        AssemblyHandle ``BaseRobot.teach()`` got back from ``self.add()``)
+        via a single per-step callback -- see jhavl/swift#85 for why
+        driving the robot model's own .q/.qd directly is deprecated.
+
+        :param robot: the robot being taught, already added to this scene
+        :param q: initial joint configuration to seed the panel/display
+            with -- may differ from ``robot.q``'s current value
+        :param handle: the AssemblyHandle ``self.add(robot, readonly=True)``
+            returned for this robot instance
+        :param block: unlike PyPlot (whose own env.hold() enters
+            matplotlib's GUI mainloop, which processes slider events on
+            its own), Swift's hold() only sleeps and polls for a
+            disconnect -- it never calls step(), so nothing would ever
+            notice a dragged slider without an active step() loop
+            running somewhere. When block, this method runs that loop
+            itself (self.run(), blocking until disconnect/^C) rather
+            than relying on teach()'s own later `if block: env.hold()`.
+            When not block, a single self.step() seeds the initial
+            display and the caller is responsible for stepping (matches
+            teach(block=False)'s existing contract for other backends).
+        :returns: True if block, signalling to teach()'s shared code
+            that this method already fully handled blocking itself --
+            teach()'s own `if block: env.hold()` must be skipped in that
+            case, not just as an optimisation: calling env.hold() a
+            second time here can hang outright (its disconnect-poll
+            never expires in headless mode, and even non-headless
+            there's a race around close() and socket.USERS). None
+            (falsy) when not block, matching PyPlot/PyPlot2's own
+            _add_teach_panel, which never blocks internally at all.
+        """
+        qlim = robot.qlim
+
+        # One label per value (x/y/z/r/p/y), matching PyPlot's own six
+        # separate fig.text() calls -- compact=True keeps this from
+        # taking up excessive sidebar space (Label's default styling is
+        # sized for an occasional standalone heading, not several
+        # stacked close together -- see swift's Label(compact=) docstring).
+        pose_labels = [Label("", compact=True) for _ in range(6)]
+        for label in pose_labels:
+            self.add(label)
+
+        def update_pose_labels(qv):
+            T = robot.fkine(qv)
+            t = np.round(T.t, 3)
+            r = np.round(T.rpy(unit="deg"), 3)
+            pose_labels[0].label = f"x: {t[0]}"
+            pose_labels[1].label = f"y: {t[1]}"
+            pose_labels[2].label = f"z: {t[2]}"
+            pose_labels[3].label = f"r: {r[0]}°"
+            pose_labels[4].label = f"p: {r[1]}°"
+            pose_labels[5].label = f"y: {r[2]}°"
+
+        def teach_update(t, values):
+            # Sliders display revolute joints in degrees, prismatic in
+            # native units (metres) -- toradians() converts the whole
+            # vector back in one call, only touching revolute entries.
+            q_display = np.array([values[f"q{j}"] for j in range(robot.n)])
+            q_new = robot.toradians(q_display)
+            update_pose_labels(q_new)
+            return q_new
+
+        # Safe with readonly=True: Swift.step()'s callback branch runs
+        # unconditionally whenever a callback is set -- readonly only
+        # gates the *other* (non-callback) per-step update path.
+        handle.callback = teach_update
+
+        for j in range(robot.n):
+            lo, hi = qlim[0, j], qlim[1, j]
+            if robot.isrevolute(j):
+                lo_disp, hi_disp, val_disp = np.degrees(lo), np.degrees(hi), np.degrees(q[j])
+                step = 1.0
+                unit = "°"
+            else:
+                lo_disp, hi_disp, val_disp = lo, hi, q[j]
+                step = (hi - lo) / 100
+                unit = "m"
+
+            self.add(
+                Slider(
+                    lambda x: None,
+                    # min/max/value stay full precision -- precision=
+                    # below only rounds the *displayed* text, unlike a
+                    # naive round() here which would bake rounding error
+                    # into the actual driven value once the callback
+                    # reads it back from env.values.
+                    min=float(lo_disp),
+                    max=float(hi_disp),
+                    step=step,
+                    value=float(val_disp),
+                    label=f"{robot.name} joint {j}",
+                    unit=unit,
+                    precision=2,
+                ),
+                name=f"q{j}",
+            )
+
+        update_pose_labels(q)
+        self.step()
+
+        if block:
+            # Unbounded (duration=None): keep responding to slider drags
+            # for as long as the browser stays connected, same as any
+            # other Swift script's own `while True: env.step(dt)` loop --
+            # just with run()'s disconnect-awareness (and ^C handling)
+            # folded in instead of looping forever after the tab is gone.
+            # Returns normally on a disconnect (graceful or mid-step);
+            # only ^C skips the rest of this method (raises SystemExit).
+            self.run()
+
+            # PyPlot's teach() mutates robot.q throughout its own session,
+            # so a caller naturally finds the final taught pose in
+            # robot.q once teach() returns -- see BaseRobot.teach()'s
+            # docstring. Swift's AssemblyHandle deliberately doesn't
+            # mirror handle.q into robot.q during the session (that's
+            # the whole point of the refactor -- see jhavl/swift#85), but
+            # write it back once, here, at the point the session ends,
+            # so callers see the same thing regardless of backend. A
+            # deliberate one-time exception for this specific
+            # single-owner interactive session, not a general precedent
+            # -- the same "stateless over stateful" tension PyPlot's own
+            # teach() already has (see desiderata.md), just accepted
+            # here rather than solved.
+            robot.q = handle.q.copy()
+
+            return True
+
 
 __all__ = [
     "Swift",