Source code for roboticstoolbox.backends.PyPlot.PyPlot2

#!/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 spatialmath import SE2

import roboticstoolbox as rp
from roboticstoolbox.backends.Connector import Connector
from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot
from roboticstoolbox.backends.PyPlot.PyPlot import _isnotebook, _resolve_render_mode
from roboticstoolbox.backends.PyPlot.RobotPlot2 import RobotPlot2
from roboticstoolbox.tools.plot import set_window_title

_mpl = False
_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 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 PyPlot2(Connector): supports_ellipse: bool = True def __init__(self) -> None: super(PyPlot2, self).__init__() self.robots: list[RobotPlot2] = [] self.ellipses: list[EllipsePlot] = [] self.sim_time: float = 0 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" if s == "": return f"PyPlot2D backend, t = {self.sim_time}, empty scene" else: return f"PyPlot2D backend, t = {self.sim_time}, scene:\n" + s
[docs] def launch(self, name: Any = None, limits: Any = None, **kwargs: Any) -> None: """ env = launch() launches a blank 2D matplotlib figure Optional keyword arguments -------------------------- render_mode One of ``'window'``, ``'notebook-widget'``, or ``'notebook-inline'``. If omitted it is chosen from the environment. animation True if frames will be stepped through in a single running cell, which selects the inline mode for the ``ipympl`` backend. 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. """ 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" labels = ["X", "Y"] if name is not None and not _isnotebook(): # jupyter does weird stuff when figures have the same name self.fig = plt.figure() else: self.fig = plt.figure() # Create a 2D axes self.ax = self.fig.add_subplot(1, 1, 1) self.ax.set_facecolor("white") set_window_title( self.fig, f"Robotics Toolbox for Python (Figure {self.ax.figure.number})", # type: ignore[union-attr] ) self.ax.set_xbound(-0.5, 0.5) self.ax.set_ybound(-0.5, 0.5) self.ax.set_xlabel(labels[0]) self.ax.set_ylabel(labels[1]) self.ax.autoscale(enable=True, axis="both", tight=False) if limits is not None: self.ax.set_xlim((limits[0], limits[1])) self.ax.set_ylim((limits[2], limits[3])) self.ax.axis("equal") # 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()
# 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)
[docs] def step(self, dt: float = 0.05) -> None: """ state = step(args) triggers the external program to make a time step of defined time updating the state of the environment as defined by the robot's actions. The will go through each robot in the list and make them act 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. :param dt: time step in seconds, which must be greater than zero in a window, where the plot pauses for ``dt``; in a notebook there is no pause :raises ValueError: if ``dt`` is not greater than zero in a window: Matplotlib's ``pause(0)`` does not return, since a timeout of zero means to wait until the event loop is stopped """ 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" ) self._step_robots(dt) # plt.ioff() self._draw_ellipses() self._draw_robots() # 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) self._update_robots()
[docs] def reset(self) -> None: """ state = reset() triggers the external program to reset to the original state defined by launch """
[docs] def restart(self) -> None: """ state = restart() triggers the external program to close and relaunch to thestate defined by launch """
[docs] def close(self) -> None: """ close() closes the plot """ # 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, eeframe: bool = True, name: bool = False, q: Any = None, **kwargs: Any, ) -> Any: """ handle = add(robot) adds the robot to the external environment. robot must be of an appropriate class. This adds a robot object to a list of robots which will act upon the step() method being called. If ``q`` is given the drawing of the robot is pinned to that pose, and the same robot can be added again in another pose; the handle that is returned is the drawing, and ``handle.q = q`` moves only that drawing. Otherwise it follows ``robot.q``. An ellipse has no handle. """ handle = None if isinstance(ob, rp.Robot2): self.robots.append( RobotPlot2(ob, self, readonly, display, eeframe, name, q=q) ) self.robots[len(self.robots) - 1].draw() handle = self.robots[-1] elif isinstance(ob, EllipsePlot): ob.ax = self.ax self.ellipses.append(ob) self.ellipses[len(self.ellipses) - 1].draw2() if self.render_mode == "notebook-inline": # Push a frame immediately so static ellipse plots render without step(). self._push_inline_frame() else: plt.draw() plt.show(block=False) return handle
[docs] def remove(self) -> None: """ id = remove(robot) removes the robot to the external environment. """
[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()
# # Private methods # def _step_robots(self, dt: Any) -> None: for rpl in self.robots: robot = rpl.robot if rpl.readonly or robot.control_type == "p": pass # pragma: no cover elif robot.control_type == "v": for i in range(robot.n): robot.q[i] += robot.qd[i] * (dt / 1000) elif robot.control_type == "a": # pragma: no cover pass else: # pragma: no cover # Should be impossible to reach raise ValueError( "Invalid robot.control_type. Must be one of 'p', 'v', or 'a'" ) def _update_robots(self) -> None: pass def _draw_robots(self) -> None: for i in range(len(self.robots)): self.robots[i].draw() def _draw_ellipses(self) -> None: for i in range(len(self.ellipses)): self.ellipses[i].draw2() 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) # def _plot_handler(self, sig, frame): # plt.pause(0.001) def _add_teach_panel( self, robot: Any, q: Any, handle: Any = None, block: bool = True ) -> None: """ Add a teach panel :param robot: Robot being taught :type robot: ERobot class :param q: initial 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 # Add text to the plots def text_trans(text: Any, q: Any) -> None: # pragma: no cover # update displayed robot pose value T = robot.fkine(q, end=robot.ee_links[0]) t = np.round(T.t, 3) r = np.round(T.theta(), 3) text[0].set_text(f"x: {t[0]}") text[1].set_text(f"y: {t[1]}") text[2].set_text(f"yaw: {r}") # Update the self state in mpl and the text def update(val: Any, text: 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.38) 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) # Set the pose text # if multiple EE, display only the first one T = SE2(robot.fkine(q, end=robot.ee_links[0])) t = np.round(T.t, 3) r = np.round(T.theta(), 3) # TODO maybe put EE name in here, possible issue with DH robot # TODO maybe display pose of all EEs, layout hassles though if robot.nbranches == 0: header = "End-effector Pose" else: header = "End-effector #0 Pose" fig.text( 0.02, 1 - ym + 0.25, header, 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.15, 1 - ym + 0.20, f"yaw: {r}", 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): # for each joint ymin = (1 - ym) - j * yh self.axjoint.append(fig.add_axes((x1, ymin, x2, 0.03), facecolor="#dbdbdb")) if robot.isrevolute(j): slider = Slider( ax=self.axjoint[j], label="q" + str(j), valmin=qlim[0, j], valmax=qlim[1, j], valinit=np.degrees(q[j]), valfmt="% .1f°", ) else: slider = Slider( ax=self.axjoint[j], label="q" + str(j), valmin=qlim[0, j], valmax=qlim[1, j], valinit=q[j], valfmt="% .1f", ) slider.on_changed(lambda x: update(x, text, robot)) self.sjoint.append(slider) robot.q = q self.step()