roboticstoolbox-python 1.3.0__cp312-cp312-win_amd64.whl

roboticstoolbox/backends/PyPlot/PyPlot.py

@@ -0,0 +1,769 @@
+#!/usr/bin/env python
+"""
+@author Jesse Haviland
+"""
+
+import time
+import io
+import sys
+import roboticstoolbox as rp
+import numpy as np
+from roboticstoolbox.backends.Connector import Connector
+
+from roboticstoolbox.backends.PyPlot.RobotPlot import RobotPlot
+from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot, ShapePlot
+from spatialmath.base.argcheck import getvector
+from spatialgeometry import Shape
+
+# from roboticstoolbox.tools import Ticker
+
+_mpl = False
+_pil = None
+_ipy_display = None
+_ipy_image = None
+_ipy_svg = None
+_ipy_clear_output = 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 display as _ipy_display
+    from IPython.display import Image as _ipy_image
+    from IPython.display import SVG as _ipy_svg
+    from IPython.display import clear_output as _ipy_clear_output
+except ImportError:  # pragma nocover
+    pass
+
+
+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.
+
+    """
+
+    def __init__(self):
+
+        super(PyPlot, self).__init__()
+        self.robots = []
+        self.ellipses = []
+        self.shapes = []
+        self.render_mode = "window"
+        self.inline_every_n = 1
+        self.inline_format = "svg"
+        self.inline_dpi = None
+        self._inline_step_count = 0
+        self._inline_display_handle = 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):
+        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
+
+    def launch(self, name=None, fig=None, ax=None, limits=None, **kwargs):
+        """
+        Launch a graphical interface
+
+        ```env = launch()``` creates a blank 3D matplotlib figure and returns
+        a reference to the backend.
+
+        Optional keyword arguments
+        --------------------------
+        ax
+            Existing Matplotlib 3D axes to render into.
+        render_mode
+            One of ``'window'``, ``'notebook-widget'``, or ``'notebook-inline'``.
+        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.
+        """
+
+        super().launch()
+
+        self.render_mode = _resolve_render_mode(kwargs.get("render_mode"))
+        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
+                manager = getattr(self.fig.canvas, "manager", None)
+                if manager is not None and hasattr(manager, "set_window_title"):
+                    manager.set_window_title(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")
+        manager = getattr(self.ax.figure.canvas, "manager", None)
+        if manager is not None and hasattr(manager, "set_window_title"):
+            manager.set_window_title(
+                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
+
+        # 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
+
+    def step(self, dt=0.05):
+        """
+        Update the graphical scene
+
+        :param dt: time step in seconds, defaults to 50 (0.05 s)
+        :type dt: int, optional
+
+        ``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.
+            - Execution is blocked for the specified interval
+
+        """
+
+        super().step()
+
+        # 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)
+
+    def reset(self):
+        """
+        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??
+
+        super().reset()
+
+    def restart(self):
+        """
+        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??
+
+        super().restart()
+
+    def close(self):
+        """
+        ``env.close()`` gracefully closes the matplotlib window
+        referenced by ``env``.
+        """
+        # TODO what does this actually do for matplotlib??
+
+        super().close()
+
+        # signal.setitimer(signal.ITIMER_REAL, 0)
+        plt.close(self.fig)
+
+    #
+    #  Methods to interface with the robots created in other environments
+    #
+
+    def add(
+        self,
+        ob,
+        readonly=False,
+        display=True,
+        jointaxes=True,
+        jointlabels=False,
+        eeframe=True,
+        shadow=True,
+        name=True,
+        options=None,
+    ):
+        """
+        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
+
+        ``id = env.add(robot)`` adds the ``robot`` to the graphical
+            environment.
+
+        .. 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.
+
+        """
+
+        super().add()
+
+        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,
+                )
+            )
+            self.robots[-1].draw()
+            id = len(self.robots)
+
+        elif isinstance(ob, EllipsePlot):
+            ob.ax = self.ax
+            self.ellipses.append(ob)
+            self.ellipses[-1].draw()
+            id = len(self.ellipses)
+
+        elif isinstance(ob, Shape):
+            # recreate the shape using matplotlib
+            self.shapes.append(ShapePlot(ob))
+            self.shapes[-1].draw(ax=self.ax)
+            id = 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 id
+
+    def remove(self, id):
+        """
+        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?
+
+        super().remove()
+
+    def hold(self):  # 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()
+
+    def getframe(self):
+        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
+        return _pil("RGB", canvas.get_width_height(), canvas.tostring_rgb())
+
+    def _push_inline_frame(self):
+        # 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):
+        """
+        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()
+
+        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, q):
+
+        if _isnotebook():
+            raise RuntimeError("cannot use teach panel under Jupyter")
+
+        fig = self.fig
+
+        # Add text to the plots
+        def text_trans(text, q):  # pragma: no cover
+            T = robot.fkine(q)
+            t = np.round(T.t, 3)
+            r = np.round(T.rpy("deg"), 3)
+            text[0].set_text("x: {0}".format(t[0]))
+            text[1].set_text("y: {0}".format(t[1]))
+            text[2].set_text("z: {0}".format(t[2]))
+            text[3].set_text("r: {0}".format(r[0]))
+            text[4].set_text("p: {0}".format(r[1]))
+            text[5].set_text("y: {0}".format(r[2]))
+
+        # Update the self state in mpl and the text
+        def update(val, text, robot):  # 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, "x: {0}".format(t[0]), fontsize=9, color="#2b2b2b"
+            )
+        )
+        text.append(
+            fig.text(
+                0.03, 1 - ym + 0.16, "y: {0}".format(t[1]), fontsize=9, color="#2b2b2b"
+            )
+        )
+        text.append(
+            fig.text(
+                0.03, 1 - ym + 0.12, "z: {0}".format(t[2]), fontsize=9, color="#2b2b2b"
+            )
+        )
+        text.append(
+            fig.text(
+                0.15, 1 - ym + 0.20, "r: {0}".format(r[0]), fontsize=9, color="#2b2b2b"
+            )
+        )
+        text.append(
+            fig.text(
+                0.15, 1 - ym + 0.16, "p: {0}".format(r[1]), fontsize=9, color="#2b2b2b"
+            )
+        )
+        text.append(
+            fig.text(
+                0.15, 1 - ym + 0.12, "y: {0}".format(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():
+    """
+    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__
+        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=None):
+    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-widget"
+
+    return "notebook-inline"