Source code for roboticstoolbox.backends.PyPlot.PyPlot

#!/usr/bin/env python
"""
@author Jesse Haviland
"""

from __future__ import annotations

import io
import sys
import time
from typing import Any

import numpy as np
from spatialgeometry import Shape
from spatialmath.base.argcheck import getvector

import roboticstoolbox as rp
from roboticstoolbox.backends.Connector import Connector
from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot, ShapePlot
from roboticstoolbox.backends.PyPlot.RobotPlot import RobotPlot
from roboticstoolbox.tools.plot import plane_view, set_window_title
from roboticstoolbox.tools.types import ArrayLike

# from roboticstoolbox.tools import Ticker

_mpl = False
_pil: Any = None
_ipy_display: Any = None
_ipy_image: Any = None
_ipy_svg: Any = None
_ipy_clear_output: Any = None

try:
    import matplotlib
    import matplotlib.pyplot as plt

    # from mpl_toolkits.mplot3d import Axes3D
    from matplotlib.widgets import Slider

    matplotlib.rcParams["pdf.fonttype"] = 42
    matplotlib.rcParams["ps.fonttype"] = 42
    plt.style.use("ggplot")
    matplotlib.rcParams["font.size"] = 7
    matplotlib.rcParams["lines.linewidth"] = 0.5
    matplotlib.rcParams["xtick.major.size"] = 1.5
    matplotlib.rcParams["ytick.major.size"] = 1.5
    matplotlib.rcParams["axes.labelpad"] = 1
    plt.rc("grid", linestyle="-", color="#dbdbdb")
    _mpl = True
except ImportError:  # pragma nocover
    pass

try:
    from IPython.display import SVG as _ipy_svg
    from IPython.display import Image as _ipy_image
    from IPython.display import clear_output as _ipy_clear_output
    from IPython.display import display as _ipy_display
except ImportError:  # pragma nocover
    pass


[docs] class PyPlot(Connector): """ Graphical backend using matplotlib matplotlib is a common and highly portable graphics library for Python, but has relatively limited 3D capability. Example: .. code-block:: python :linenos: import roboticstoolbox as rtb robot = rtb.models.DH.Panda() # create a robot pyplot = rtb.backends.PyPlot() # create a PyPlot backend pyplot.add(robot) # add the robot to the backend robot.q = robot.qz # set the robot configuration pyplot.step() # update the backend and graphical view .. note:: PyPlot is the default backend, and ``robot.plot(q)`` effectively performs lines 7-8 above. """ supports_ellipse: bool = True ax: Any # the 3D Matplotlib axes, set by launch() fig: Any # the Matplotlib figure, set by launch() timer: Any # time display, set by launch() limits: ArrayLike | None def __init__(self) -> None: super(PyPlot, self).__init__() self.sim_time: float = 0 self.robots: list[RobotPlot] = [] self.ellipses: list[EllipsePlot] = [] self.shapes: list[ShapePlot] = [] self.render_mode = "window" self.inline_every_n = 1 self.inline_format = "svg" self.inline_dpi: float | None = None self._inline_step_count = 0 self._inline_display_handle: Any = None self._inline_is_jl = False if not _mpl: # pragma nocover raise ImportError( "\n\nYou do not have matplotlib installed, do:\n" "pip install matplotlib\n\n" ) def __repr__(self) -> str: s = "" for robot in self.robots: s += f" robot: {robot.name}\n" for ellipse in self.ellipses: s += f" ellipse: {ellipse}\n" for shape in self.shapes: s += f" shape: {shape}\n" if s == "": return f"PyPlot3D backend, t = {self.sim_time}, empty scene" else: return f"PyPlot3D backend, t = {self.sim_time}, scene:\n" + s
[docs] def launch( self, name: str | None = None, fig: Any = None, ax: Any = None, limits: ArrayLike | None = None, view: str | None = None, **kwargs: Any, ) -> None: """ Launch a graphical interface ```env = launch()``` creates a blank 3D matplotlib figure and returns a reference to the backend. :param view: ``"xy"``, ``"xz"`` or ``"yz"`` to start with a flat, axis-aligned view of that plane, see :func:`~roboticstoolbox.tools.plot.plane_view`, or None for the usual three-dimensional view. The view is not locked, the plot can be rotated as usual. :raises ValueError: if ``view`` is not one of those Optional keyword arguments -------------------------- ax Existing Matplotlib 3D axes to render into. render_mode One of ``'window'``, ``'notebook-widget'``, or ``'notebook-inline'``. If omitted it is chosen from the environment, see :func:`_resolve_render_mode`. animation True if frames will be stepped through in a single running cell. In a notebook with the ``ipympl`` backend this selects the inline mode, since an ``ipympl`` figure cannot update while the cell is running. inline_every_n Push one inline frame every N simulation steps (inline mode only). inline_format Inline frame format: ``'svg'`` (default) or ``'png'``. inline_dpi DPI for PNG inline frames only. Ignored for SVG. """ # checked first, so that a bad name does not leave a figure behind view_init = plane_view(view) if view is not None else None self.render_mode = _resolve_render_mode( kwargs.get("render_mode"), bool(kwargs.get("animation", False)) ) self.inline_every_n = max(1, int(kwargs.get("inline_every_n", 1))) self.inline_format = kwargs.get("inline_format", "svg") if self.inline_format not in ["png", "svg"]: raise ValueError("inline_format must be either 'png' or 'svg'") inline_dpi = kwargs.get("inline_dpi", None) if inline_dpi is not None: inline_dpi = float(inline_dpi) if inline_dpi <= 0: raise ValueError("inline_dpi must be > 0") self.inline_dpi = inline_dpi self._inline_step_count = 0 self._inline_display_handle = None self._inline_is_jl = sys.platform == "emscripten" self.limits = limits if limits is not None: self.limits = getvector(limits, 6) projection = "ortho" labels = ["X", "Y", "Z"] if name is None: name = "Robotics Toolbox for Python" if ax is not None: if fig is not None and ax.figure is not fig: raise ValueError("ax does not belong to fig") # Caller supplied axes; reuse its parent figure. self.ax = ax self.fig = ax.figure else: if fig is None: self.fig = plt.figure(name) else: self.fig = fig set_window_title(self.fig, name) # Create a 3D axes self.ax = self.fig.add_subplot(111, projection="3d", proj_type=projection) if not hasattr(self.ax, "set_zbound"): raise ValueError("ax must be a 3D matplotlib axes") if ax is None: self.fig.subplots_adjust(left=-0.09, bottom=0, top=1, right=0.99) self.ax.set_facecolor("white") set_window_title( self.ax.figure, f"Robotics Toolbox for Python (Figure {self.ax.figure.number})", ) self.ax.set_xbound(-0.5, 0.5) self.ax.set_ybound(-0.5, 0.5) self.ax.set_zbound(0.0, 0.5) self.ax.set_xlabel(labels[0]) self.ax.set_ylabel(labels[1]) self.ax.set_zlabel(labels[2]) if limits is not None: self.ax.set_xlim3d([limits[0], limits[1]]) self.ax.set_ylim3d([limits[2], limits[3]]) self.ax.set_zlim3d([limits[4], limits[5]]) self.limits = limits if view_init is not None: self.ax.view_init(**view_init) # disable the display of value under cursor # self.ax.format_coord = lambda x, y: '' # add time display in top-right corner self.timer = self.fig.text(0.85, 0.95, "") # In inline notebook mode (notably JupyterLite), keeping this figure # registered with pyplot can trigger repeated auto-display of blank # Figure reprs. Detach it and drive rendering via _push_inline_frame(). if self.render_mode == "notebook-inline": plt.close(self.fig) if self.render_mode == "window": plt.ion() plt.show() else: if self.render_mode == "notebook-inline": plt.ioff() else: plt.ion() if self.render_mode != "notebook-inline": self.fig.canvas.draw() self.sim_time = 0
# # Set the signal handler and a 0.1 second plot updater # signal.signal(signal.SIGALRM, self._plot_handler) # signal.setitimer(signal.ITIMER_REAL, 0.1, 0.1) # TODO still need to finish this, and get Jupyter animation working
[docs] def step(self, dt: float = 0.05) -> None: """ Update the graphical scene :param dt: time step in seconds, defaults to 50 (0.05 s) :type dt: int, optional :raises ValueError: if ``dt`` is not greater than zero in a window, where the plot pauses for ``dt`` ``env.step(args)`` triggers an update of the 3D scene in the matplotlib window referenced by ``env``. .. note:: - Each robot in the scene is updated based on their control type (position, velocity, acceleration, or torque). - Upon acting, the other three of the four control types will be updated in the internal state of the robot object. - The control type is defined by the robot object, and not all robot objects support all control types. - In a window the execution is blocked for the specified interval, which must be greater than zero: Matplotlib's ``pause(0)`` does not return, since a timeout of zero means to wait until the event loop is stopped. In a notebook there is no pause and ``dt`` of zero is fine. .. versionchanged:: 1.5.0 In a window, ``dt`` of zero or less raises ``ValueError``, it used to wait for ever. """ if dt <= 0 and self.render_mode == "window": raise ValueError( f"dt is {dt}, it must be greater than zero: a pause of zero seconds " "does not return" ) # update the robot's state for rpl in self.robots: robot = rpl.robot if rpl.readonly or robot.control_mode == "p": pass # pragma: no cover elif robot.control_mode == "v": for i in range(robot.n): robot.q[i] += robot.qd[i] * (dt) elif robot.control_mode == "a": # pragma: no cover pass else: # pragma: no cover raise ValueError( "Invalid robot.control_type. Must be one of 'p', 'v', or 'a'" ) # plt.ioff() # update all ellipses for ellipse in self.ellipses: ellipse.draw() # update all shapes for shape in self.shapes: shape.draw() # update all robots for robot in self.robots: robot.draw() self._set_axes_equal() # update time and display it on plot if self.sim_time > 0: self.timer.set_text(f"t = {self.sim_time:.2f}") self.sim_time += dt # plt.ion() if self.render_mode == "window": plt.draw() plt.pause(dt) elif self.render_mode == "notebook-widget": plt.draw() self.fig.canvas.draw_idle() time.sleep(dt) else: if self._inline_step_count % self.inline_every_n == 0: self._push_inline_frame() self._inline_step_count += 1 time.sleep(dt)
[docs] def reset(self) -> None: """ Reset the graphical scene ``env.reset()`` triggers a reset of the 3D scene in the matplotlib window referenced by ``env``. It is restored to the original state defined by ``launch()``. """ # TODO what does this actually do for matplotlib?? pass
[docs] def restart(self) -> None: """ Restart the graphics display ``env.restart()`` triggers a restart of the matplotlib view referenced by ``env``. It is closed and relaunched to the original state defined by ``launch()``. """ # TODO what does this actually do for matplotlib?? pass
[docs] def close(self) -> None: """ ``env.close()`` gracefully closes the matplotlib window referenced by ``env``. """ # TODO what does this actually do for matplotlib?? # signal.setitimer(signal.ITIMER_REAL, 0) plt.close(self.fig)
# # Methods to interface with the robots created in other environments #
[docs] def add( self, ob: Any, readonly: bool = False, display: bool = True, jointaxes: bool = True, jointlabels: bool = False, eeframe: bool = True, shadow: bool = True, name: bool = True, options: Any = None, q: ArrayLike | None = None, color: Any = None, ) -> Any: """ Add a robot to the graphical scene :param ob: The object to add to the plot (robot or ellipse) :type ob: DHRobot or EllipsePlot :param readonly: Do not update the state of the object (i.e. display not simulate), defaults to False :type readonly: bool, optional :param display: Display the object, defaults to True :type display: bool, optional :param jointaxes: Show the joint axes of the robot with arrows, defaults to True :type jointaxes: bool, optional :param eeframe: Show the end-effector frame of the robot, defaults to True :type eeframe: bool, optional :param shadow: Display a shadow of the robot on the x-y gound plane, defaults to True :type shadow: bool, optional :param name: Display the name of the robot, defaults to True :type name: bool, optional :param options: colors and line widths, a dict of dicts that is merged over the defaults, for example ``{"robot": {"color": "r", "linewidth": 3}}`` :param q: the joint coordinates to draw, by default those of the robot :param color: the color of the robot's line, shorthand for ``options["robot"]["color"]`` which it overrides :returns: for a robot, its :class:`~roboticstoolbox.backends.PyPlot.RobotPlot.RobotPlot` handle, for anything else an integer id ``handle = env.add(robot)`` adds the ``robot`` to the graphical environment. The handle is the drawing of the robot in this scene, and its ``q`` is the pose that it draws. If ``q`` is given the drawing is pinned to that pose, and the same robot can be added again in another pose, each drawn where it was put; ``handle.q = q`` moves only that drawing. If ``q`` is not given the drawing follows ``robot.q``, as it has always done. .. versionchanged:: 1.5.0 Added ``q`` and ``color``, and a robot now returns its plot handle, not an integer id, which nothing could use. .. note:: - ``robot`` must be of an appropriate class. - Adds the robot object to a list of robots which will be updated when the ``step()`` method is called. """ handle: Any if color is not None: options = dict(options or {}) options["robot"] = {**options.get("robot", {}), "color": color} if ( isinstance(ob, rp.DHRobot) or isinstance(ob, rp.ERobot) or isinstance(ob, rp.Robot) ): self.robots.append( RobotPlot( ob, self, readonly, display, jointaxes, jointlabels, eeframe, shadow, name, options, q, ) ) self.robots[-1].draw() handle = self.robots[-1] elif isinstance(ob, EllipsePlot): ob.ax = self.ax self.ellipses.append(ob) self.ellipses[-1].draw() handle = len(self.ellipses) elif isinstance(ob, Shape): # recreate the shape using matplotlib self.shapes.append(ShapePlot(ob)) self.shapes[-1].draw(ax=self.ax) handle = len(self.shapes) self._set_axes_equal() if self.render_mode == "notebook-inline": # In inline/JupyterLite mode, push a frame immediately so static # plots (eg. plot_vellipse/plot_fellipse) are visible without step(). self._push_inline_frame() else: plt.draw() # matplotlib refresh plt.show(block=False) return handle
[docs] def remove(self, id: int) -> None: """ Remove a robot or shape from the graphical scene :param id: The id of the robot to remove. Can be either the DHLink or GraphicalRobot :type id: class:`~roboticstoolbox.robot.DHRobot.DHRobot`, class:`roboticstoolbox.backends.VPython.graphics_robot.GraphicalRobot` :param fig_num: The canvas index to delete the robot from, defaults to the initial one :type fig_num: int, optional :raises ValueError: Figure number must be between 0 and total number of canvases :raises TypeError: Input must be a DHLink or GraphicalRobot ``env.remove(robot)`` removes the ``robot`` from the graphical environment. """ # TODO should be an id to remove? pass
[docs] def hold(self) -> None: # pragma: no cover """ hold() keeps the plot open i.e. stops the plot from closing once the main script has finished. """ # signal.setitimer(signal.ITIMER_REAL, 0) plt.ioff() # keep stepping the environment while figure is open while True: if not plt.fignum_exists(self.fig.number): break self.step()
[docs] def getframe(self) -> Any: global _pil if _pil is None: try: import PIL _pil = PIL.Image.frombytes except ImportError: # pragma nocover pass if _pil is None: raise RuntimeError( "to save movies PIL must be installed:\npip3 install PIL" ) # make the background white, looks better than grey stipple self.ax.xaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) self.ax.yaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) self.ax.zaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) plt.gcf().canvas.draw() # render the frame and save as a PIL image in the list canvas = self.fig.canvas image = _pil("RGBA", canvas.get_width_height(), bytes(canvas.buffer_rgba())) return image.convert("RGB")
def _push_inline_frame(self) -> None: # Push a snapshot into notebook output for inline animation. if _ipy_display is None: return buf = io.BytesIO() if self.inline_format == "svg": if _ipy_svg is None: return self.fig.savefig(buf, format="svg") frame = _ipy_svg(data=buf.getvalue().decode("utf-8")) else: if _ipy_image is None: return self.fig.savefig(buf, format="png", dpi=self.inline_dpi) frame = _ipy_image(data=buf.getvalue()) if self._inline_is_jl and _ipy_clear_output is not None: _ipy_clear_output(wait=True) _ipy_display(frame) return if self._inline_display_handle is None: self._inline_display_handle = _ipy_display(frame, display_id=True) elif hasattr(self._inline_display_handle, "update"): self._inline_display_handle.update(frame) else: _ipy_display(frame) # # Private methods # # def _plot_handler(self, sig, frame): # try: # plt.pause(0.001) # except(AttributeError): # pass def _set_axes_equal(self) -> None: """ Make axes of 3D plot have equal scale so that spheres appear as spheres, cubes as cubes, etc.. This is one possible solution to Matplotlib's ax.set_aspect('equal') and ax.axis('equal') not working for 3D. """ if self.limits is not None: return self.ax.autoscale(enable=True, axis="both", tight=False) x_limits = self.ax.get_xlim3d() y_limits = self.ax.get_ylim3d() z_limits = self.ax.get_zlim3d() # Matplotlib knows the data a 3D line was created with but not the data it # was moved to, which is how every robot is drawn. Take in the points of # every robot in the scene, with the margin that autoscale leaves, so that # the view shows all of them and not only the last. points = [ np.c_[line.get_data_3d()] for rpl in self.robots if rpl.drawn and rpl.links is not None for line in rpl.links ] if points: pts = np.vstack([p.reshape(-1, 3) for p in points]) lo, hi = pts.min(axis=0), pts.max(axis=0) pad = 0.05 * np.maximum(hi - lo, 1e-9) lo, hi = lo - pad, hi + pad x_limits = (min(x_limits[0], lo[0]), max(x_limits[1], hi[0])) y_limits = (min(y_limits[0], lo[1]), max(y_limits[1], hi[1])) z_limits = (min(z_limits[0], lo[2]), max(z_limits[1], hi[2])) x_range = abs(x_limits[1] - x_limits[0]) x_middle = np.mean(x_limits) y_range = abs(y_limits[1] - y_limits[0]) y_middle = np.mean(y_limits) z_range = abs(z_limits[1] - z_limits[0]) z_middle = np.mean(z_limits) # The plot bounding box is a sphere in the sense of the infinity # norm, hence I call half the max range the plot radius. plot_radius = 0.5 * max([x_range, y_range, z_range]) self.ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius]) 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: Any, q: ArrayLike, handle: Any = None, block: bool = True ) -> None: # 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") fig = self.fig # Add text to the plots def text_trans(text: list[Any], q: ArrayLike) -> None: # pragma: no cover T = robot.fkine(q) t = np.round(T.t, 3) r = np.round(T.rpy("deg"), 3) text[0].set_text(f"x: {t[0]}") text[1].set_text(f"y: {t[1]}") text[2].set_text(f"z: {t[2]}") text[3].set_text(f"r: {r[0]}") text[4].set_text(f"p: {r[1]}") text[5].set_text(f"y: {r[2]}") # Update the self state in mpl and the text def update(val: float, text: list[Any], robot: Any) -> None: # pragma: no cover for j in range(robot.n): if robot.isrevolute(j): robot.q[j] = np.radians(self.sjoint[j].val) else: robot.q[j] = self.sjoint[j].val teach_vellipse = getattr(self, "_teach_vellipse", None) if teach_vellipse is not None: teach_vellipse.q = robot.q teach_fellipse = getattr(self, "_teach_fellipse", None) if teach_fellipse is not None: teach_fellipse.q = robot.q text_trans(text, robot.q) fig.subplots_adjust(left=0.25) text = [] x1 = 0.04 x2 = 0.22 yh = 0.04 ym = 0.5 - (robot.n * yh) / 2 + 0.17 / 2 self.axjoint = [] self.sjoint = [] qlim = robot.todegrees(robot.qlim) # if np.all(qlim == 0): # pragma: no cover # qlim[0, :] = -180 # qlim[1, :] = 180 # Set the pose text T = robot.fkine(q) t = np.round(T.t, 3) r = np.round(T.rpy(), 3) fig.text( 0.02, 1 - ym + 0.25, "End-effector Pose", fontsize=9, weight="bold", color="#4f4f4f", ) text.append( fig.text( 0.03, 1 - ym + 0.20, f"x: {t[0]}", fontsize=9, color="#2b2b2b" ) ) text.append( fig.text( 0.03, 1 - ym + 0.16, f"y: {t[1]}", fontsize=9, color="#2b2b2b" ) ) text.append( fig.text( 0.03, 1 - ym + 0.12, f"z: {t[2]}", fontsize=9, color="#2b2b2b" ) ) text.append( fig.text( 0.15, 1 - ym + 0.20, f"r: {r[0]}", fontsize=9, color="#2b2b2b" ) ) text.append( fig.text( 0.15, 1 - ym + 0.16, f"p: {r[1]}", fontsize=9, color="#2b2b2b" ) ) text.append( fig.text( 0.15, 1 - ym + 0.12, f"y: {r[2]}", fontsize=9, color="#2b2b2b" ) ) fig.text( 0.02, 1 - ym + 0.06, "Joint angles", fontsize=9, weight="bold", color="#4f4f4f", ) for j in range(robot.n): ymin = (1 - ym) - j * yh ax = fig.add_axes([x1, ymin, x2, 0.03], facecolor="#dbdbdb") self.axjoint.append(ax) if robot.isrevolute(j): slider = Slider( ax=ax, label="q" + str(j), valmin=qlim[0, j], valmax=qlim[1, j], valinit=np.degrees(q[j]), valfmt="% .1f°", ) else: slider = Slider( ax=ax, label="q" + str(j), valmin=qlim[0, j], valmax=qlim[1, j], valinit=robot.q[j], valfmt="% .1f", ) slider.on_changed(lambda x: update(x, text, robot)) self.sjoint.append(slider) robot.q = q self.step()
def _isnotebook() -> bool: """ Determine if code is being run from a Jupyter notebook or JupyterLite ``_isnotebook`` is True if running Jupyter notebook, JupyterLite, or similar, else False :references: - https://stackoverflow.com/questions/15411967/how-can-i-check-if-code- is-executed-in-the-ipython-notebook/39662359#39662359 """ import sys # Check if running in Pyodide/JupyterLite if sys.platform == "emscripten": return True # Fall back to checking IPython shell type try: shell = get_ipython().__class__.__name__ # type: ignore[name-defined] if shell == "ZMQInteractiveShell": return True # Jupyter notebook or qtconsole elif shell == "TerminalInteractiveShell": return False # Terminal running IPython else: return False # Other type (?) except NameError: return False # Probably standard Python interpreter def _resolve_render_mode( render_mode: str | None = None, animation: bool = False ) -> str: """ Choose how a figure is rendered, from the environment :param render_mode: an explicit choice, ``'window'``, ``'notebook-widget'`` or ``'notebook-inline'``, which is used as it is :param animation: True if the caller is going to animate by stepping through frames inside a single running cell :returns: the render mode In a notebook the choice follows the matplotlib backend, so the same call works whether the notebook says ``%matplotlib ipympl`` or ``%matplotlib inline``: - ``'window'`` outside a notebook, - ``'notebook-widget'`` for ``ipympl``, an interactive figure that can be rotated, - ``'notebook-inline'`` otherwise, static images pushed into the cell output. An ``ipympl`` figure updates only when the kernel handles a message from the browser, which it cannot do while a cell is running. So an animation with ``animation=True`` would show the first and the last frame only, and the inline mode, which does not depend on the browser, is chosen for it instead. """ if render_mode is not None: if render_mode not in ["window", "notebook-widget", "notebook-inline"]: raise ValueError( "render_mode must be one of 'window', " "'notebook-widget', or 'notebook-inline'" ) return render_mode if not _isnotebook(): return "window" backend = matplotlib.get_backend().lower() if "ipympl" in backend or "widget" in backend or "nbagg" in backend: return "notebook-inline" if animation else "notebook-widget" return "notebook-inline"