Skip to content

Commit 7958f17

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents 27ded2c + 7cbb6a1 commit 7958f17

File tree

23 files changed

+1018
-143
lines changed

23 files changed

+1018
-143
lines changed

‎README.md

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -110,8 +110,8 @@ orientation parallel to y-axis (O=+Y)).
110110
from spatialmath import SE3
111111

112112
T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
113-
sol = robot.ikine_unc(T) # solve IK
114-
print(sol.q) # display joint angles
113+
sol = robot.ikine_min(T) # solve IK
114+
print(sol.q) # display joint angles
115115

116116
[-0.01044 7.876 1.557 -6.81 1.571 4.686 0.5169]
117117

‎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/rtb.py

Lines changed: 34 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,13 +31,23 @@
3131
help='specify script to run')
3232
parser.add_argument('--backend', '-b', default=None,
3333
help='specify graphics frontend')
34+
parser.add_argument('--color', '-c', default='neutral',
35+
help='specify terminal color scheme (neutral, lightbg, nocolor, linux), linux is for dark mode')
36+
parser.add_argument('--confirmexit', '-x', default=False,
37+
help='confirm exit')
38+
parser.add_argument('--prompt', '-p', default='>>> ',
39+
help='input prompt')
40+
parser.add_argument('--resultprefix', '-r', default=None,
41+
help='execution result prefix, include {} for execution count number')
42+
parser.add_argument('--showassign', '-a', default=False,
43+
help='display the result of assignments')
3444
args = parser.parse_args()
3545

3646
if args.backend is not None:
3747
print(f"Using matplotlub backend {args.backend}")
3848
plt.use(args.backend)
3949

40-
# load some models
50+
# load some robot models
4151
puma = models.DH.Puma560()
4252
panda = models.DH.Panda()
4353

@@ -68,12 +78,32 @@
6878
## drop into IPython
6979
import IPython
7080
from traitlets.config import Config
81+
from IPython.terminal.prompts import ClassicPrompts
82+
from IPython.terminal.prompts import Prompts
83+
from pygments.token import Token
84+
85+
class MyPrompt(Prompts):
86+
def in_prompt_tokens(self, cli=None):
87+
return [(Token.Prompt, args.prompt)]
88+
def out_prompt_tokens(self, cli=None):
89+
if args.resultprefix is None:
90+
# traditional behaviour
91+
return [
92+
(Token.OutPrompt, 'Out['),
93+
(Token.OutPromptNum, str(self.shell.execution_count)),
94+
(Token.OutPrompt, ']: '),
95+
]
96+
else:
97+
return [(Token.Prompt, args.resultprefix.format(self.shell.execution_count))]
7198

7299
# set configuration options, there are lots, see
73100
# https://ipython.readthedocs.io/en/stable/config/options/terminal.html
74101
c = Config()
75-
c.InteractiveShellEmbed.colors = "Linux"
76-
c.InteractiveShell.colors = 'Neutral'
77-
c.InteractiveShell.confirm_exit = False
102+
c.InteractiveShellEmbed.colors = args.color
103+
c.InteractiveShell.confirm_exit = args.confirmexit
104+
# c.InteractiveShell.prompts_class = ClassicPrompts
105+
c.InteractiveShell.prompts_class = MyPrompt
106+
if args.showassign:
107+
c.InteractiveShell.ast_node_interactivity = 'last_expr_or_assign'
78108

79109
IPython.embed(config=c)

‎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: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -125,7 +125,7 @@ def launch(self, name=None, limits=None):
125125
plt.ion()
126126
plt.show()
127127

128-
self.t = 0
128+
self.sim_time = 0
129129

130130
# # Set the signal handler and a 0.1 second plot updater
131131
# signal.signal(signal.SIGALRM, self._plot_handler)
@@ -165,9 +165,9 @@ def step(self, dt=0.05):
165165
self._set_axes_equal()
166166

167167
# update time and display it on plot
168-
if self.t > 0:
169-
self.timer.set_text(f"t = {self.t:.2f}")
170-
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
171171

172172
# plt.ion()
173173

‎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)):

‎roboticstoolbox/backends/ROS/ROS.py

Lines changed: 23 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -11,8 +11,22 @@
1111
import time
1212
import subprocess
1313
import os
14-
import rospy
15-
from std_msgs.msg import Float32MultiArray
14+
# import rospy
15+
# from std_msgs.msg import Float32MultiArray
16+
17+
_ros = None
18+
rospy = None
19+
20+
21+
def _import_ros(): # pragma nocover
22+
import importlib
23+
global rospy
24+
try:
25+
ros = importlib.import_module('rospy')
26+
except ImportError:
27+
print(
28+
'\nYou must have ROS installed')
29+
raise
1630

1731

1832
class ROS(Connector): # pragma nocover
@@ -112,14 +126,14 @@ def __init__(self, robot):
112126
self.robot = robot
113127
self.v = np.zeros(robot.n)
114128

115-
self.velocity_publisher = rospy.Publisher(
116-
'/joint_velocity',
117-
Float32MultiArray, queue_size=1)
129+
# self.velocity_publisher = rospy.Publisher(
130+
# '/joint_velocity',
131+
# Float32MultiArray, queue_size=1)
118132

119133
self.relay()
120134

121-
def relay(self):
135+
# def relay(self):
122136

123-
while True:
124-
data = Float32MultiArray(data=self.robot.q)
125-
self.velocity_publisher.publish(data)
137+
# while True:
138+
# data = Float32MultiArray(data=self.robot.q)
139+
# self.velocity_publisher.publish(data)

0 commit comments

Comments
 (0)