8000 Merge branch 'master' of github.com:petercorke/robotics-toolbox-python · hanm2019/robotics-toolbox-python@97991b7 · GitHub
[go: up one dir, main page]
Skip to content

Commit 97991b7

Browse files
jhavljhavl
committed
Merge branch 'master' of github.com:petercorke/robotics-toolbox-python
2 parents e34f538 + 99e94a9 commit 97991b7

File tree

14 files changed

+132
-83
lines changed

14 files changed

+132
-83
lines changed

‎README.md

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -229,9 +229,9 @@ The toolbox is incredibly useful for developing and prototyping algorithms for r
229229

230230
### Publication List
231231

232-
**NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators**, J. Haviland and P. Corke. In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
232+
J. Haviland and P. Corke, "**NEO: A Novel Expeditious Optimisation Algorithm for Reactive Motion Control of Manipulators**," in _IEEE Robotics and Automation Letters_, doi: 10.1109/LRA.2021.3056060. In the video, the robot is controlled using the Robotics toolbox for Python and features a recording from the [Swift](https://github.com/jhavl/swift) Simulator.
233233

234-
[[Paper](https://arxiv.org/abs/2010.08686)] [[Project Website](https://jhavl.github.io/neo/)] [[Video](https://youtu.be/jSLPJBr8QTY)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/master/examples/neo.py)]
234+
[[Arxiv Paper](https://arxiv.org/abs/2010.08686)] [[IEEE Xplore](https://ieeexplore.ieee.org/document/9343718)] [[Project Website](https://jhavl.github.io/neo/)] [[Video](https://youtu.be/jSLPJBr8QTY)] [[Code Example](https://github.com/petercorke/robotics-toolbox-python/blob/master/examples/neo.py)]
235235

236236
<p>
237237
<a href="https://youtu.be/jSLPJBr8QTY">

‎examples/readme.py

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,14 @@
99
# IK
1010
from spatialmath import SE3
1111

12-
T = SE3(0.8, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
13-
q_pickup, *_ = robot.ikine(T) # solve IK, ignore additional outputs
14-
print(q_pickup)# display joint angles
15-
print(robot.fkine(q_pickup)) # FK shows that desired end-effector pose was achieved
12+
T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
13+
sol = robot.ikine_LMS(T) # solve IK, ignore additional outputs
14+
print(sol.q) # display joint angles
15+
print(robot.fkine(sol.q)) # FK shows that desired end-effector pose was achieved
1616

1717

18-
qt = rtb.trajectory.jtraj(robot.qz, q_pickup, 50)
19-
robot.plot(qt.q, movie="panda1.gif")
18+
qtraj = rtb.jtraj(robot.qz, sol.q, 50)
19+
robot.plot(qtraj.q, movie="panda1.gif")
2020

2121
# URDF + Swift version
2222
dt = 0.050 # simulation timestep in seconds
@@ -27,7 +27,7 @@
2727
env.launch("chrome") # activate it
2828
env.add(robot) # add robot to the 3D scene
2929
env.start_recording("panda2", 1 / dt)
30-
for qk in qt.q: # for each joint configuration on trajectory
30+
for qk in qtraj.q: # for each joint configuration on trajectory
3131
robot.q = qk # update the robot state
3232
env.step() # update visualization
3333
env.stop_recording()

‎examples/rtb.py renamed to ‎examples/rtbtool

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939
help='input prompt')
4040
parser.add_argument('--resultprefix', '-r', default=None,
4141
help='execution result prefix, include {} for execution count number')
42-
parser.add_argument('--showassign', '-a', default=False,
42+
parser.add_argument('--showassign', '-a', default=False, action='store_true',
4343
help='display the result of assignments')
4444
args = parser.parse_args()
4545

‎roboticstoolbox/mobile/PoseGraph.py

Lines changed: 34 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,7 @@ def __init__(self, filename, laser=False, verbose=False):
161161
lasermeta = tokens[2:6]
162162
firstlaser = False
163163

164+
v = self.graph.add_vertex()
164165
v.theta = np.arange(0, nbeams) * float(tokens[4]) + float(tokens[2])
165166
v.range = np.array([float(x) for x in tokens[9:nbeams+9]])
166167
v.time = float(tokens[21+nbeams])
@@ -189,7 +190,6 @@ def scan(self, i):
189190
return v.range, v.theta
190191

191192
def scanxy(self, i):
192-
v = self.vindex[i]
193193

194194
range, theta = self.scan(i)
195195
x = range * np.cos(theta)
@@ -328,10 +328,6 @@ def bresenham(p1, p2):
328328

329329
return x, y
330330

331-
332-
333-
334-
335331

336332
# This source code is part of the graph optimization package
337333
# deveoped for the lectures of robotics2 at the University of Freiburg.
@@ -367,43 +363,43 @@ def bresenham(p1, p2):
367363
# PURPOSE.
368364

369365

370-
# %ls-slam.m
371-
# %this file is released under the creative common license
366+
# %ls-slam.m
367+
# %this file is released under the creative common license
368+
369+
# solves a graph-based slam problem via least squares
370+
# vmeans: matrix containing the column vectors of the poses of the vertices
371+
# the vertices are odrered such that vmeans[i] corresponds to the ith id
372+
# eids: matrix containing the column vectors [idFrom, idTo]' of the ids of the vertices
373+
# eids[k] corresponds to emeans[k] and einfs[k].
374+
# emeans: matrix containing the column vectors of the poses of the edges
375+
# einfs: 3d matrix containing the information matrices of the edges
376+
# einfs(:,:,k) refers to the information matrix of the k-th edge.
377+
# n: number of iterations
378+
# newmeans: matrix containing the column vectors of the updated vertices positions
372379

373-
# solves a graph-based slam problem via least squares
374-
# vmeans: matrix containing the column vectors of the poses of the vertices
375-
# the vertices are odrered such that vmeans[i] corresponds to the ith id
376-
# eids: matrix containing the column vectors [idFrom, idTo]' of the ids of the vertices
377-
# eids[k] corresponds to emeans[k] and einfs[k].
378-
# emeans: matrix containing the column vectors of the poses of the edges
379-
# einfs: 3d matrix containing the information matrices of the edges
380-
# einfs(:,:,k) refers to the information matrix of the k-th edge.
381-
# n: number of iterations
382-
# newmeans: matrix containing the column vectors of the updated vertices positions
380+
def optimize(self, iterations = 10, animate = False, retain = False):
383381

384-
def optimize(self, iterations = 10, animate = False, retain = False):
385-
386-
387-
g2 = PGraph(self.graph) # deep copy
382+
383+
g2 = PGraph(self.graph) # deep copy
384+
385+
eprev = math.inf
386+
for i in range(iterations):
387+
if animate:
388+
if not retain:
389+
plt.clf()
390+
g2.plot()
391+
plt.pause(0.5)
388392

389-
eprev = math.inf
390-
for i in range(iterations):
391-
if animate:
392-
if not retain:
393-
plt.clf()
394-
g2.plot()
395-
plt.pause(0.5)
396-
397-
vmeans, energy = linearize_and_solve(g2)
398-
g2.setcoord(vmeans)
399-
400-
if energy >= eprev:
401-
break
402-
eprev = energy
393+
vmeans, energy = linearize_and_solve(g2)
394+
g2.setcoord(vmeans)
403395

404-
self.graph = g2
396+
if energy >= eprev:
397+
break
398+
eprev = energy
399+
400+
self.graph = g2
405401

406-
return g2
402+
return g2
407403

408404

409405
#computes the taylor expansion of the error function of the k_th edge
@@ -460,7 +456,7 @@ def linear_factors(self, edge):
460456

461457
ztinv = base.trinv2(zt_ij)
462458
T = ztinv @ f_ij
463-
e = np.r_[base.transl2(T), base.angle(T)]
459+
e = tr2xyt(T)
464460
ztinv[0:2,2] = 0
465461
A = ztinv @ A
466462
B = ztinv @ B

‎roboticstoolbox/mobile/bug2.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -127,7 +127,7 @@ def query(self, start=None, goal=None, animate=False, movie=None, current=False)
127127
% Options::
128128
% 'animate' show a simulation of the robot moving along the path
129129
% 'movie',M create a movie
130-
% 'current' show the current position position as a black circle
130+
% 'current' show the current position as a black circle
131131
%
132132
% Notes::
133133
% - START and GOAL are given as X,Y coordinates in the grid map, not as
@@ -172,7 +172,7 @@ def query(self, start= F438 None, goal=None, animate=False, movie=None, current=False)
172172
plt.plot(robot[0], robot[1], 'y.')
173173
plt.pause(0.1)
174174
if current:
175-
h = self.plot(robot[0], robot[1])
175+
h = self.plot(robot)
176176
plt.draw()
177177
if movie is not None:
178178
anim.plot(h)

‎roboticstoolbox/models/DH/Puma560.py

Lines changed: 21 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
import numpy as np
2020
from roboticstoolbox import DHRobot, RevoluteDH
2121
from spatialmath import SE3
22+
from spatialmath import base
2223

2324

2425
class Puma560(DHRobot):
@@ -228,7 +229,9 @@ def ikine_a(self, T, config="lun"):
228229
GTRI/ATRP/IIMB, Georgia Institute of Technology, 2/13/95
229230
230231
"""
231-
def ik3(robot, T, config):
232+
def ik3(robot, T, config='lun'):
233+
234+
config = self.config_validate(config, ('lr', 'ud', 'nf'))
232235

233236
# solve for the first three joints
234237

@@ -253,8 +256,11 @@ def ik3(robot, T, config):
253256
r = np.sqrt(Px**2 + Py**2)
254257
if 'r' in config:
255258
theta[0] = np.arctan2(Py, Px) + np.arcsin(d3 / r)
256-
else:
259+
elif 'l' in config:
257260
theta[0] = np.arctan2(Py, Px) + np.pi - np.arcsin(d3 / r)
261+
else:
262+
raise ValueError('bad configuration string')
263+
258264

259265
# Solve for theta[1]
260266
# V114 is defined in equation 43, p.39.
@@ -264,10 +270,12 @@ def ik3(robot, T, config):
264270
# configuration parameter n2
265271
if 'u' in config:
266272
n2 = 1
267-
else:
273+
elif 'd' in config:
268274
n2 = -1
275+
else:
276+
raise ValueError('bad configuration string')
269277

270-
if 'r' in config:
278+
if 'l' in config:
271279
n2 = -n2
272280

273281
V114 = Px * np.cos(theta[0]) + Py * np.sin(theta[0])
@@ -289,8 +297,9 @@ def ik3(robot, T, config):
289297
den = np.cos(theta[1]) * Pz - np.sin(theta[1]) * V114
290298
theta[2] = np.arctan2(a3, d4) - np.arctan2(num, den)
291299

292-
return theta
300+
theta = base.angdiff(theta)
293301

302+
return theta
294303

295304
return self.ikine_6s(T, config, ik3)
296305

@@ -300,3 +309,10 @@ def ik3(robot, T, config):
300309
puma = Puma560(symbolic=False)
301310
print(puma)
302311
print(puma.dyntable())
312+
T = puma.fkine(puma.qn)
313+
print(puma.ikine_a(T, 'lu').q)
314+
print(puma.ikine_a(T, 'ru').q)
315+
print(puma.ikine_a(T, 'ld').q)
316+
print(puma.ikine_a(T, 'rd').q)
317+
318+
puma.plot(puma.qz)

‎roboticstoolbox/robot/DHFactor.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -449,8 +449,8 @@ def __str__(self, q=None):
449449
q = "q{0}"
450450
else:
451451
q = "q"
452-
q = "q{0}"
453-
# For et in the object, display it, data comes from properties
452+
453+
# For each ET in the object, display it, data comes from properties
454454
# which come from the named tuple
455455
for et in self:
456456

‎roboticstoolbox/robot/DHRobot.py

Lines changed: 43 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1041,7 +1041,7 @@ def jacob0(self, q=None, T=None):
10411041
return tr2jac(trinv(T.A)) @ self.jacobe(q)
10421042

10431043
def jacob_dot(self, q=None, qd=None):
1044-
"""
1044+
r"""
10451045
Derivative of Jacobian
10461046
10471047
:param q: The joint configuration of the robot (Optional,
@@ -1651,7 +1651,49 @@ def ikine_6s(self, T, config, ikfunc):
16511651
else:
16521652
return solutions
16531653

1654+
def config_validate(self, config, allowables):
1655+
"""
1656+
Validate a configuration string
1657+
1658+
:param config: a configuration string
1659+
:type config: str
1660+
:param allowable: [description]
1661+
:type allowable: tuple of str
1662+
:raises ValueError: bad character in configuration string
1663+
:return: configuration string
1664+
:rtype: str
1665+
1666+
For analytic inverse kinematics the Toolbox uses a string whose
1667+
letters indicate particular solutions, eg. for the Puma 560
16541668
1669+
========= ===================
1670+
Character Meaning
1671+
========= ===================
1672+
'l' lefty
1673+
'r' righty
1674+
'u' elbow up
1675+
'd' elbow down
1676+
'n' wrist not flipped
1677+
'f' wrist flipped
1678+
========= ===================
1679+
1680+
This method checks that the configuration string is valid and adds
1681+
default values for missing characters. For example:
1682+
1683+
config = self.config_validate(config, ('lr', 'ud', 'nf'))
1684+
1685+
indicates the valid characters, and the first character in each
1686+
string is the default, ie. if neither 'l' or 'r' is given then
1687+
'l' will be added to the string.
1688+
1689+
"""
1690+
for c in config:
1691+
if not any([c in allowable for allowable in allowables]):
1692+
raise ValueError(f"bad config specifier <{c}>")
1693+
for allowable in allowables:
1694+
if all([a not in config for a in allowable]):
1695+
config += allowable[0]
1696+
return config
16551697
class SerialLink(DHRobot):
16561698
def __init__(self, *args, **kwargs):
16571699
print('SerialLink is deprecated, use DHRobot instead')

‎roboticstoolbox/robot/ERobot.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1202,7 +1202,7 @@ def _getlink(self, link, default=None):
12021202
raise TypeError('unknown argument')
12031203

12041204
def jacob0(self, q, endlink=None, startlink=None, offset=None, T=None):
1205-
"""
1205+
r"""
12061206
Manipulator geometric Jacobian in the base frame
12071207
12081208
:param q: Joint coordinate vector
@@ -1309,7 +1309,7 @@ def jacob0(self, q, endlink=None, startlink=None, offset=None, T=None):
13091309
return J
13101310

13111311
def jacobe(self, q, endlink=None, startlink=None, offset=None, T=None):
1312-
"""
1312+
r"""
13131313
Manipulator geometric Jacobian in the end-effector frame
13141314
13151315
:param q: Joint coordinate vector

‎roboticstoolbox/robot/IK.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -240,6 +240,7 @@ def ikine_LM(
240240
for k in range(slimit):
241241
# choose a random joint coordinate
242242
q0_k = np.random.rand(self.n) * qspan + qlim[0, :]
243+
print('search starts at ', q0_k)
243244

244245
# recurse into the solver
245246
solution = self.ikine_LM(

0 commit comments

Comments
 (0)
0