#!/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
[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.
"""
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
[docs]
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
[docs]
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)
[docs]
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()
[docs]
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()
[docs]
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
#
[docs]
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
[docs]
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()
[docs]
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()
[docs]
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"