Skip to content

Commit a2775d5

Browse files
petercorkepetercorke
committed
working 2D PyPlot version for revolute, prismatic and branched robots
1 parent 227fbf1 commit a2775d5

File tree

2 files changed

+181
-31
lines changed

2 files changed

+181
-31
lines changed

‎roboticstoolbox/backends/PyPlot/PyPlot2.py

Lines changed: 54 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -74,12 +74,12 @@ def launch(self, name=None, limits=None):
7474
self.ax.set_ylabel(labels[1])
7575

7676
self.ax.autoscale(enable=True, axis='both', tight=False)
77-
self.ax.axis('equal')
7877

7978
if limits is not None:
8079
self.ax.set_xlim([limits[0], limits[1]])
8180
self.ax.set_ylim([limits[2], limits[3]])
8281

82+
self.ax.axis('equal')
8383

8484
plt.ion()
8585
plt.show()
@@ -159,12 +159,12 @@ def add(
159159

160160
super().add()
161161

162-
if isinstance(ob, rp.DHRobot) or isinstance(ob, rp.ERobot):
162+
if isinstance(ob, rp.ERobot2):
163163
self.robots.append(
164164
RobotPlot2(
165165
ob, self.ax, readonly, display,
166166
eeframe, name))
167-
self.robots[len(self.robots) - 1].draw2()
167+
self.robots[len(self.robots) - 1].draw()
168168

169169

170170
elif isinstance(ob, EllipsePlot):
@@ -217,7 +217,7 @@ def _update_robots(self):
217217
def _draw_robots(self):
218218

219219
for i in range(len(self.robots)):
220-
self.robots[i].draw2()
220+
self.robots[i].draw()
221221

222222
def _draw_ellipses(self):
223223

@@ -227,24 +227,36 @@ def _draw_ellipses(self):
227227
# def _plot_handler(self, sig, frame):
228228
# plt.pause(0.001)
229229

230-
def _add_teach_panel(self, robot):
230+
def _add_teach_panel(self, robot, q):
231+
"""
232+
Add a teach panel
233+
234+
:param robot: Robot being taught
235+
:type robot: ERobot class
236+
:param q: inital joint angles in radians
237+
:type q: array_like(n)
238+
"""
231239
fig = self.fig
232240

233241
# Add text to the plots
234-
def text_trans(text): # pragma: no cover
235-
T = robot.fkine()
242+
def text_trans(text, q): # pragma: no cover
243+
# update displayed robot pose value
244+
T = robot.fkine(q, end=robot.ee_links[0])
236245
t = np.round(T.t, 3)
237-
r = np.round(T.rpy(), 3)
246+
r = np.round(T.theta(), 3)
238247
text[0].set_text("x: {0}".format(t[0]))
239248
text[1].set_text("y: {0}".format(t[1]))
240-
text[2].set_text("yaw: {0}".format(r[2]))
249+
text[2].set_text("yaw: {0}".format(r))
241250

242251
# Update the self state in mpl and the text
243252
def update(val, text, robot): # pragma: no cover
244-
for i in range(robot.n):
245-
robot.q[i] = self.sjoint[i].val * np.pi/180
253+
for j in range(robot.n):
254+
if robot.isrevolute(j):
255+
robot.q[j] = self.sjoint[j].val * np.pi / 180
256+
else:
257+
robot.q[j] = self.sjoint[j].val
246258

247-
text_trans(text)
259+
text_trans(text, robot.q)
248260

249261
# Step the environment
250262
self.step(0)
@@ -260,19 +272,23 @@ def update(val, text, robot): # pragma: no cover
260272
self.axjoint = []
261273
self.sjoint = []
262274

263-
qlim = np.copy(robot.qlim) * 180/np.pi
264-
265-
if np.all(qlim == 0): # pragma nocover
266-
qlim[0, :] = -180
267-
qlim[1, :] = 180
275+
qlim = robot.todegrees(robot.qlim)
268276

269277
# Set the pose text
270-
T = robot.fkine()
278+
# if multiple EE, display only the first one
279+
T = robot.fkine(q, end=robot.ee_links[0])
271280
t = np.round(T.t, 3)
272-
r = np.round(T.rpy(), 3)
281+
r = np.round(T.theta(), 3)
273282

283+
# TODO maybe put EE name in here, possible issue with DH robot
284+
# TODO maybe display pose of all EEs, layout hassles though
285+
286+
if robot.nbranches == 0:
287+
header = "End-effector Pose"
288+
else:
289+
header = "End-effector #0 Pose"
274290
fig.text(
275-
0.02, 1 - ym + 0.25, "End-effector Pose",
291+
0.02, 1 - ym + 0.25, header,
276292
fontsize=9, weight="bold", color="#4f4f4f")
277293
text.append(fig.text(
278294
0.03, 1 - ym + 0.20, "x: {0}".format(t[0]),
@@ -281,20 +297,30 @@ def update(val, text, robot): # pragma: no cover
281297
0.03, 1 - ym + 0.16, "y: {0}".format(t[1]),
282298
fontsize=9, color="#2b2b2b"))
283299
text.append(fig.text(
284-
0.15, 1 - ym + 0.20, "yaw: {0}".format(r[0]),
300+
0.15, 1 - ym + 0.20, "yaw: {0}".format(r),
285301
fontsize=9, color="#2b2b2b"))
286302
fig.text(
287303
0.02, 1 - ym + 0.06, "Joint angles",
288304
fontsize=9, weight="bold", color="#4f4f4f")
289305

290-
for i in range(robot.n):
291-
ymin = (1 - ym) - i * yh
306+
for j in range(robot.n):
307+
# for each joint
308+
ymin = (1 - ym) - j * yh
292309
self.axjoint.append(
293310
fig.add_axes([x1, ymin, x2, 0.03], facecolor='#dbdbdb'))
294311

295-
self.sjoint.append(
296-
Slider(
297-
self.axjoint[i], 'q' + str(i),
298-
qlim[0, i], qlim[1, i], robot.q[i] * 180/np.pi, "% .1f°"))
312+
if robot.isrevolute(j):
313+
slider = Slider(
314+
self.axjoint[j], 'q' + str(j),
315+
qlim[0, j], qlim[1, j], q[j] * 180/np.pi, "% .1f°")
316+
else:
317+
slider = Slider(
318+
self.axjoint[j], 'q' + str(j),
319+
qlim[0, j], qlim[1, j], q[j], "% .1f")
299320

300-
self.sjoint[i].on_changed(lambda x: update(x, text, robot))
321+
slider.on_changed(lambda x: update(x, text, robot))
322+
self.sjoint.append(slider)
323+
324+
325+
robot.q = q
326+
self.step()

‎roboticstoolbox/backends/PyPlot/RobotPlot2.py

Lines changed: 127 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,15 +5,139 @@
55

66
# import numpy as np
77
from roboticstoolbox.backends.PyPlot.RobotPlot import RobotPlot
8-
9-
8+
import numpy as np
9+
from spatialmath import SE2
1010
class RobotPlot2(RobotPlot):
1111

1212
def __init__(
1313
self, robot, ax, readonly, display=True,
1414
eeframe=True, name=True):
1515

16-
super(RobotPlot2, self).__init__(
16+
super().__init__(
1717
robot, ax, readonly, display=display,
1818
jointaxes=False, shadow=False, eeframe=eeframe, name=name
1919
)
20+
21+
def axes_calcs(self):
22+
# Joint and ee poses
23+
T = self.robot.fkine_path(self.robot.q)
24+
25+
# Joint and ee position matrix
26+
# one column per frame
27+
28+
loc = T.t.T
29+
30+
# Joint axes position matrix
31+
joints = np.zeros((3, self.robot.n))
32+
33+
# Axes arrow transforms
34+
35+
36+
# ee axes arrows
37+
Tex = T[-1] * Tjx
38+
Tey = T[-1] * Tjy
39+
40+
return loc, joints, [Tex, Tey]
41+
42+
def draw(self):
43+
if not self.display:
44+
return
45+
46+
if not self.drawn:
47+
self.init()
48+
return
49+
50+
# loc, joints, ee = self.axes_calcs()
51+
52+
53+
# Update the robot links
54+
55+
# compute all link frames
56+
T = self.robot.fkine_path(self.robot.q)
57+
58+
# draw all the line segments for the noodle plog
59+
for i, segment in enumerate(self.segments):
60+
linkframes = []
61+
for link in segment:
62+
if link is None:
63+
linkframes.append(self.robot.base)
64+
else:
65+
linkframes.append(T[link.number + 1])
66+
points = np.array([linkframe.t for linkframe in linkframes])
67+
68+
self.links[i].set_xdata(points[:,0])
69+
self.links[i].set_ydata(points[:,1])
70+
71+
# draw the end-effectors
72+
# Remove old ee coordinate frame
73+
if self.eeframes:
74+
for quiver in self.eeframes:
75+
quiver.remove()
76+
77+
self.eeframes = []
78+
79+
Tjx = SE2(0.06, 0)
80+
Tjy = SE2(0, 0.06)
81+
red = '#F84752' # '#EE9494'
82+
green = '#BADA55' # '#93E7B0'
83+
84+
# Plot ee coordinate frame
85+
for link in self.robot.ee_links:
86+
Te = T[link.number + 1]
87+
88+
# ee axes arrows
89+
Tex = Te * Tjx
90+
Tey = Te * Tjy
91+
92+
xaxis = self._plot_quiver(Te.t, Tex.t, red, 2)
93+
yaxis = self._plot_quiver(Te.t, Tey.t, green, 2)
94+
95+
self.eeframes.extend([xaxis, yaxis])
96+
97+
98+
99+
def init(self):
100+
101+
self.drawn = True
102+
103+
self.segments = self.robot.segments()
104+
105+
# Joint and ee poses
106+
Tb = self.robot.base
107+
# loc, joints, ee = self.axes_calcs()
108+
109+
# Plot robot name
110+
if self.showname:
111+
self.name = self.ax.text(
112+
Tb.t[0] + 0.05, Tb.t[1], self.robot.name)
113+
114+
# # Plot ee coordinate frame
115+
# if self.eeframe:
116+
# self.ee_axes.append(
117+
# self._plot_quiver(
118+
# loc[:, -1], ee[0].t, '#EE9494', 2))
119+
# self.ee_axes.append(
120+
# self._plot_quiver(
121+
# loc[:, -1], ee[1].t, '#93E7B0', 2))
122+
123+
# Initialize the robot links
124+
self.links = []
125+
for i in range(len(self.segments)):
126+
line, = self.ax.plot(
127+
# loc[0, :], loc[1, :], linewidth=5, color='#E16F6D')
128+
0, 0, linewidth=5, color='#778899')
129+
self.links.append(line)
130+
131+
self.eeframes = []
132+
133+
def _plot_quiver(self, p0, p1, col, width):
134+
# draw arrow from p0 (tail) to p1 (head)
135+
qv = self.ax.quiver(
136+
p0[0], p0[1],
137+
p1[0] - p0[0],
138+
p1[1] - p0[1],
139+
linewidth=width,
140+
color=col
141+
)
142+
143+
return qv

0 commit comments

Comments
 (0)