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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy