Skip to content

Commit d9ec931

Browse files
jhavljhavl
committed
pyplot changes
1 parent a289985 commit d9ec931

File tree

5 files changed

+110
-66
lines changed

5 files changed

+110
-66
lines changed

‎examples/plot.py

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -16,4 +16,14 @@
1616
q = np.random.rand(100, 7)
1717

1818
# Plot the joint trajectory with a 50ms delay between configurations
19-
panda.plot(q=q, dt=0.050, 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/RobotPlot.py

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -143,13 +143,22 @@ def draw(self):
143143

144144
# Remove oldjoint z coordinates
145145
if self.jointaxes:
146-
for i in range(len(self.robot.links)):
147-
self.joints[i].remove()
146+
j = 0
147+
# for joint in self.joints:
148+
# joint.remove()
148149

149150
# Plot joint z coordinates
150151
for i in range(len(self.robot.links)):
151-
self.joints[i] = \
152-
self._plot_quiver(loc[:, i+1], joints[:, i], '#8FC1E2', 2)
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)
153162

154163
# Update the robot links
155164
self.links[0].set_xdata(loc[0, :])

‎roboticstoolbox/robot/IK.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -304,7 +304,7 @@ def ikine_LM(
304304
e = base.tr2delta(self.fkine(q).A, Tk.A)
305305

306306
# Are we there yet?
307-
if np.linalg.norm(W @ e) < tol:
307+
if base.norm(W @ e) < tol:
308308
break
309309

310310
# Compute the Jacobian
@@ -492,7 +492,7 @@ def ikine_LMS(
492492
tcount = 0 # Total iteration count
493493

494494
# bool vector indicating revolute joints
495-
revolutes = np.array([link.isrevolute() for link in self])
495+
revolutes = np.array([link.isrevolute for link in self])
496496

497497
q = q0
498498
for Tk in T:

‎roboticstoolbox/robot/Robot.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -800,14 +800,14 @@ def plot(
800800
'Plotting in Swift is not implemented for DHRobots yet')
801801

802802
elif backend.lower() == 'pyplot':
803-
if isinstance(self, rtb.ERobot): # pragma nocover
804-
raise NotImplementedError(
805-
'Plotting in PyPlot is not implemented for ERobots yet')
806-
elif isinstance(self, rtb.DHRobot):
807-
env = self._plot_pyplot(
808-
q=q, block=block, dt=dt, limits=limits, vellipse=vellipse,
809-
fellipse=fellipse, jointaxes=jointaxes, eeframe=eeframe,
810-
shadow=shadow, name=name, movie=movie)
803+
# if isinstance(self, rtb.ERobot): # pragma nocover
804+
# raise NotImplementedError(
805+
# 'Plotting in PyPlot is not implemented for ERobots yet')
806+
# elif isinstance(self, rtb.DHRobot):
807+
env = self._plot_pyplot(
808+
q=q, block=block, dt=dt, limits=limits, vellipse=vellipse,
809+
fellipse=fellipse, jointaxes=jointaxes, eeframe=eeframe,
810+
shadow=shadow, name=name, movie=movie)
811811

812812
return env
813813

0 commit comments

Comments
 (0)