Repository navigation
Expand file tree
/
Copy pathPyPlot.py
More file actions
891 lines (738 loc) · 30.3 KB
/
Copy pathPyPlot.py
File metadata and controls
891 lines (738 loc) · 30.3 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
#!/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 isnotebook as _isnotebook
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
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
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._widget_owned = False
self._widget_pending = False
# JupyterLite and Colab do not update a display handle in place, so each
# frame must replace the last one by clearing the cell output
self._inline_is_jl = (
sys.platform == "emscripten" or "google.colab" in sys.modules
)
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:
if self.render_mode == "notebook-widget":
# With interactive mode on, ipympl displays the figure as soon as
# it is created, while still empty, and plt.show() displays it
# again; frontends differ in what that does (a blank figure in
# VS Code, three copies in Colab). Create it quietly and display
# the finished canvas once, in add().
with plt.ioff():
self.fig = plt.figure(name)
self._widget_owned = True
self._widget_pending = True
else:
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
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)
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
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
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
#
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()
elif self._widget_owned and _ipy_display is not None:
# a widget figure that launch() created: show it once, when first drawn,
# and after that only redraw it
if self._widget_pending:
self.fig.canvas.draw()
_ipy_display(self.fig.canvas)
self._widget_pending = False
else:
self.fig.canvas.draw_idle()
else:
plt.draw() # matplotlib refresh
plt.show(block=False)
return handle
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
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()
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 _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"