10000 ET tests passing · ctc-eng/robotics-toolbox-python@80eafec · GitHub
[go: up one dir, main page]
Skip to content

Commit 80eafec

Browse files
jhavljhavl
committed
ET tests passing
1 parent 62add1e commit 80eafec

File tree

9 files changed

+210
-210
lines changed

9 files changed

+210
-210
lines changed

‎roboticstoolbox/backend/urdf/urdf.py

Lines changed: 28 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -1640,64 +1640,64 @@ def __init__(self, name, links, joints=None,
16401640

16411641
for j in self.joints:
16421642

1643-
ets = []
1643+
ets = rp.ET()
16441644
T = sm.SE3(j.origin)
16451645
trans = T.t
16461646
rot = j.rpy
16471647
# print(trans)
16481648
# print(rot)
16491649

16501650
if trans[0] != 0:
1651-
ets.append(rp.ET.Ttx(trans[0]))
1651+
ets = ets * rp.ET.tx(trans[0])
16521652

16531653
if trans[1] != 0:
1654-
ets.append(rp.ET.Tty(trans[1]))
1654+
ets = ets * rp.ET.ty(trans[1])
16551655

16561656
if trans[2] != 0:
1657-
ets.append(rp.ET.Ttz(trans[2]))
1657+
ets = ets * rp.ET.tz(trans[2])
16581658

16591659
if rot[0] != 0:
1660-
ets.append(rp.ET.TRx(rot[0]))
1660+
ets = ets * rp.ET.rx(rot[0])
16611661

16621662
if rot[1] != 0:
1663-
ets.append(rp.ET.TRy(rot[1]))
1663+
ets = ets * rp.ET.ry(rot[1])
16641664

16651665
if rot[2] != 0:
1666-
ets.append(rp.ET.TRz(rot[2]))
1666+
ets = ets * rp.ET.rz(rot[2])
16671667

16681668
if j.joint_type == 'revolute' or \
16691669
j.joint_type == 'continuous': # pragma nocover
16701670
if j.axis[0] == 1:
1671-
ets.append(rp.ET.TRx())
1671+
ets = ets * rp.ET.rx()
16721672
elif j.axis[0] == -1:
1673-
ets.append(rp.ET.TRy(np.pi))
1674-
ets.append(rp.ET.TRx())
1673+
ets = ets * rp.ET.ry(np.pi)
1674+
ets = ets * rp.ET.rx()
16751675
elif j.axis[1] == 1:
1676-
ets.append(rp.ET.TRy())
1676+
ets = ets * rp.ET.ry()
16771677
elif j.axis[1] == -1:
1678-
ets.append(rp.ET.TRz(np.pi))
1679-
ets.append(rp.ET.TRy())
1678+
ets = ets * rp.ET.rz(np.pi)
1679+
ets = ets * rp.ET.ry()
16801680
elif j.axis[2] == 1:
1681-
ets.append(rp.ET.TRz())
1681+
ets = ets * rp.ET.rz()
16821682
elif j.axis[2] == -1:
1683-
ets.append(rp.ET.TRx(np.pi))
1684-
ets.append(rp.ET.TRz())
1683+
ets = ets * rp.ET.rx(np.pi)
1684+
ets = ets * rp.ET.rz()
16851685
elif j.joint_type == 'prismatic': # pragma nocover
16861686
if j.axis[0] == 1:
1687-
ets.append(rp.ET.Ttx())
1687+
ets = ets * rp.ET.tx()
16881688
elif j.axis[0] == -1:
1689-
ets.append(rp.ET.TRy(np.pi))
1690-
ets.append(rp.ET.Ttx())
1689+
ets = ets * rp.ET.ry(np.pi)
1690+
ets = ets * rp.ET.tx()
16911691
elif j.axis[1] == 1:
1692-
ets.append(rp.ET.Tty())
1692+
ets = ets * rp.ET.ty()
16931693
elif j.axis[1] == -1:
1694-
ets.append(rp.ET.TRz(np.pi))
1695-
ets.append(rp.ET.Tty())
1694+
ets = ets * rp.ET.rz(np.pi)
1695+
ets = ets * rp.ET.ty()
16961696
elif j.axis[2] == 1:
1697-
ets.append(rp.ET.Ttz())
1697+
ets = ets * rp.ET.tz()
16981698
elif j.axis[2] == -1:
1699-
ets.append(rp.ET.TRx(np.pi))
1700-
ets.append(rp.ET.Ttz())
1699+
ets = ets * rp.ET.rx(np.pi)
1700+
ets = ets * rp.ET.tz()
17011701

17021702
try:
17031703
qlim = [j.limit.lower, j.limit.upper]
@@ -1724,7 +1724,7 @@ def __init__(self, name, links, joints=None,
17241724
if not found:
17251725
link = self._link_map[self.joints[i].parent]
17261726
base_link = rp.ELink(
1727-
[],
1727+
rp.ET(),
17281728
name=link.name)
17291729
elinks[i]._parent.append(base_link)
17301730
try:
@@ -1938,6 +1938,6 @@ def _from_xml(cls, node, path):
19381938
if child.tag not in valid_tags:
19391939
extra_xml_node.append(child)
19401940

1941-
data = ET.tostring(extra_xml_node)
1942-
kwargs['other_xml'] = data
1941+
# data = ET.ostring(extra_xml_node)
1942+
# kwargs['other_xml'] = data
19431943
return URDF(**kwargs)

‎roboticstoolbox/models/ETS/Frankie.py

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -40,69 +40,69 @@ def __init__(self):
4040
tool_offset = (103)*mm
4141

4242
b0 = ELink(
43-
[ET.TRz()],
43+
ET.rz(),
4444
name='base0',
4545
parent=None
4646
)
4747

4848
b1 = ELink(
49-
[ET.Ttx()],
49+
ET.tx(),
5050
name='base1',
5151
parent=b0
5252
)
5353

5454
l0 = ELink(
55-
[ET.Ttz(0.333), ET.TRz()],
55+
ET.tz(0.333) * ET.rz(),
5656
name='link0',
5757
parent=b1
5858
)
5959

6060
l1 = ELink(
61-
[ET.TRx(-90*deg), ET.TRz()],
61+
ET.rx(-90*deg) * ET.rz(),
6262
name='link1',
6363
parent=l0
6464
)
6565

6666
l2 = ELink(
67-
[ET.TRx(90*deg), ET.Ttz(0.316), ET.TRz()],
67+
ET.rx(90*deg) * ET.tz(0.316) * ET.rz(),
6868
name='link2',
6969
parent=l1
7070
)
7171

7272
l3 = ELink(
73-
[ET.Ttx(0.0825), ET.TRx(90*deg), ET.TRz()],
73+
ET.tx(0.0825) * ET.rx(90*deg) * ET.rz(),
7474
name='link3',
7575
parent=l2
7676
)
7777

7878
l4 = ELink(
79-
[ET.Ttx(-0.0825), ET.TRx(-90*deg), ET.Ttz(0.384), ET.TRz()],
79+
ET.tx(-0.0825) * ET.rx(-90*deg) * ET.tz(0.384) * ET.rz(),
8080
name='link4',
8181
parent=l3
8282
)
8383

8484
l5 = ELink(
85-
[ET.TRx(90*deg), ET.TRz()],
85+
ET.rx(90*deg) * ET.rz(),
8686
name='link5',
8787
parent=l4
8888
)
8989

9090
l6 = ELink(
91-
[ET.Ttx(0.088), ET.TRx(90*deg), ET.Ttz(0.107), ET.TRz()],
91+
ET.tx(0.088) * ET.rx(90*deg) * ET.tz(0.107) * ET.rz(),
9292
name='link6',
9393
parent=l5
9494
)
9595

9696
ee = ELink(
97-
[ET.Ttz(tool_offset), ET.TRz(-np.pi/4)],
97+
ET.tz(tool_offset) * ET.rz(-np.pi/4),
9898
name='ee',
9999
parent=l6
100100
)
101101

102-
ETlist = [b0, b1, l0, l1, l2, l3, l4, l5, l6, ee]
102+
ets = [b0, b1, l0, l1, l2, l3, l4, l5, l6, ee]
103103

104104
super(Frankie, self).__init__(
105-
ETlist,
105+
ets,
106106
name='Frankie',
107107
manufacturer='Franka Emika, Omron')
108108

‎roboticstoolbox/models/ETS/Panda.py

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -29,57 +29,57 @@ def __init__(self):
2929
tool_offset = (103)*mm
3030

3131
l0 = ELink(
32-
[ET.Ttz(0.333), ET.TRz()],
32+
ET.tz(0.333) * ET.rz(),
3333
name='link0',
3434
parent=None
3535
)
3636

3737
l1 = ELink(
38-
[ET.TRx(-90*deg), ET.TRz()],
38+
ET.rx(-90*deg) * ET.rz(),
3939
name='link1',
4040
parent=l0
4141
)
4242

4343
l2 = ELink(
44-
[ET.TRx(90*deg), ET.Ttz(0.316), ET.TRz()],
44+
ET.rx(90*deg) * ET.tz(0.316) * ET.rz(),
4545
name='link2',
4646
parent=l1
4747
)
4848

4949
l3 = ELink(
50-
[ET.Ttx(0.0825), ET.TRx(90*deg), ET.TRz()],
50+
ET.tx(0.0825) * ET.rx(90*deg) * ET.rz(),
5151
name='link3',
5252
parent=l2
5353
)
5454

5555
l4 = ELink(
56-
[ET.Ttx(-0.0825), ET.TRx(-90*deg), ET.Ttz(0.384), ET.TRz()],
56+
ET.tx(-0.0825) * ET.rx(-90*deg) * ET.tz(0.384) * ET.rz(),
5757
name='link4',
5858
parent=l3
5959
)
6060

6161
l5 = ELink(
62-
[ET.TRx(90*deg), ET.TRz()],
62+
ET.rx(90*deg) * ET.rz(),
6363
name='link5',
6464
parent=l4
6565
)
6666

6767
l6 = ELink(
68-
[ET.Ttx(0.088), ET.TRx(90*deg), ET.Ttz(0.107), ET.TRz()],
68+
ET.tx(0.088) * ET.rx(90*deg) * ET.tz(0.107) * ET.rz(),
6969
name='link6',
7070
parent=l5
7171
)
7272

7373
ee = ELink(
74-
[ET.Ttz(tool_offset), ET.TRz(-np.pi/4)],
74+
ET.tz(tool_offset) * ET.rz(-np.pi/4),
7575
name='ee',
7676
parent=l6
7777
)
7878

79-
ETlist = [l0, l1, l2, l3, l4, l5, l6, ee]
79+
ets = [l0, l1, l2, l3, l4, l5, l6, ee]
8080

8181
super(Panda, self).__init__(
82-
ETlist,
82+
ets,
8383
name='Panda',
8484
manufacturer='Franka Emika')
8585

‎roboticstoolbox/robot/ELink.py

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
from spatialmath import SE3
88
from spatialmath.base.argcheck import getvector, verifymatrix, isscalar
99
import roboticstoolbox as rp
10+
from roboticstoolbox.robot.ET import ET
1011

1112

1213
class ELink(object):
@@ -44,7 +45,7 @@ class ELink(object):
4445

4546
def __init__(
4647
self,
47-
ET_list=[],
48+
ets=ET(),
4849
name='',
4950
parent=None,
5051
qlim=np.zeros(2),
@@ -63,7 +64,12 @@ def __init__(
6364
self.STATIC = 0
6465
self.VARIABLE = 1
6566

66-
self._ets = ET_list
67+
if isinstance(ets, ET):
68+
self._ets = ets
69+
else:
70+
raise TypeError(
71+
'The ets argument must be of type ETS')
72+
6773
self._q_idx = []
6874

6975
self._name = name
@@ -73,7 +79,9 @@ def __init__(
7379
elif parent is None:
7480
parent = []
7581
elif not isinstance(parent, list):
76-
raise TypeError('The parent link must be of type ELink or list of Elink')
82+
raise TypeError(
83+
'The parent link must be of type ELink'
84+
' or list of Elink')
7785

7886
self._parent = parent
7987
self._child = []
@@ -83,8 +91,8 @@ def __init__(
8391

8492
# Initialise joints
8593
for i in range(self.M):
86-
if ET_list[i].jtype is not ET_list[i].STATIC:
87-
ET_list[i].j = len(self._q_idx)
94+
if ets[i].jtype is not ets[i].STATIC:
95+
ets[i].j = len(self._q_idx)
8896
self._q_idx.append(i)
8997

9098
if len(self._q_idx) > 1:
@@ -303,7 +311,7 @@ def __str__(self):
303311
def _copy(self):
304312
# Copy the Link
305313
link = ELink( # noqa
306-
ET_list=self.ets,
314+
ets=self.ets,
307315
qlim=self.qlim,
308316
m=self.m,
309317
r=self.r,
@@ -379,8 +387,16 @@ def A(self, q=None):
379387
else:
380388
T = self.ets[k].T()
381389

390+
print(type(T))
391+
print(T)
392+
393+
if not isinstance(T, SE3):
394+
print(q)
395+
print(self.ets[1].T(1))
396+
382397
tr = tr * T
383398

399+
384400
return tr
385401

386402
def islimit(self, q):

0 commit comments

Comments
 (0)
0