roboticstoolbox-python 1.4.0__py3-none-any.whl

roboticstoolbox/backends/swift/__init__.py

@@ -0,0 +1,165 @@
+import numpy as np
+
+from swift.SwiftRoute import SwiftServer, SwiftSocket, start_servers
+from swift.Elements import (
+    SwiftElement,
+    Slider,
+    Select,
+    Checkbox,
+    Radio,
+    Button,
+    Label,
+)
+from swift.Swift import Swift as _SwiftBase
+
+
+class Swift(_SwiftBase):
+    """Swift backend with RTB capability flags."""
+
+    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",
+    "Slider",
+    "SwiftElement",
+    "Label",
+    "Select",
+    "Button",
+    "Checkbox",
+    "Radio",
+    "SwiftServer",
+    "SwiftSocket",
+    "start_servers",
+]

roboticstoolbox/bin/_bintools.py

@@ -0,0 +1,61 @@
+"""
+Shared helpers for RTB command-line tools.
+"""
+
+from __future__ import annotations
+
+import argparse
+import textwrap
+
+
+def link(uri: str, label: str | None = None) -> str:
+    """Return a terminal hyperlink escape sequence.
+
+    :param uri: target URL
+    :param label: display label, defaults to the URI itself
+    :return: OSC-8 hyperlink string
+    """
+    # https://stackoverflow.com/questions/40419276/python-how-to-print-text-to-console-as-hyperlink
+    if label is None:
+        label = uri
+    escape_mask = "\033]8;{};{}\033\\{}\033]8;;\033\\"
+    return escape_mask.format("", uri, label)
+
+
+class CustomHelpFormatter(argparse.HelpFormatter):
+    """Argparse formatter that wraps at word boundaries on explicit newlines."""
+
+    def _split_lines(self, text: str, width: int) -> list[str]:
+        lines = text.splitlines()
+        wrapped: list[str] = []
+        for line in lines:
+            wrapped.extend(
+                textwrap.wrap(
+                    line, width, break_long_words=False, break_on_hyphens=False
+                )
+            )
+        return wrapped
+
+
+class CustomDefaultsHelpFormatter(
+    CustomHelpFormatter, argparse.ArgumentDefaultsHelpFormatter
+):
+    """Custom formatter that also appends default values in help output."""
+
+
+class LineWrapRawTextHelpFormatter(argparse.RawDescriptionHelpFormatter):
+    """Argparse formatter that reflowes whitespace and wraps at 80 columns."""
+
+    def _split_lines(self, text: str, width: int) -> list[str]:
+        text = self._whitespace_matcher.sub(" ", text).strip()
+        return textwrap.wrap(text, 80)
+
+
+class LineWrapRawTextDefaultsHelpFormatter(
+    LineWrapRawTextHelpFormatter, argparse.ArgumentDefaultsHelpFormatter
+):
+    """Line-wrapped formatter that also appends default values."""
+
+
+RTB_URL = "https://github.com/petercorke/robotics-toolbox-python"
+RTB_LINK = link(RTB_URL, "Robotics Toolbox for Python")

roboticstoolbox/bin/rtbtool.py

@@ -0,0 +1,404 @@
+#!/usr/bin/env python3
+"""
+Interactive Robotics Toolbox shell — starts an IPython session with NumPy,
+RTB, and SpatialMath pre-imported.
+
+Usage::
+
+    $ rtbtool
+    $ rtbtool myscript.py
+"""
+
+# import stuff
+import argparse
+import pathlib
+import shlex
+import sys
+import os
+from importlib.metadata import version
+
+from roboticstoolbox.bin._bintools import LineWrapRawTextDefaultsHelpFormatter
+
+try:
+    from colored import fg, bg, attr
+
+    _colored = True
+    # print('using colored output')
+except ImportError:
+    # print('colored not found')
+    _colored = False
+    fg = lambda *args, **kwargs: ""
+    bg = lambda *args, **kwargs: ""
+    attr = lambda *args, **kwargs: ""
+
+# imports for use by IPython and user
+import math
+from math import pi  # lgtm [py/unused-import]
+import numpy as np
+from scipy import linalg, optimize
+import matplotlib.pyplot as plt  # lgtm [py/unused-import]
+import matplotlib as mpl
+
+from spatialmath import *  # lgtm [py/polluting-import]
+from spatialmath.base import *
+import spatialmath.base as smb
+from spatialmath.base import sym
+
+from spatialgeometry import *  # lgtm [py/polluting-import]
+
+from roboticstoolbox import *  # lgtm [py/unused-import]
+
+_OPTIONS_ENVVAR = "RTB_OPTIONS"
+
+
+def env_arguments(parser):
+    """Return command-line style options from the environment.
+
+    :param parser: argument parser used for error reporting
+    :type parser: :class:`argparse.ArgumentParser`
+    :return: tokenised environment arguments
+    :rtype: list[str]
+    """
+    options = os.environ.get(_OPTIONS_ENVVAR)
+    if not options:
+        return []
+
+    try:
+        return shlex.split(options)
+    except ValueError as exc:
+        parser.error(f"invalid {_OPTIONS_ENVVAR}: {exc}")
+
+
+def parse_arguments():
+    parser = argparse.ArgumentParser(
+        description="Robotics Toolbox shell",
+        formatter_class=LineWrapRawTextDefaultsHelpFormatter,
+        epilog=(
+            "options can be set via the environment variable RTB_OPTIONS, "
+            "for example:\n\n"
+            "    $ export RTB_OPTIONS=\"--backend TkAgg --prompt 'rtb> ' "
+            '--reload --showassign"\n'
+        ),
+    )
+    parser.add_argument("script", default=None, nargs="?", help="specify script to run")
+    parser.add_argument(
+        "--backend", "-B", default=None, help="specify graphics backend"
+    )
+    parser.add_argument(
+        "--theme",
+        "-t",
+        default="neutral",
+        help="specify terminal color theme (neutral, lightbg, nocolor, linux), linux is for dark mode",
+    )
+    parser.add_argument(
+        "--confirmexit",
+        "-x",
+        default=False,
+        action="store_true",
+        help="confirm exit",
+    )
+    parser.add_argument("--prompt", "-P", default="(rtb) >>> ", help="input prompt")
+    parser.add_argument(
+        "--resultprefix",
+        "-R",
+        default=None,
+        help="execution result prefix, include {} for execution count number",
+    )
+    parser.add_argument(
+        "--reload",
+        default=False,
+        action="store_true",
+        help="enable autoreload of any imported modules, same as IPython's builtin %%autoreload 2",
+    )
+    parser.add_argument(
+        "--no-banner",
+        dest="banner",
+        default=True,
+        action="store_false",
+        help="suppress startup banner",
+    )
+    parser.add_argument(
+        "--showassign",
+        "-a",
+        default=False,
+        action="store_true",
+        help="display the result of assignments",
+    )
+    parser.add_argument(
+        "--book",
+        default=False,
+        action="store_true",
+        help="use defaults as per RVC book",
+    )
+    parser.add_argument(
+        "--ansi",
+        default=False,
+        action="store_true",
+        help="use ANSImatrix to display matrices",
+    )
+    parser.add_argument(
+        "--examples",
+        "-e",
+        default=False,
+        action="store_true",
+        help="change working directory to shipped examples",
+    )
+    parser.add_argument(
+        "--swift",
+        "-s",
+        default=False,
+        action="store_true",
+        help="use Swift as default backend",
+    )
+    parser.add_argument(
+        "--test",
+        default=False,
+        action="store_true",
+        help="non-interactive environment smoke test: print package versions, "
+        "exercise one real numeric code path per package, exit 0/1 "
+        "instead of starting an interactive shell",
+    )
+
+    argv = env_arguments(parser) + sys.argv[1:]
+    args, rest = parser.parse_known_args(argv)
+
+    if args.script is not None:
+        args.banner = False
+
+    return args, rest
+
+
+def get_versions() -> list[str]:
+    """Package version strings shown in the banner and by --test."""
+    return [
+        f"RTB=={version('roboticstoolbox-python')}",
+        f"SMTB=={version('spatialmath-python')}",
+        f"SG=={version('spatialgeometry')}",
+        f"NumPy=={version('numpy')}",
+        f"SciPy=={version('scipy')}",
+        f"Matplotlib=={version('matplotlib')}",
+    ]
+
+
+def make_banner():
+    # banner template
+    # https://patorjk.com/software/taag/#p=display&f=Cybermedium&t=Robotics%20Toolbox%0A
+
+    banner = r"""\
+    ____ ____ ___  ____ ___ _ ____ ____    ___ ____ ____ _    ___  ____ _  _
+    |__/ |  | |__] |  |  |  | |    [__      |  |  | |  | |    |__] |  |  \/
+    |  \ |__| |__] |__|  |  | |___ ___]     |  |__| |__| |___ |__] |__| _/\_
+
+    for Python"""
+
+    # create banner
+    banner += " (" + ", ".join(get_versions()) + ")"
+    banner += r"""
+
+    import math
+    import numpy as np
+    from scipy import linalg, optimize
+    import matplotlib.pyplot as plt
+    from spatialmath import *
+    from spatialmath.base import *
+    from spatialmath.base import sym
+    from roboticstoolbox import *"
+
+    # useful variables
+    from math import pi
+    puma = models.DH.Puma560()
+    panda = models.DH.Panda()
+
+    func/object?       - show brief help
+    help(func/object)  - show detailed help
+    func/object??      - show source code
+
+    """
+
+    return banner
+
+
+def examples_path():
+    # Repository layout: <root>/src/roboticstoolbox/bin/rtbtool.py
+    return pathlib.Path(__file__).resolve().parents[3] / "examples"
+
+
+def startup():
+    plt.ion()
+
+
+def run_smoke_test() -> bool:
+    """Non-interactive environment sanity check, used by --test.
+
+    Not a substitute for the pytest suite -- a fast, human- or script-run
+    "did this environment actually come together correctly" check: real
+    versions, confirmation the compiled extensions loaded, and one real
+    numeric result compared against a known-correct value. The last part
+    matters specifically because a compiled extension built against the
+    wrong NumPy ABI can load successfully and still compute garbage --
+    checking that it merely *imported* wouldn't catch that.
+    """
+    print(", ".join(get_versions()))
+
+    from roboticstoolbox.ets.fknm import _C_AVAILABLE as fknm_c
+    from roboticstoolbox.robot.frne import _C_AVAILABLE as frne_c
+
+    panda = models.DH.Panda()
+    T = panda.fkine(panda.qr).A
+    expected = np.array(
+        [
+            [9.9500416528e-01, 0.0000000000e00, 9.9833416647e-02, 4.8400688203e-01],
+            [0.0000000000e00, -1.0000000000e00, -1.2032944640e-16, -6.8775459668e-17],
+            [9.9833416647e-02, 1.2490009027e-16, -9.9500416528e-01, 4.1302777713e-01],
+            [0.0000000000e00, 0.0000000000e00, 0.0000000000e00, 1.0000000000e00],
+        ]
+    )
+
+    checks = [
+        ("fknm compiled extension loaded", fknm_c),
+        ("frne compiled extension loaded", frne_c),
+        (
+            "Panda.fkine(qr) matches expected (1e-9)",
+            bool(np.allclose(T, expected, atol=1e-9)),
+        ),
+    ]
+
+    for name, passed in checks:
+        print(f"[{'PASS' if passed else 'FAIL'}] {name}")
+
+    n_passed = sum(1 for _, passed in checks if passed)
+    print(f"rtbtool --test: {n_passed}/{len(checks)} checks passed")
+    return n_passed == len(checks)
+
+
+def main():
+    args, ipython_args = parse_arguments()
+
+    if args.test:
+        sys.exit(0 if run_smoke_test() else 1)
+
+    try:
+        import IPython
+        from IPython.terminal.prompts import Prompts
+        from pygments.token import Token
+        from traitlets.config import Config
+    except ImportError as e:
+        sys.exit(
+            f"rtbtool requires IPython and pygments, which are not "
+            f"installed ({e}).\nInstall them with:\n\n"
+            "    pip install roboticstoolbox-python[tool]\n"
+        )
+
+    # setup defaults
+    np.set_printoptions(
+        linewidth=120,
+        formatter={"float": lambda x: f"{0:8.4g}" if abs(x) < 1e-10 else f"{x:8.4g}"},
+    )
+
+    if args.book:
+        # set book options
+        args.resultprefix = ""
+        args.prompt = ">>> "
+        args.showassign = True
+        args.ansi = False
+        args.examples = True
+
+    if args.examples:
+        path = examples_path()
+        if path.exists() and path.is_dir():
+            print(f"Changing working directory to {path}")
+            os.chdir(path)
+        else:
+            print(f"Examples directory not found: {path}")
+
+    # load some robot models after argument handling to avoid import-time side effects
+    puma = models.DH.Puma560()
+    panda = models.DH.Panda()
+
+    # set default backend for Robot.plot
+    if args.swift:
+        Robot.default_backend = "swift"
+
+    # set matrix printing mode for spatialmath
+    SE3._ansimatrix = args.ansi
+
+    # set default matplotlib backend
+    if args.backend is not None:
+        print(f"Using matplotlb backend {args.backend}")
+        mpl.use(args.backend)
+
+    # build the banner, import * packages and their versions
+
+    if args.banner:
+        banner = make_banner()
+        print(fg("yellow") + banner + attr(0))
+
+    if args.showassign and args.banner:
+        print(
+            fg("red")
+            + "Results of assignments will be displayed, use trailing ; to suppress"
+            + attr(0)
+            + "\n"
+        )
+
+    # drop into IPython
+    class MyPrompt(Prompts):
+        def in_prompt_tokens(self, cli=None):
+            return [(Token.Prompt, args.prompt)]
+
+        def out_prompt_tokens(self, cli=None):
+            if args.resultprefix is None:
+                # traditional behaviour
+                return [
+                    (Token.OutPrompt, "Out["),
+                    (Token.OutPromptNum, str(self.shell.execution_count)),
+                    (Token.OutPrompt, "]: "),
+                ]
+            else:
+                return [
+                    (Token.Prompt, args.resultprefix.format(self.shell.execution_count))
+                ]
+
+    # set configuration options, there are lots, see
+    # https://ipython.readthedocs.io/en/stable/config/options/terminal.html
+    c = Config()
+    c.InteractiveShellEmbed.colors = args.theme
+    c.InteractiveShell.confirm_exit = args.confirmexit
+    # c.InteractiveShell.prompts_class = ClassicPrompts
+    c.InteractiveShell.prompts_class = MyPrompt
+    if args.showassign:
+        c.InteractiveShell.ast_node_interactivity = "last_expr_or_assign"
+    c.TerminalIPythonApp.force_interact = False
+    # set precision, same as %precision
+    c.PlainTextFormatter.float_precision = "%.3f"
+
+    # set up a script to be executed by IPython when we get there
+    code = None
+    if args.script is not None:
+        path = pathlib.Path(args.script)
+        if not path.exists():
+            raise ValueError(f"script does not exist: {args.script}")
+        code = path.open("r").readlines()
+    if code is None:
+        code = [
+            "startup()",
+            "_prec = get_ipython().run_line_magic('precision', '%.3g'); "
+            "print(f'Default numeric formatting: {_prec}')",
+        ]
+    else:
+        code.append("plt.ion()")
+
+    if args.reload:
+        code = ["%load_ext autoreload", "%autoreload 2"] + code
+
+    c.InteractiveShellApp.exec_lines = code
+    namespace = {k: v for k, v in globals().items() if not k.startswith("__")}
+    namespace.update({"puma": puma, "panda": panda})
+
+    # clear argv so IPython doesn't try to reparse arguments we've already consumed
+    sys.argv = sys.argv[:1]
+    IPython.start_ipython(config=c, user_ns=namespace, argv=ipython_args)
+
+
+if __name__ == "__main__":
+    main()