Skip to content

Commit a9c0fd1

Browse files
petercorkepetercorke
committed
* 'master' of https://github.com/petercorke/robotics-toolbox-python: pyplot changes Fixed error message URDFs working Noodle Plot for ETS robots Fix plot example fixed mmc ik MMC IK basic implementation
2 parents 1424cbf + db546cb commit a9c0fd1

File tree

8 files changed

+349
-200
lines changed

8 files changed

+349
-200
lines changed

‎examples/plot.py

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,17 @@
1313
panda.q = panda.qr
1414

1515
# Make 100 random sets of joint angles
16-
q = np.random.rand(7, 100)
16+
q = np.random.rand(100, 7)
1717

1818
# Plot the joint trajectory with a 50ms delay between configurations
19-
panda.plot(q=q, dt=50, vellipse=True, fellipse=True)
19+
# panda.plot(q=q, backend='pyplot', dt=0.050, vellipse=True, fellipse=True)
20+
panda.plot(q=q, backend='pyplot', dt=0.050, vellipse=False, fellipse=False)
21+
# # q = panda.qr
22+
23+
# env = rp.backends.PyPlot()
24+
# env.launch()
25+
26+
# env.add(panda)
27+
# print(panda)
28+
29+
# env.hold()

‎examples/swifty.py

Lines changed: 76 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
import roboticstoolbox as rtb
77
import spatialmath as sm
88
import numpy as np
9+
import numpy.testing as nt
910

1011
# Launch the simulator Swift
1112
# env = rtb.backends.Swift()
@@ -78,59 +79,83 @@
7879
r = rtb.models.ETS.Panda()
7980
r.q = r.qr
8081

81-
q1 = r.qr
82-
q2 = q1 + 0.1
83-
84-
85-
def derivative(f, a, method='central', h=0.01):
86-
'''Compute the difference formula for f'(a) with step size h.
87-
88-
Parameters
89-
----------
90-
f : function
91-
Vectorized function of one variable
92-
a : number
93-
Compute derivative at x = a
94-
method : string
95-
Difference formula: 'forward', 'backward' or 'central'
96-
h : number
97-
Step size in difference formula
98-
99-
Returns
100-
-------
101-
float
102-
Difference formula:
103-
central: f(a+h) - f(a-h))/2h
104-
forward: f(a+h) - f(a))/h
105-
backward: f(a) - f(a-h))/h
106-
'''
107-
if method == 'central':
108-
return (f(a + h) - f(a - h))/(2*h)
109-
elif method == 'forward':
110-
return (f(a + h) - f(a))/h
111-
elif method == 'backward':
112-
return (f(a) - f(a - h))/h
113-
else:
114-
raise ValueError("Method must be 'central', 'forward' or 'backward'.")
115-
116-
117-
# Numerical hessian wrt q[0]
118-
# d = derivative(lambda x: r.jacob0(np.r_[x, q1[1:]]), q1[0], h=0.1)
119-
# print(np.round(h1[:, :, 0], 3))
120-
# print(np.round(d, 3))
121-
122-
# Numerical third wrt q[0]
123-
d = derivative(lambda x: r.hessian0(np.r_[x, q1[1:]]), q1[0], h=0.01)
124-
print(np.round(d[3:, :, 0], 3))
125-
print(np.round(r.third(q1)[3:, :, 0, 0], 3))
126-
127-
l = r.deriv(q1, 3)
128-
print(np.round(l[3:, :, 0, 0], 3))
82+
q2 = [1, 2, -1, -2, 1, 1, 2]
83+
Tep = r.fkine(q2)
84+
Tep = sm.SE3(0.7, 0.2, 0.1) * sm.SE3.OA([0, 1, 0], [0, 0, -1])
12985

86+
import cProfile
87+
cProfile.run('qp = r.ikine_mmc(Tep)')
88+
89+
90+
91+
# print(r.fkine(q2))
92+
# print(r.fkine(qp))
93+
94+
95+
# q1 = r.qr
96+
# q2 = q1 + 0.1
97+
98+
99+
# def derivative(f, a, method='central', h=0.01):
100+
# '''Compute the difference formula for f'(a) with step size h.
101+
102+
# Parameters
103+
# ----------
104+
# f : function
105+
# Vectorized function of one variable
106+
# a : number
107+
# Compute derivative at x = a
108+
# method : string
109+
# Difference formula: 'forward', 'backward' or 'central'
110+
# h : number
111+
# Step size in difference formula
112+
113+
# Returns
114+
# -------
115+
# float
116+
# Difference formula:
117+
# central: f(a+h) - f(a-h))/2h
118+
# forward: f(a+h) - f(a))/h
119+
# backward: f(a) - f(a-h))/h
120+
# '''
121+
# if method == 'central':
122+
# return (f(a + h) - f(a - h))/(2*h)
123+
# elif method == 'forward':
124+
# return (f(a + h) - f(a))/h
125+
# elif method == 'backward':
126+
# return (f(a) - f(a - h))/h
127+
# else:
128+
# raise ValueError("Method must be 'central', 'forward' or 'backward'.")
129+
130+
131+
# # Numerical hessian wrt q[0]
132+
# # d = derivative(lambda x: r.jacob0(np.r_[x, q1[1:]]), q1[0], h=0.1)
133+
# # print(np.round(h1[:, :, 0], 3))
134+
# # print(np.round(d, 3))
135+
136+
# # Numerical third wrt q[0]
137+
# # d = derivative(lambda x: r.hessian0(np.r_[x, q1[1:]]), q1[0], h=0.01)
138+
# # print(np.round(d[:, :, 0], 3))
139+
# # print(np.round(r.third(q1)[:, :, 0, 0], 3))
140+
141+
# # l = r.partial_fkine0(q1, 3)
142+
# # print(np.round(l[:, :, 0, 0], 3))
143+
144+
145+
# # Numerical fourth wrt q[0]
146+
# # d = derivative(lambda x: r.third(np.r_[x, q1[1:]]), q1[0], h=0.01)
147+
# # print(np.round(d[:, :, 0, 0], 3))
148+
149+
# # l = r.partial_fkine0(q1, 4)
150+
# # print(np.round(l[:, :, 0, 0, 0], 3))
151+
152+
153+
154+
# j = r.jacob0(r.q)
130155
# def runner():
131-
# for i in range(10000):
132-
# r.jacob0(r.q)
133-
# # r.hessian0(r.q)
156+
# for i in range(1):
157+
# r.partial_fkine0(r.q, 7)
158+
# # r.hessian0(r.q, j)
134159

135160

136161
# import cProfile

‎roboticstoolbox/backends/PyPlot/PyPlot.py

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -180,7 +180,6 @@ def step(self, dt=0.05):
180180

181181
self._update_robots()
182182

183-
184183
def reset(self):
185184
"""
186185
Reset the graphical scene
@@ -399,7 +398,7 @@ def _add_teach_panel(self, robot):
399398

400399
if _isnotebook():
401400
raise RuntimeError('cannot use teach panel under Jupyter')
402-
401+
403402
fig = self.fig
404403

405404
# Add text to the plots
@@ -484,7 +483,6 @@ def update(val, text, robot): # pragma: no cover
484483
self.sjoint[i].on_changed(lambda x: update(x, text, robot))
485484

486485

487-
488486
def _isnotebook():
489487
"""
490488
Determine if code is being run from a Jupyter notebook
@@ -505,4 +503,4 @@ def _isnotebook():
505503
else:
506504
return False # Other type (?)
507505
except NameError:
508-
return False # Probably standard Python interpreter
506+
return False # Probably standard Python interpreter

‎roboticstoolbox/backends/PyPlot/RobotPlot.py

Lines changed: 68 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -52,14 +52,21 @@ def __init__(
5252

5353
def axes_calcs(self):
5454
# Joint and ee poses
55-
T = self.robot.fkine_all()
56-
Te = self.robot.fkine()
55+
T = self.robot.fkine_all(self.robot.q)
56+
57+
try:
58+
Te = self.robot.fkine(self.robot.q)
59+
except ValueError:
60+
print(
61+
"\nError: Branched robot's not yet supported "
62+
"with PyPlot backend\n")
63+
raise
64+
5765
Tb = self.robot.base
5866

5967
# Joint and ee position matrix
60-
loc = np.zeros([3, self.robot.n + 2])
68+
loc = np.zeros([3, len(self.robot.links) + 1])
6169
loc[:, 0] = Tb.t
62-
loc[:, self.robot.n + 1] = Te.t
6370

6471
# Joint axes position matrix
6572
joints = np.zeros((3, self.robot.n))
@@ -75,23 +82,35 @@ def axes_calcs(self):
7582
Tez = Te * Tjz
7683

7784
# Joint axes arrow calcs
78-
for i in range(self.robot.n):
79-
loc[:, i + 1] = T[i].t
85+
if isinstance(self.robot, rp.ERobot):
86+
i = 0
87+
j = 0
88+
for link in self.robot.links:
89+
loc[:, i + 1] = link._fk.t
8090

81-
if isinstance(self.robot, rp.DHRobot) \
82-
or self.robot.ets[self.robot.q_idx[i]].axis == 'Rz' \
83-
or self.robot.ets[self.robot.q_idx[i]].axis == 'tz':
84-
Tji = T[i] * Tjz
91+
if link.isjoint:
92+
if link.v.axis == 'Rz' or link.v.axis == 'tz':
93+
Tji = link._fk * Tjz
94+
95+
elif link.v.axis == 'Ry' or link.v.axis == 'ty':
96+
Tji = link._fk * Tjy
97+
98+
elif link.v.axis == 'Rx' or link.v.axis == 'tx':
99+
Tji = link._fk * Tjx
85100

86-
# elif self.robot.ets[self.robot.q_idx[i]].axis == 'Ry' \
87-
# or self.robot.ets[self.robot.q_idx[i]].axis == 'ty':
88-
# Tji = T[i] * Tjy
101+
joints[:, j] = Tji.t
102+
j += 1
89103

90-
# elif self.robot.ets[self.robot.q_idx[i]].axis == 'Rx' \
91-
# or self.robot.ets[self.robot.q_idx[i]].axis == 'tx':
92-
# Tji = T[i] * Tjx
104+
i += 1
105+
loc = np.c_[loc, loc[:, -1]]
106+
else:
107+
# End effector offset (tool of robot)
108+
loc = np.c_[loc, Te.t]
93109

94-
joints[:, i] = Tji.t
110+
for i in range(self.robot.n):
111+
loc[:, i + 1] = T[i].t
112+
Tji = T[i] * Tjz
113+
joints[:, i] = Tji.t
95114

96115
return loc, joints, [Tex, Tey, Tez]
97116

@@ -114,23 +133,32 @@ def draw(self):
114133
# Plot ee coordinate frame
115134
self.ee_axes[0] = \
116135
self._plot_quiver(
117-
loc[:, self.robot.n + 1], ee[0].t, '#EE9494', 2)
136+
loc[:, -1], ee[0].t, '#EE9494', 2)
118137
self.ee_axes[1] = \
119138
self._plot_quiver(
120-
loc[:, self.robot.n + 1], ee[1].t, '#93E7B0', 2)
139+
loc[:, -1], ee[1].t, '#93E7B0', 2)
121140
self.ee_axes[2] = \
122141
self._plot_quiver(
123-
loc[:, self.robot.n + 1], ee[2].t, '#54AEFF', 2)
142+
loc[:, -1], ee[2].t, '#54AEFF', 2)
124143

125144
# Remove oldjoint z coordinates
126145
if self.jointaxes:
127-
for i in range(self.robot.n):
128-
self.joints[i].remove()
146+
j = 0
147+
# for joint in self.joints:
148+
# joint.remove()
129149

130150
# Plot joint z coordinates
131-
for i in range(self.robot.n):
132-
self.joints[i] = \
133-
self._plot_quiver(loc[:, i+1], joints[:, i], '#8FC1E2', 2)
151+
for i in range(len(self.robot.links)):
152+
if isinstance(self.robot, rp.DHRobot) or \
153+
self.robot.links[i].isjoint:
154+
self.joints.append(
155+
self._plot_quiver(
156+
loc[:, i+1], joints[:, j], '#8FC1E2', 2))
157+
j += 1
158+
159+
# for i in range(len(self.robot.links)):
160+
# self.joints[i] = \
161+
# self._plot_quiver(loc[:, i+1], joints[:, i], '#8FC1E2', 2)
134162

135163
# Update the robot links
136164
self.links[0].set_xdata(loc[0, :])
@@ -161,10 +189,10 @@ def draw2(self):
161189
# Plot ee coordinate frame
162190
self.ee_axes[0] = \
163191
self._plot_quiver2(
164-
loc[:, self.robot.n + 1], ee[0].t, '#EE9494', 2)
192+
loc[:, -1], ee[0].t, '#EE9494', 2)
165193
self.ee_axes[1] = \
166194
self._plot_quiver2(
167-
loc[:, self.robot.n + 1], ee[1].t, '#93E7B0', 2)
195+
loc[:, -1], ee[1].t, '#93E7B0', 2)
168196

169197
# Update the robot links
170198
self.links[0].set_xdata(loc[0, :])
@@ -187,19 +215,24 @@ def init(self):
187215
if self.eeframe:
188216
self.ee_axes.append(
189217
self._plot_quiver(
190-
loc[:, self.robot.n + 1], ee[0].t, '#EE9494', 2))
218+
loc[:, -1], ee[0].t, '#EE9494', 2))
191219
self.ee_axes.append(
192220
self._plot_quiver(
193-
loc[:, self.robot.n + 1], ee[1].t, '#93E7B0', 2))
221+
loc[:, -1], ee[1].t, '#93E7B0', 2))
194222
self.ee_axes.append(
195223
self._plot_quiver(
196-
loc[:, self.robot.n + 1], ee[2].t, '#54AEFF', 2))
224+
loc[:, -1], ee[2].t, '#54AEFF', 2))
197225

198226
# Plot joint z coordinates
199227
if self.jointaxes:
200-
for i in range(self.robot.n):
201-
self.joints.append(
202-
self._plot_quiver(loc[:, i+1], joints[:, i], '#8FC1E2', 2))
228+
j = 0
229+
for i in range(len(self.robot.links)):
230+
if isinstance(self.robot, rp.DHRobot) or \
231+
self.robot.links[i].isjoint:
232+
self.joints.append(
233+
self._plot_quiver(
234+
loc[:, i+1], joints[:, j], '#8FC1E2', 2))
235+
j += 1
203236

204237
# Plot the shadow of the robot links, draw first so robot is always
205238
# in front
@@ -229,10 +262,10 @@ def init2(self):
229262
if self.eeframe:
230263
self.ee_axes.append(
231264
self._plot_quiver2(
232-
loc[:, self.robot.n + 1], ee[0].t, '#EE9494', 2))
265+
loc[:, -1], ee[0].t, '#EE9494', 2))
233266
self.ee_axes.append(
234267
self._plot_quiver2(
235-
loc[:, self.robot.n + 1], ee[1].t, '#93E7B0', 2))
268+
loc[:, -1], ee[1].t, '#93E7B0', 2))
236269

237270
# Plot the robot links
238271
self.links = self.ax.plot(

‎roboticstoolbox/robot/ELink.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -245,14 +245,14 @@ def jindex(self):
245245
def jindex(self, j):
246246
self._jindex = j
247247

248-
def isrevolute(self):
249-
"""
250-
Checks if the joint is of revolute type
251-
252-
:return: Ture if is revolute
253-
:rtype: bool
254-
"""
255-
return self.v.isrevolute
248+
# def isrevolute(self):
249+
# """
250+
# Checks if the joint is of revolute type
251+
252+
# :return: Ture if is revolute
253+
# :rtype: bool
254+
# """
255+
# return self.v.isrevolute
256256

257257
@property
258258
def isprismatic(self):

0 commit comments

Comments
 (0)