Skip to content

Commit 9b6c4ab

Browse files
petercorkepetercorke
committed
bug cleanup
1 parent 52d6f4c commit 9b6c4ab

File tree

6 files changed

+101
-87
lines changed

6 files changed

+101
-87
lines changed

‎roboticstoolbox/robot/ELink.py

Lines changed: 25 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,15 @@ class BaseELink(Link):
1515

1616
def __init__(self, name=None, parent=None, joint_name=None, **kwargs):
1717

18-
super().__init__(kwargs)
18+
super().__init__(**kwargs)
1919

2020
self._name = name
2121

2222
if parent is not None:
2323
if isinstance(parent, (str, BaseELink)):
2424
self._parent = parent
2525
else:
26-
raise ValueError('parent must be BaseELink subclass or str')
26+
raise TypeError('parent must be BaseELink subclass or str')
2727
else:
2828
self._parent = None
2929

@@ -260,10 +260,27 @@ def collision(self):
260260
return self._collision
261261

262262
def ets(self):
263+
"""
264+
Link transform in ETS form
265+
266+
:return: elementary transform sequence for link transform
267+
:rtype: ETS or ETS2 instance
268+
269+
The sequence:
270+
271+
- has at least one element
272+
- may include zero or more constant transforms
273+
- no more than one variable transform, which if present will
274+
be last in the sequence
275+
"""
263276
if self.v is None:
277+
# no variable transform, return the constant part
264278
return self._ets
265279
else:
266-
self.v.jindex = self.jindex
280+
if self.jindex is not None:
281+
# inherit the jindex of the link if known
282+
self.v.jindex = self.jindex
283+
# return concatenation of constant and variable parts
267284
return self._ets * self.v
268285

269286
@collision.setter
@@ -359,6 +376,7 @@ def __init__(
359376
v = ets.pop()
360377
if jindex is not None:
361378
v.jindex = jindex
379+
self.jindex = jindex
362380
elif jindex is None and v.jindex is not None:
363381
jindex = v.jindex
364382

@@ -731,3 +749,7 @@ def A(self, q=0.0, **kwargs):
731749
return SE2()
732750
else:
733751
return SE2(T, check=False)
752+
753+
754+
l0 = ELink(qlim=[-1, 1])
755+
print(l0.qlim)

‎roboticstoolbox/robot/ERobot.py

Lines changed: 15 additions & 43 deletions
Original file line numberDiff line numberDiff line change
@@ -985,13 +985,18 @@ def __init__(self, arg, **kwargs):
985985
parent = None
986986
for j, ets_j in enumerate(arg.split()):
987987
elink = ELink(ets_j, parent=parent, name=f"link{j:d}")
988-
if elink.qlim is none and elink.v.qlim is not None:
988+
if elink.qlim is None and elink.v is not None and elink.v.qlim is not None:
989989
elink.qlim = elink.v.qlim
990990
parent = elink
991991
links.append(elink)
992992

993993
elif islistof(arg, ELink):
994994
links = arg
995+
996+
elif isinstance(arg, ERobot):
997+
# we're passed an ERobot, clone it
998+
links = [link.copy() for link in arg]
999+
9951000
else:
9961001
raise TypeError(
9971002
'constructor argument must be ETS or list of ELink')
@@ -2038,7 +2043,7 @@ def __init__(self, arg, **kwargs):
20382043
for j, ets_j in enumerate(arg.split()):
20392044
elink = ELink2(ets_j, parent=parent, name=f"link{j:d}")
20402045
parent = elink
2041-
if elink.qlim is none and elink.v.qlim is not None:
2046+
if elink.qlim is None and elink.v is not None and elink.v.qlim is not None:
20422047
elink.qlim = elink.v.qlim
20432048
links.append(elink)
20442049

@@ -2166,44 +2171,11 @@ def fkine(self, q, unit='rad', end=None, start=None):
21662171

21672172
if __name__ == "__main__": # pragma nocover
21682173

2169-
# import roboticstoolbox as rtb
2170-
np.set_printoptions(precision=4, suppress=True)
2171-
2172-
robot = ERobot([
2173-
ELink(ETS.rz(), name='link1'),
2174-
ELink(
2175-
ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(),
2176-
name='link2', parent='link1'),
2177-
ELink(ETS.tx(1), name='ee_1', parent='link2'),
2178-
ELink(
2179-
ETS.tx(1) * ETS.ty(0.5) * ETS.rz(),
2180-
name='link3', parent='link1'),
2181-
ELink(ETS.tx(1), name='ee_2', parent='link3')
2182-
])
2183-
print(robot)
2184-
2185-
# p = rtb.models.URDF.Puma560()
2186-
# p.fkine(p.qz)
2187-
# p.jacob0(p.qz)
2188-
# p.jacobe(p.qz)
2189-
2190-
# robot = rtb.models.ETS.Panda()
2191-
# print(robot)
2192-
# print(robot.base, robot.tool)
2193-
# print(robot.ee_links)
2194-
# ets = robot.ets()
2195-
# print(ets)
2196-
# print('n', ets.n)
2197-
# ets2 = ets.compile()
2198-
# print(ets2)
2199-
2200-
# q = np.random.rand(7)
2201-
# # print(ets.eval(q))
2202-
# # print(ets2.eval(q))
2203-
2204-
# J1 = robot.jacob0(q)
2205-
# J2 = ets2.jacob0(q)
2206-
# print(J1-J2)
2207-
2208-
# print(robot[2].v, robot[2].v.jindex)
2209-
# print(robot[2].Ts)
2174+
e1 = ELink(ETS.rz(), jindex=0)
2175+
e2 = ELink(ETS.rz(), jindex=1, parent=e1)
2176+
e3 = ELink(ETS.rz(), jindex=2, parent=e2)
2177+
e4 = ELink(ETS.rz(), jindex=5, parent=e3)
2178+
2179+
ERobot([e1, e2, e3, e4])
2180+
2181+
pass

‎roboticstoolbox/robot/Robot.py

Lines changed: 13 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
import roboticstoolbox as rtb
66
from spatialmath import SE3, SE2
77
from spatialmath.base.argcheck import isvector, getvector, getmatrix, \
8-
getunit
8+
getunit, verifymatrix
99
from roboticstoolbox.robot.Link import Link
1010
# from spatialmath.base.transforms3d import tr2delta
1111
# from roboticstoolbox.tools import urdf
@@ -55,9 +55,10 @@ def __init__(
5555
self.symbolic = symbolic
5656
self.tool = tool
5757
self._reach = None
58+
self._base = base
5859

59-
if base is None:
60-
self.base = SE3()
60+
# if base is None:
61+
# self.base = SE3()
6162

6263
if keywords is not None and not isinstance(keywords, (tuple, list)):
6364
raise TypeError('keywords must be a list or tuple')
@@ -532,11 +533,9 @@ def todegrees(self, q):
532533

533534
q = getmatrix(q, (None, self.n))
534535

535-
j = 0
536-
for k, revolute in enumerate(self.revolutejoints):
536+
for j, revolute in enumerate(self.revolutejoints):
537537
if revolute:
538538
q[:,j] *= 180.0 / np.pi
539-
j += 1
540539
if q.shape[0] == 1:
541540
return q[0]
542541
else:
@@ -569,9 +568,9 @@ def toradians(self, q):
569568

570569
q = getmatrix(q, (None, self.n))
571570

572-
for k, revolute in enumerate(self.revolutejoints):
571+
for j, revolute in enumerate(self.revolutejoints):
573572
if revolute:
574-
q[:,k] *= np.pi / 180.0
573+
q[:,j] *= np.pi / 180.0
575574
if q.shape[0] == 1:
576575
return q[0]
577576
else:
@@ -881,12 +880,12 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes='all'):
881880

882881
J = self.jacob0(q, start=start, end=end)
883882
else:
884-
verifymatrix(J, (6, n))
883+
verifymatrix(J, (6, self.n))
885884

886885
if H is None:
887886
H = self.hessian0(J0=J, start=start, end=end)
888887
else:
889-
verifymatrix(H, (6, n, n))
888+
verifymatrix(H, (6, self.n, self.n))
890889

891890
manipulability = self.manipulability(
892891
q, J=J, start=start, end=end, axes=axes)
@@ -895,9 +894,9 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes='all'):
895894
H = H[axes, :, :]
896895

897896
b = np.linalg.inv(J @ np.transpose(J))
898-
Jm = np.zeros((n, 1))
897+
Jm = np.zeros((self.n, 1))
899898

900-
for i in range(n):
899+
for i in range(self.n):
901900
c = J @ np.transpose(H[:, :, i])
902901
Jm[i, 0] = manipulability * \
903902
np.transpose(c.flatten('F')) @ b.flatten('F')
@@ -979,7 +978,7 @@ def base(self):
979978
is an identity matrix.
980979
"""
981980
if self._base is None:
982-
if isinstance(self, ERobot2):
981+
if isinstance(self, rtb.ERobot2):
983982
self._base = SE2()
984983
else:
985984
self._base = SE3()
@@ -1314,7 +1313,7 @@ def plot(
13141313
if q.shape[0] == 1:
13151314
env.launch(self.name + ' Plot', limits=limits, fig=fig)
13161315
else:
1317-
env.launch(self.name + ' Trajectory Plot', limits, fig=fig)
1316+
env.launch(self.name + ' Trajectory Plot', limits=limits, fig=fig)
13181317

13191318
env.add(
13201319
self, readonly=True, jointaxes=jointaxes, jointlabels=jointlabels,

‎tests/test_DHRobot.py

Lines changed: 14 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,6 @@ def test_todegrees(self):
5252
ans = np.array([np.pi, 180, np.pi, 90])
5353

5454
nt.assert_array_almost_equal(r0.todegrees(q), ans)
55-
nt.assert_array_almost_equal(r0.todegrees(), np.zeros(4))
5655

5756
def test_toradians(self):
5857
l0 = rp.PrismaticDH()
@@ -156,10 +155,8 @@ def test_fkine(self):
156155
])
157156

158157
r0 = rp.DHRobot([l0, l1, l2, l3])
159-
r0.q = q
160158

161159
nt.assert_array_almost_equal(r0.fkine(q).A, T1)
162-
nt.assert_array_almost_equal(r0.fkine().A, T1)
163160

164161
def test_fkine_traj(self):
165162
l0 = rp.PrismaticDH()
@@ -1074,7 +1071,7 @@ def test_inertia(self):
10741071
# nt.assert_array_almost_equal(I1[0, :, :], Ir, decimal=4)
10751072
# nt.assert_array_almost_equal(I1[1, :, :], Ir, decimal=4)
10761073

1077-
def test_cinertia(self):
1074+
def test_inertia_x(self):
10781075
puma = rp.models.DH.Puma560()
10791076
q = puma.qn
10801077

@@ -1086,8 +1083,8 @@ def test_cinertia(self):
10861083
[0.2795, -0.0703, 0.2767, 0.0000, 0.1713, 0.0000],
10871084
[0.0000, -0.9652, -0.0000, 0.1941, 0.0000, 0.5791]]
10881085

1089-
M0 = puma.inertia_x(q)
1090-
M1 = puma.inertia_x(np.c_[q, q].T)
1086+
M0 = puma.inertia_x(q, analytical=None)
1087+
M1 = puma.inertia_x(np.c_[q, q].T, analytical=None)
10911088

10921089
nt.assert_array_almost_equal(M0, Mr, decimal=4)
10931090
nt.assert_array_almost_equal(M1[0, :, :], Mr, decimal=4)
@@ -1192,14 +1189,21 @@ def test_jacob_dot(self):
11921189
puma = rp.models.DH.Puma560()
11931190
puma.q = puma.qr
11941191
puma.qd = puma.qr
1195-
q = puma.qr
1192+
q = puma.qn
1193+
qd = [0.1, -0.2, 0.3, -0.4, 0.5, -0.6]
1194+
1195+
from roboticstoolbox.tools import hessian_numerical
11961196

1197-
j0 = puma.jacob_dot(q, q)
1197+
j0 = puma.jacob_dot(q, qd)
1198+
1199+
H = hessian_numerical(lambda q: puma.jacob0(q), q)
1200+
Jd = np.zeros((6, puma.n))
1201+
for i in range(puma.n):
1202+
Jd += H[:, :, i] * qd[i]
11981203

11991204
res = [-0.0000, -1.0654, -0.3702, 2.4674, 0, 0]
12001205

1201-
nt.assert_array_almost_equal(j0, res, decimal=4)
1202-
nt.assert_array_almost_equal(j1, res, decimal=4)
1206+
nt.assert_array_almost_equal(j0, Jd, decimal=4)
12031207

12041208
def test_yoshi(self):
12051209
puma = rp.models.DH.Puma560()

‎tests/test_ELink.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,7 @@ def test_str_ets(self):
2121

2222
l0 = rp.ELink(rx * ry * tz)
2323

24-
ans = 'ELink[Rx(88.41°) * Ry(88.41°) * tz(1)] '
24+
ans = 'ELink[Rx(88.41°) ⊕ Ry(88.41°) ⊕ tz(1)] '
2525

2626
self.assertEqual(str(l0), ans)
2727

‎tests/test_ERobot.py

Lines changed: 33 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,8 @@
77
import numpy.testing as nt
88
import numpy as np
99
import roboticstoolbox as rtb
10-
from roboticstoolbox import ERobot, ELink, ETS
10+
from roboticstoolbox import ERobot, ELink, ELink2, ETS, ETS2, ERobot2
11+
from spatialmath import SE2, SE3
1112
import unittest
1213
import spatialmath as sm
1314
import spatialgeometry as gm
@@ -280,21 +281,37 @@ def test_fkine_traj(self):
280281
nt.assert_array_almost_equal(TT[3].A, ans)
281282

282283
def test_fkine_all(self):
283-
pm = rtb.models.DH.Panda()
284-
p = rtb.models.ETS.Panda()
285-
q = np.array([1.0, 2, 3, 4, 5, 6, 7])
286-
p.q = q
287-
pm.q = q
288-
289-
p.fkine_all(q)
290-
r2 = pm.fkine_all(q)
291-
292-
for i in range(7):
293-
nt.assert_array_almost_equal(p.links[i]._fk, r2[i].A)
294-
295-
p.fkine_all(q)
296-
for i in range(7):
297-
nt.assert_array_almost_equal(p.links[i]._fk, r2[i].A)
284+
a1 = 1
285+
a2 = 1
286+
e = ETS2.r() * ETS2.tx(a1) * ETS2.r() * ETS2.tx(a2);
287+
robot = ERobot2(e)
288+
289+
T = robot.fkine_all([0, 0])
290+
self.assertIsInstance(T, SE2)
291+
self.assertEqual(len(T), 4)
292+
293+
nt.assert_array_almost_equal(T[0].A, SE2().A)
294+
nt.assert_array_almost_equal(T[1].A, SE2().A)
295+
nt.assert_array_almost_equal(T[2].A, SE2(1, 0).A)
296+
nt.assert_array_almost_equal(T[3].A, SE2(2, 0).A)
297+
298+
robot = ERobot2([
299+
ELink2(ETS2.r(), name='link1'),
300+
ELink2(ETS2.tx(1.2) * ETS2.ty(-0.5) * ETS2.r(), name='link2', parent='link1'),
301+
ELink2(ETS2.tx(1), name='ee_1', parent='link2'),
302+
ELink2(ETS2.tx(0.6) * ETS2.ty(0.5) * ETS2.r(), name='link3', parent='link1'),
303+
ELink2(ETS2.tx(1), name='ee_2', parent='link3')
304+
])
305+
T = robot.fkine_all([0, 0, 0])
306+
self.assertIsInstance(T, SE2)
307+
self.assertEqual(len(T), 6)
308+
309+
nt.assert_array_almost_equal(T[0].A, SE2().A)
310+
nt.assert_array_almost_equal(T[1].A, SE2().A)
311+
nt.assert_array_almost_equal(T[2].A, SE2(1.2, -0.5).A)
312+
nt.assert_array_almost_equal(T[3].A, SE2(2.2, -0.5).A)
313+
nt.assert_array_almost_equal(T[4].A, SE2(0.6, 0.5).A)
314+
nt.assert_array_almost_equal(T[5].A, SE2(1.6, 0.5).A)
298315

299316
def test_jacob0(self):
300317
panda = rtb.models.ETS.Panda()

0 commit comments

Comments
 (0)