8000 plot2 fixed · A905275/robotics-toolbox-python@92e5a15 · GitHub
[go: up one dir, main page]
Skip to content

Commit 92e5a15

Browse files
jhavljhavl
committed
plot2 fixed
1 parent fe75d3d commit 92e5a15

File tree

3 files changed

+227
-100
lines changed

3 files changed

+227
-100
lines changed

‎roboticstoolbox/robot/DHRobot.py

Lines changed: 57 additions & 57 deletions
Original file line numberDiff line numberDiff line change
@@ -1594,72 +1594,72 @@ def plot_fellipse(
15941594
fellipse, block, limits,
15951595
jointaxes=jointaxes, eeframe=eeframe, shadow=shadow, name=name)
15961596

1597-
def plot2(
1598-
self, q=None, block=True, dt=50, limits=None,
1599-
vellipse=False, fellipse=False,
1600-
eeframe=True, name=False):
1601-
"""
1602-
2D Graphical display and animation
1597+
# def plot2(
1598+
# self, q=None, block=True, dt=50, limits=None,
1599+
# vellipse=False, fellipse=False,
1600+
# eeframe=True, name=False):
1601+
# """
1602+
# 2D Graphical display and animation
16031603

1604-
:param block: Block operation of the code and keep the figure open
1605-
:type block: bool
1606-
:param q: The joint configuration of the robot (Optional,
1607-
if not supplied will use the stored q values).
1608-
:type q: float ndarray(n)
1609-
:param dt: if q is a trajectory, this describes the delay in
1610-
milliseconds between frames
1611-
:type dt: int
1612-
:param limits: Custom view limits for the plot. If not supplied will
1613-
autoscale, [x1, x2, y1, y2, z1, z2]
1614-
:type limits: ndarray(6)
1615-
:param vellipse: (Plot Option) Plot the velocity ellipse at the
1616-
end-effector
1617-
:type vellipse: bool
1618-
:param vellipse: (Plot Option) Plot the force ellipse at the
1619-
end-effector
1620-
:type vellipse: bool
1621-
:param eeframe: (Plot Option) Plot the end-effector coordinate frame
1622-
at the location of the end-effector. Uses three arrows, red,
1623-
green and blue to indicate the x, y, and z-axes.
1624-
:type eeframe: bool
1625-
:param name: (Plot Option) Plot the name of the robot near its base
1626-
:type name: bool
1604+
# :param block: Block operation of the code and keep the figure open
1605+
# :type block: bool
1606+
# :param q: The joint configuration of the robot (Optional,
1607+
# if not supplied will use the stored q values).
1608+
# :type q: float ndarray(n)
1609+
# :param dt: if q is a trajectory, this describes the delay in
1610+
# milliseconds between frames
1611+
# :type dt: int
1612+
# :param limits: Custom view limits for the plot. If not supplied will
1613+
# autoscale, [x1, x2, y1, y2, z1, z2]
1614+
# :type limits: ndarray(6)
1615+
# :param vellipse: (Plot Option) Plot the velocity ellipse at the
1616+
# end-effector
1617+
# :type vellipse: bool
1618+
# :param vellipse: (Plot Option) Plot the force ellipse at the
1619+
# end-effector
1620+
# :type vellipse: bool
1621+
# :param eeframe: (Plot Option) Plot the end-effector coordinate frame
1622+
# at the location of the end-effector. Uses three arrows, red,
1623+
# green and blue to indicate the x, y, and z-axes.
1624+
# :type eeframe: bool
1625+
# :param name: (Plot Option) Plot the name of the robot near its base
1626+
# :type name: bool
16271627

1628-
:return: A reference to the PyPlot object which controls the
1629-
matplotlib figure
1630-
:rtype: PyPlot
1628+
# :return: A reference to the PyPlot object which controls the
1629+
# matplotlib figure
1630+
# :rtype: PyPlot
16311631

1632-
- ``robot.plot2(q)`` displays a 2D graphical view of a robot based on the
1633-
kinematic model and the joint configuration ``q``. This is a stick
1634-
figure polyline which joins the origins of the link coordinate frames.
1635-
The plot will autoscale with an aspect ratio of 1.
1632+
# - ``robot.plot2(q)`` displays a 2D graphical view of a robot based on the
1633+
# kinematic model and the joint configuration ``q``. This is a stick
1634+
# figure polyline which joins the origins of the link coordinate frames.
1635+
# The plot will autoscale with an aspect ratio of 1.
16361636

1637-
- ``robot.plot2()`` as above but use the stored ``q`` value.
1637+
# - ``robot.plot2()`` as above but use the stored ``q`` value.
16381638

1639-
If ``q`` (m,n) representing a joint-space trajectory it will create an
1640-
animation with a pause of ``dt`` seconds between each frame.
1639+
# If ``q`` (m,n) representing a joint-space trajectory it will create an
1640+
# animation with a pause of ``dt`` seconds between each frame.
16411641

1642-
.. note::
1643-
- By default this method will block until the figure is dismissed.
1644-
To avoid this set ``block=False``.
1645-
- The polyline joins the origins of the link frames, but for
1646-
some Denavit-Hartenberg models those frames may not actually
1647-
be on the robot, ie. the lines to not neccessarily represent
1648-
the links of the robot.
1642+
# .. note::
1643+
# - By default this method will block until the figure is dismissed.
1644+
# To avoid this set ``block=False``.
1645+
# - The polyline joins the origins of the link frames, but for
1646+
# some Denavit-Hartenberg models those frames may not actually
1647+
# be on the robot, ie. the lines to not neccessarily represent
1648+
# the links of the robot.
16491649

1650-
:seealso: :func:`teach2`
1650+
# :seealso: :func:`teach2`
16511651

1652-
"""
1652+
# """
16531653

1654-
# try:
1655-
return _plot2(
1656-
self, block, q, dt, limits,
1657-
vellipse=vellipse, fellipse=fellipse,
1658-
eeframe=eeframe, name=name)
1659-
# except ModuleNotFoundError:
1660-
# print(
1661-
# 'Could not find matplotlib.'
1662-
# ' Matplotlib required for this function')
1654+
# # try:
1655+
# return _plot2(
1656+
# self, block, q, dt, limits,
1657+
# vellipse=vellipse, fellipse=fellipse,
1658+
# eeframe=eeframe, name=name)
1659+
# # except ModuleNotFoundError:
1660+
# # print(
1661+
# # 'Could not find matplotlib.'
1662+
# # ' Matplotlib required for this function')
16631663

16641664
def teach2(
16651665
self, q=None, block=True, limits=None,

‎roboticstoolbox/robot/Robot.py

Lines changed: 164 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
from roboticstoolbox.tools.null import null
1717
from ansitable import ANSITable, Column
1818

19-
from roboticstoolbox.backends.PyPlot import PyPlot
19+
from roboticstoolbox.backends.PyPlot import PyPlot, PyPlot2
2020
from roboticstoolbox.backends.Swift import Swift
2121

2222
try:
@@ -1227,14 +1227,79 @@ def nofriction(self, coulomb=True, viscous=False):
12271227
# --------------------------------------------------------------------- #
12281228

12291229
def plot(
1230-
self, backend='Swift', q=None, block=True, dt=0.050,
1230+
self, q, backend='Swift', block=True, dt=0.050,
12311231
limits=None, vellipse=False, fellipse=False,
12321232
jointaxes=True, eeframe=True, shadow=True, name=True, movie=None
12331233
):
1234+
"""
1235+
Graphical display and animation
1236+
1237+
:param q: The joint configuration of the robot.
1238+
:type q: float ndarray(n)
1239+
:param backend: The graphical backend to use, currently 'swift'
1240+
and 'pyplot' are implemented. Defaults to 'swift'
1241+
:type backend: string
1242+
:param block: Block operation of the code and keep the figure open
1243+
:type block: bool
1244+
:param dt: if q is a trajectory, this describes the delay in
1245+
seconds between frames
1246+
:type dt: float
1247+
:param limits: Custom view limits for the plot. If not supplied will
1248+
autoscale, [x1, x2, y1, y2, z1, z2]
1249+
(this option is for 'pyplot' only)
1250+
:type limits: ndarray(6)
1251+
:param vellipse: (Plot Option) Plot the velocity ellipse at the
1252+
end-effector (this option is for 'pyplot' only)
1253+
:type vellipse: bool
1254+
:param vellipse: (Plot Option) Plot the force ellipse at the
1255+
end-effector (this option is for 'pyplot' only)
1256+
:type vellipse: bool
1257+
:param jointaxes: (Plot Option) Plot an arrow indicating the axes in
1258+
which the joint revolves around(revolute joint) or translates
1259+
along (prosmatic joint) (this option is for 'pyplot' only)
1260+
:type jointaxes: bool
1261+
:param eeframe: (Plot Option) Plot the end-effector coordinate frame
1262+
at the location of the end-effector. Uses three arrows, red,
1263+
green and blue to indicate the x, y, and z-axes.
1264+
(this option is for 'pyplot' only)
1265+
:type eeframe: bool
1266+
:param shadow: (Plot Option) Plot a shadow of the robot in the x-y
1267+
plane. (this option is for 'pyplot' only)
1268+
:type shadow: bool
1269+
:param name: (Plot Option) Plot the name of the robot near its base
1270+
(this option is for 'pyplot' only)
1271+
:type name: bool
1272+
:param movie: name of file in which to save an animated GIF
1273+
(this option is for 'pyplot' only)
1274+
:type movie: str
1275+
1276+
:return: A reference to the environment object which controls the
1277+
figure
1278+
:rtype: Swift or PyPlot
1279+
1280+
- ``robot.plot(q, 'pyplot')`` displays a graphical view of a robot
1281+
based on the kinematic model and the joint configuration ``q``.
1282+
This is a stick figure polyline which joins the origins of the
1283+
link coordinate frames. The plot will autoscale with an aspect
1284+
ratio of 1.
1285+
1286+
If ``q`` (m,n) representing a joint-space trajectory it will create an
1287+
animation with a pause of ``dt`` seconds between each frame.
1288+
1289+
.. note::
1290+
- By default this method will block until the figure is dismissed.
1291+
To avoid this set ``block=False``.
1292+
- For PyPlot, the polyline joins the origins of the link frames,
1293+
but for some Denavit-Hartenberg models those frames may not
1294+
actually be on the robot, ie. the lines to not neccessarily
1295+
represent the links of the robot.
1296+
1297+
:seealso: :func:`teach`
1298+
"""
12341299

12351300
if backend.lower() == 'swift':
12361301
if isinstance(self, rtb.ERobot):
1237-
self._plot_swift(q, block)
1302+
env = self._plot_swift(q=q, block=block)
12381303
elif isinstance(self, rtb.DHRobot):
12391304
raise NotImplementedError(
12401305
'Plotting in Swift is not implemented for DHRobots yet')
@@ -1244,14 +1309,17 @@ def plot(
12441309
raise NotImplementedError(
12451310
'Plotting in PyPlot is not implemented for ERobots yet')
12461311
elif isinstance(self, rtb.DHRobot):
1247-
self._plot_pyplot(
1248-
q, block, dt, limits, vellipse, fellipse,
1249-
jointaxes, eeframe, shadow, name, movie)
1312+
env = self._plot_pyplot(
1313+
q=q, block=block, dt=dt, limits=limits, vellipse=vellipse,
1314+
fellipse=fellipse, jointaxes=jointaxes, eeframe=eeframe,
1315+
shadow=shadow, name=name, movie=movie)
1316+
1317+
return env
12501318

12511319
def _plot_pyplot(
1252-
self, q, block, dt, limits=None,
1253-
vellipse=False, fellipse=False,
1254-
jointaxes=True, eeframe=True, shadow=True, name=True, movie=None):
1320+
self, q, block, dt, limits,
1321+
vellipse, fellipse,
1322+
jointaxes, eeframe, shadow, name, movie):
12551323

12561324
# Make an empty 3D figure
12571325
env = PyPlot()
@@ -1276,11 +1344,13 @@ def _plot_pyplot(
12761344
fell = self.fellipse(centre='ee')
12771345
env.add(fell)
12781346

1347+
# Stop lint error
1348+
images = [] # list of images saved from each plot
1349+
12791350
if movie is not None:
12801351
if not _pil_exists:
12811352
raise RuntimeError(
12821353
'to save movies PIL must be installed:\npip3 install PIL')
1283-
images = [] # list of images saved from each plot
12841354
# make the background white, looks better than grey stipple
12851355
env.ax.w_xaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
12861356
env.ax.w_yaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
@@ -1335,44 +1405,99 @@ def _plot_swift(self, q, block):
13351405

13361406
return env
13371407

1338-
# def _plot2(
1339-
# robot, block, q, dt, limits=None,
1340-
# vellipse=False, fellipse=False,
1341-
# eeframe=True, name=True):
1408+
# --------------------------------------------------------------------- #
13421409

1343-
# # Make an empty 2D figure
1344-
# env = rp.backends.PyPlot2()
1410+
def plot2(
1411+
self, q, block=True, dt=0.05, limits=None,
1412+
vellipse=False, fellipse=False,
1413+
eeframe=True, name=False):
1414+
"""
1415+
2D Graphical display and animation
1416+
1417+
:param block: Block operation of the code and keep the figure open
1418+
:type block: bool
1419+
:param q: The joint configuration of the robot (Optional,
1420+
if not supplied will use the stored q values).
1421+
:type q: float ndarray(n)
1422+
:param dt: if q is a trajectory, this describes the delay in
1423+
milliseconds between frames
1424+
:type dt: int
1425+
:param limits: Custom view limits for the plot. If not supplied will
1426+
autoscale, [x1, x2, y1, y2, z1, z2]
1427+
:type limits: ndarray(6)
1428+
:param vellipse: (Plot Option) Plot the velocity ellipse at the
1429+
end-effector
1430+
:type vellipse: bool
1431+
:param vellipse: (Plot Option) Plot the force ellipse at the
1432+
end-effector
1433+
:type vellipse: bool
1434+
:param eeframe: (Plot Option) Plot the end-effector coordinate frame
1435+
at the location of the end-effector. Uses three arrows, red,
1436+
green and blue to indicate the x, y, and z-axes.
1437+
:type eeframe: bool
1438+
:param name: (Plot Option) Plot the name of the robot near its base
1439+
:type name: bool
1440+
1441+
:return: A reference to the PyPlot object which controls the
1442+
matplotlib figure
1443+
:rtype: PyPlot
1444+
1445+
- ``robot.plot2(q)`` displays a 2D graphical view of a robot based on
1446+
the kinematic model and the joint configuration ``q``. This is a
1447+
stick figure polyline which joins the origins of the link coordinate
1448+
frames. The plot will autoscale with an aspect ratio of 1.
1449+
1450+
If ``q`` (m,n) representing a joint-space trajectory it will create an
1451+
animation with a pause of ``dt`` seconds between each frame.
13451452
1346-
# q = getmatrix(q, (None, robot.n))
1453+
.. note::
1454+
- By default this method will block until the figure is dismissed.
1455+
To avoid this set ``block=False``.
1456+
- The polyline joins the origins of the link frames, but for
1457+
some Denavit-Hartenberg models those frames may not actually
1458+
be on the robot, ie. the lines to not neccessarily represent
1459+
the links of the robot.
13471460
1348-
# # Add the robot to the figure in readonly mode
1349-
# if q.shape[0] == 1:
1350-
# env.launch(robot.name + ' Plot', limits)
1351-
# else:
1352-
# env.launch(robot.name + ' Trajectory Plot', limits)
1461+
:seealso: :func:`teach2`
13531462
1354-
# env.add(
1355-
# robot, readonly=True,
1356-
# eeframe=eeframe, name=name)
1463+
"""
13571464

1358-
# if vellipse:
1359-
# vell = robot.vellipse(centre='ee')
1360-
# env.add(vell)
1465+
if isinstance(self, rtb.ERobot):
1466+
raise NotImplementedError(
1467+
"2D Plotting of ERobot's not implementedyet")
13611468

1362-
# if fellipse:
1363-
# fell = robot.fellipse(centre='ee')
1364-
# env.add(fell)
1469+
# Make an empty 2D figure
1470+
env = PyPlot2()
13651471

1366-
# for qk in q:
1367-
# robot.q = qk
1368-
# env.step()
1369-
# time.sleep(dt/1000)
1472+
q = getmatrix(q, (None, self.n))
13701473

1371-
# # Keep the plot open
1372-
# if block: # pragma: no cover
1373-
# env.hold()
1474+
# Add the self to the figure in readonly mode
1475+
if q.shape[0] == 1:
1476+
env.launch(self.name + ' Plot', limits)
1477+
else:
1478+
env.launch(self.name + ' Trajectory Plot', limits)
13741479

1375-
# return env
1480+
env.add(
1481+
self, readonly=True,
1482+
eeframe=eeframe, name=name)
1483+
1484+
if vellipse:
1485+
vell = self.vellipse(centre='ee')
1486+
env.add(vell)
1487+
1488+
if fellipse:
1489+
fell = self.fellipse(centre='ee')
1490+
env.add(fell)
1491+
1492+
for qk in q:
1493+
self.q = qk
1494+
env.step()
1495+
1496+
# Keep the plot open
1497+
if block: # pragma: no cover
1498+
env.hold()
1499+
1500+
return env
13761501

13771502

13781503
# def _teach(

0 commit comments

Comments
 (0)
0