Skip to content

Commit 4168ef3

Browse files
petercorkepetercorke
committed
2 parents 340bb00 + 54337a8 commit 4168ef3

File tree

20 files changed

+670
-61
lines changed

20 files changed

+670
-61
lines changed

‎examples/baur.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -32,17 +32,17 @@
3232
n = 7
3333

3434
# Set the desired end-effector pose
35-
Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
35+
Tep = panda.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3)
3636

3737
arrived = False
3838

3939
while not arrived:
4040

4141
# The pose of the Panda's end-effector
42-
Te = panda.fkine()
42+
Te = panda.fkine(panda.q)
4343

4444
# The manipulator Jacobian in the end-effecotr frame
45-
Je = panda.jacobe()
45+
Je = panda.jacobe(panda.q)
4646

4747
# Calulate the required end-effector spatial velocity for the robot
4848
# to approach the goal. Gain is set to 1.0
@@ -52,7 +52,7 @@
5252
Y = 0.01
5353

5454
# The manipulability Jacobian
55-
Jm = panda.jacobm()
55+
Jm = panda.jacobm(panda.q)
5656

5757
# The minimum angle (in radians) in which the joint is allowed to approach
5858
# to its limit

‎examples/mmc.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -31,14 +31,14 @@
3131
n = 7
3232

3333
# Set the desired end-effector pose
34-
Tep = panda.fkine() * sm.SE3(0.3, 0.2, 0.3)
34+
Tep = panda.fkine(panda.q) * sm.SE3(0.3, 0.2, 0.3)
3535

3636
arrived = False
3737

3838
while not arrived:
3939

4040
# The pose of the Panda's end-effector
41-
Te = panda.fkine()
41+
Te = panda.fkine(panda.q)
4242

4343
# Transform from the end-effector to desired pose
4444
eTep = Te.inv() * Tep
@@ -63,7 +63,7 @@
6363
Q[n:, n:] = (1 / e) * np.eye(6)
6464

6565
# The equality contraints
66-
Aeq = np.c_[panda.jacobe(), np.eye(6)]
66+
Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
6767
beq = v.reshape((6,))
6868

6969
# The inequality constraints for joint limit avoidance

‎examples/neo.py

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@
4949
env.add(target)
5050

5151
# Set the desired end-effector pose to the location of target
52-
Tep = panda.fkine()
52+
Tep = panda.fkine(panda.q)
5353
Tep.A[:3, 3] = target.base.t
5454
Tep.A[2, 3] += 0.1
5555

@@ -58,7 +58,7 @@
5858
while not arrived:
5959

6060
# The pose of the Panda's end-effector
61-
Te = panda.fkine()
61+
Te = panda.fkine(panda.q)
6262

6363
# Transform from the end-effector to desired pose
6464
eTep = Te.inv() * Tep
@@ -83,7 +83,7 @@
8383
Q[n:, n:] = (1 / e) * np.eye(6)
8484

8585
# The equality contraints
86-
Aeq = np.c_[panda.jacobe(), np.eye(6)]
86+
Aeq = np.c_[panda.jacobe(panda.q), np.eye(6)]
8787
beq = v.reshape((6,))
8888

8989
# The inequality constraints for joint limit avoidance
@@ -108,7 +108,8 @@
108108
# object on the robot to the collision in the scene
109109
c_Ain, c_bin = panda.link_collision_damper(
110110
collision, panda.q[:n], 0.3, 0.05, 1.0,
111-
panda.link_dict['panda_link1'], panda.link_dict['panda_hand'])
111+
startlink=panda.link_dict['panda_link1'],
112+
endlink=panda.link_dict['panda_hand'])
112113

113114
# If there are any parts of the robot within the influence distance
114115
# to the collision in the scene
@@ -120,7 +121,7 @@
120121
bin = np.r_[bin, c_bin]
121122

122123
# Linear component of objective function: the manipulability Jacobian
123-
c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)]
124+
c = np.r_[-panda.jacobm(panda.q).reshape((n,)), np.zeros(6)]
124125

125126
# The lower and upper bounds on the joint velocity and slack variable
126127
lb = -np.r_[panda.qdlim[:n], 10 * np.ones(6)]

‎examples/ros.py

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
#!/usr/bin/env python
2+
"""
3+
@author Jesse Haviland
4+
"""
5+
6+
import roboticstoolbox as rtb
7+
import spatialmath as sm
8+
import numpy as np
9+
import os
10+
11+
# Launch the simulator Swift
12+
env = rtb.backends.ROS()
13+
env.launch(ros_master_uri='http://localhost:11311')
14+
15+
os.system('rostopic list')
16+
17+
# # Create a Panda robot object
18+
# panda = rtb.models.Panda()
19+
20+
# # Set joint angles to ready configuration
21+
# panda.q = panda.qr
22+
23+
# # Add the Panda to the simulator
24+
# env.add(panda)
25+
26+

‎examples/swifty.py

Lines changed: 15 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -209,11 +209,16 @@ def rand_v():
209209

210210
x = []
211211
y = []
212+
rands = 100
213+
v = np.zeros((rands, 6))
214+
215+
for i in range(rands):
216+
v[i, :] = rand_v()
212217

213218
for i in range(10000000):
214219

215220
q = rand_q()
216-
v = rand_v()
221+
# v = rand_v()
217222

218223
J = r.jacob0(q)
219224
Jt = J[:3, :]
@@ -222,15 +227,16 @@ def rand_v():
222227

223228
q_n = [10000, 0]
224229
q_m = [10000, 0]
230+
q_mn = 0
225231

226232
# cond = np.linalg.cond(J[:3, :])
227233
m = r.manipulability(J=J, axes='trans')
228234
# infn = np.linalg.norm(Jt, 2)
229235
psi = (np.cbrt(np.linalg.det(Jt @ np.transpose(Jt)))) / \
230236
(np.trace(Jt @ np.transpose(Jt)) / 3)
231237

232-
for j in range(1000):
233-
qd = np.linalg.pinv(J) @ v
238+
for j in range(rands):
239+
qd = np.linalg.pinv(J) @ v[j, :]
234240

235241
if np.max(qd) > q_m[1]:
236242
q_m[1] = np.max(qd)
@@ -243,12 +249,16 @@ def rand_v():
243249
elif np.linalg.norm(qd) < q_n[0]:
244250
q_n[0] = np.linalg.norm(qd)
245251

252+
q_mn += np.linalg.norm(qd)
253+
254+
q_mn /= rands
255+
246256
# # ax.plot(m, np.log10(cond), 'o', color='black')
247257
# ax.plot(m, infn, 'o', color='black')
248258
# # ax.plot(1, 0.002, 'o', color='black')
249259

250-
x.append(psi)
251-
y.append(np.log10(q_m[1]))
260+
x.append(m)
261+
y.append(np.log10(q_n[0]))
252262
# y.append(psi)
253263

254264
# ax.plot(m, np.log10(q_m[1]), 'o', color='black')

‎examples/test.py

Lines changed: 11 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -14,27 +14,22 @@
1414
env.launch()
1515

1616
# Create a Panda robot object
17-
puma = rtb.models.Puma560()
17+
r = rtb.models.Frankie()
1818

1919
# Set joint angles to ready configuration
20-
puma.q = puma.qr
20+
r.q = r.qr
2121

2222
# Add the puma to the simulator
23-
env.add(puma)
23+
env.add(r)
24+
time.sleep(1)
2425

25-
# Tep = puma.fkine() * sm.SE3.Tz(0.1)
26+
Tep = r.fkine(r.q) * sm.SE3.Tx(1.0) * sm.SE3.Ty(1.0)
2627

27-
# arrived = False
28+
arrived = False
2829

29-
# dt = 0.05
30+
dt = 0.05
3031

31-
# while not arrived:
32-
33-
# start = time.time()
34-
# v, arrived = rtb.p_servo(puma.fkine(), Tep, 0.1)
35-
# puma.qd = np.linalg.pinv(puma.jacobe()) @ v
36-
# env.step(50)
37-
# stop = time.time()
38-
39-
# if stop - start < dt:
40-
# time.sleep(dt - (stop - start))
32+
while not arrived:
33+
v, arrived = rtb.p_servo(r.fkine(r.q), Tep, 0.1)
34+
r.qd = np.linalg.pinv(r.jacobe(r.q)) @ v
35+
env.step(dt)

‎roboticstoolbox/backends/PyPlot/PyPlot.py

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
try:
1818
import matplotlib
1919
import matplotlib.pyplot as plt
20+
from mpl_toolkits.mplot3d import Axes3D
2021
from matplotlib.widgets import Slider
2122
matplotlib.rcParams['pdf.fonttype'] = 42
2223
matplotlib.rcParams['ps.fonttype'] = 42
@@ -124,7 +125,7 @@ def launch(self, name=None, limits=None):
124125
plt.ion()
125126
plt.show()
126127

127-
self.t = 0
128+
self.sim_time = 0
128129

129130
# # Set the signal handler and a 0.1 second plot updater
130131
# signal.signal(signal.SIGALRM, self._plot_handler)
@@ -164,9 +165,9 @@ def step(self, dt=0.05):
164165
self._set_axes_equal()
165166

166167
# update time and display it on plot
167-
if self.t > 0:
168-
self.timer.set_text(f"t = {self.t:.2f}")
169-
self.t += dt
168+
if self.sim_time > 0:
169+
self.timer.set_text(f"t = {self.sim_time:.2f}")
170+
self.sim_time += dt
170171

171172
# plt.ion()
172173

‎roboticstoolbox/backends/PyPlot/RobotPlot.py

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -144,8 +144,12 @@ def draw(self):
144144
# Remove oldjoint z coordinates
145145
if self.jointaxes:
146146
j = 0
147-
# for joint in self.joints:
148-
# joint.remove()
147+
148+
for joint in self.joints:
149+
self.ax.collections.remove(joint)
150+
151+
del self.joints
152+
self.joints = []
149153

150154
# Plot joint z coordinates
151155
for i in range(len(self.robot.links)):

0 commit comments

Comments
 (0)