Skip to content

Commit c362ef7

Browse files
jhavljhavl
committed
ETS variable change
1 parent 1dabde5 commit c362ef7

File tree

7 files changed

+195
-173
lines changed

7 files changed

+195
-173
lines changed

‎roboticstoolbox/backend/urdf/urdf.py

Lines changed: 14 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1644,6 +1644,7 @@ def __init__(self, name, links, joints=None,
16441644
T = sm.SE3(j.origin)
16451645
trans = T.t
16461646
rot = j.rpy
1647+
v = None
16471648
# print(trans)
16481649
# print(rot)
16491650

@@ -1668,36 +1669,36 @@ def __init__(self, name, links, joints=None,
16681669
if j.joint_type == 'revolute' or \
16691670
j.joint_type == 'continuous': # pragma nocover
16701671
if j.axis[0] == 1:
1671-
ets = ets * rp.ETS.rx()
1672+
v = rp.ETS.rx()
16721673
elif j.axis[0] == -1:
16731674
ets = ets * rp.ETS.ry(np.pi)
1674-
ets = ets * rp.ETS.rx()
1675+
v = rp.ETS.rx()
16751676
elif j.axis[1] == 1:
1676-
ets = ets * rp.ETS.ry()
1677+
v = rp.ETS.ry()
16771678
elif j.axis[1] == -1:
16781679
ets = ets * rp.ETS.rz(np.pi)
1679-
ets = ets * rp.ETS.ry()
1680+
v = rp.ETS.ry()
16801681
elif j.axis[2] == 1:
1681-
ets = ets * rp.ETS.rz()
1682+
v = rp.ETS.rz()
16821683
elif j.axis[2] == -1:
16831684
ets = ets * rp.ETS.rx(np.pi)
1684-
ets = ets * rp.ETS.rz()
1685+
v = rp.ETS.rz()
16851686
elif j.joint_type == 'prismatic': # pragma nocover
16861687
if j.axis[0] == 1:
1687-
ets = ets * rp.ETS.tx()
1688+
v = rp.ETS.tx()
16881689
elif j.axis[0] == -1:
16891690
ets = ets * rp.ETS.ry(np.pi)
1690-
ets = ets * rp.ETS.tx()
1691+
v = rp.ETS.tx()
16911692
elif j.axis[1] == 1:
1692-
ets = ets * rp.ETS.ty()
1693+
v = rp.ETS.ty()
16931694
elif j.axis[1] == -1:
16941695
ets = ets * rp.ETS.rz(np.pi)
1695-
ets = ets * rp.ETS.ty()
1696+
v = rp.ETS.ty()
16961697
elif j.axis[2] == 1:
1697-
ets = ets * rp.ETS.tz()
1698+
v = rp.ETS.tz()
16981699
elif j.axis[2] == -1:
16991700
ets = ets * rp.ETS.rx(np.pi)
1700-
ets = ets * rp.ETS.tz()
1701+
v = rp.ETS.tz()
17011702

17021703
try:
17031704
qlim = [j.limit.lower, j.limit.upper]
@@ -1707,6 +1708,7 @@ def __init__(self, name, links, joints=None,
17071708
elinks.append(
17081709
rp.ELink(
17091710
ets,
1711+
v,
17101712
name=j.name,
17111713
qlim=qlim
17121714
)

‎roboticstoolbox/models/ETS/Frankie.py

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

4242
b0 = ELink(
43-
ETS.rz(),
43+
v=ETS.rz(),
4444
name='base0',
4545
parent=None
4646
)
4747

4848
b1 = ELink(
49-
ETS.tx(),
49+
v=ETS.tx(),
5050
name='base1',
5151
parent=b0
5252
)
5353

5454
l0 = ELink(
55-
ETS.tz(0.333) * ETS.rz(),
55+
ETS.tz(0.333),
56+
ETS.rz(),
5657
name='link0',
5758
parent=b1
5859
)
5960

6061
l1 = ELink(
61-
ETS.rx(-90*deg) * ETS.rz(),
62+
ETS.rx(-90*deg),
63+
ETS.rz(),
6264
name='link1',
6365
parent=l0
6466
)
6567

6668
l2 = ELink(
67-
ETS.rx(90*deg) * ETS.tz(0.316) * ETS.rz(),
69+
ETS.rx(90*deg) * ETS.tz(0.316),
70+
ETS.rz(),
6871
name='link2',
6972
parent=l1
7073
)
7174

7275
l3 = ELink(
73-
ETS.tx(0.0825) * ETS.rx(90*deg) * ETS.rz(),
76+
ETS.tx(0.0825) * ETS.rx(90*deg),
77+
ETS.rz(),
7478
name='link3',
7579
parent=l2
7680
)
7781

7882
l4 = ELink(
79-
ETS.tx(-0.0825) * ETS.rx(-90*deg) * ETS.tz(0.384) * ETS.rz(),
83+
ETS.tx(-0.0825) * ETS.rx(-90*deg) * ETS.tz(0.384),
84+
ETS.rz(),
8085
name='link4',
8186
parent=l3
8287
)
8388

8489
l5 = ELink(
85-
ETS.rx(90*deg) * ETS.rz(),
90+
ETS.rx(90*deg),
91+
ETS.rz(),
8692
name='link5',
8793
parent=l4
8894
)
8995

9096
l6 = ELink(
91-
ETS.tx(0.088) * ETS.rx(90*deg) * ETS.tz(0.107) * ETS.rz(),
97+
ETS.tx(0.088) * ETS.rx(90*deg) * ETS.tz(0.107),
98+
ETS.rz(),
9299
name='link6',
93100
parent=l5
94101
)

‎roboticstoolbox/models/ETS/Panda.py

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

3131
l0 = ELink(
32-
ETS.tz(0.333) * ETS.rz(),
32+
ETS.tz(0.333),
33+
ETS.rz(),
3334
name='link0',
3435
parent=None
3536
)
3637

3738
l1 = ELink(
38-
ETS.rx(-90*deg) * ETS.rz(),
39+
ETS.rx(-90*deg),
40+
ETS.rz(),
3941
name='link1',
4042
parent=l0
4143
)
4244

4345
l2 = ELink(
44-
ETS.rx(90*deg) * ETS.tz(0.316) * ETS.rz(),
46+
ETS.rx(90*deg) * ETS.tz(0.316),
47+
ETS.rz(),
4548
name='link2',
4649
parent=l1
4750
)
4851

4952
l3 = ELink(
50-
ETS.tx(0.0825) * ETS.rx(90*deg) * ETS.rz(),
53+
ETS.tx(0.0825) * ETS.rx(90*deg),
54+
ETS.rz(),
5155
name='link3',
5256
parent=l2
5357
)
5458

5559
l4 = ELink(
56-
ETS.tx(-0.0825) * ETS.rx(-90*deg) * ETS.tz(0.384) * ETS.rz(),
60+
ETS.tx(-0.0825) * ETS.rx(-90*deg) * ETS.tz(0.384),
61+
ETS.rz(),
5762
name='link4',
5863
parent=l3
5964
)
6065

6166
l5 = ELink(
62-
ETS.rx(90*deg) * ETS.rz(),
67+
ETS.rx(90*deg),
68+
ETS.rz(),
6369
name='link5',
6470
parent=l4
6571
)
6672

6773
l6 = ELink(
68-
ETS.tx(0.088) * ETS.rx(90*deg) * ETS.tz(0.107) * ETS.rz(),
74+
ETS.tx(0.088) * ETS.rx(90*deg) * ETS.tz(0.107),
75+
ETS.rz(),
6976
name='link6',
7077
parent=l5
7178
)

‎roboticstoolbox/robot/ELink.py

Lines changed: 42 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ class ELink(object):
4646
def __init__(
4747
self,
4848
ets=ETS(),
49-
vet=None,
49+
v=None,
5050
name='',
5151
parent=None,
5252
qlim=np.zeros(2),
@@ -71,8 +71,6 @@ def __init__(
7171
raise TypeError(
7272
'The ets argument must be of type ETS')
7373

74-
self._q_idx = []
75-
7674
self._name = name
7775

7876
if isinstance(parent, list):
@@ -89,21 +87,31 @@ def __init__(
8987
self._M = len(self._ets)
9088

9189
# Initialise joints
92-
for i in range(self.M):
93-
if ets[i].jtype is not ets[i].STATIC:
94-
ets[i].j = len(self._q_idx)
95-
self._q_idx.append(i)
90+
if isinstance(ets, ETS):
91+
self._Ts = SE3()
92+
for i in range(self.M):
93+
if ets[i].jtype is not ets[i].STATIC:
94+
raise ValueError('The transforms in ets must be constant')
95+
96+
self._Ts *= ets[i].T()
97+
98+
elif isinstance(ets, SE3):
99+
self._Ts = ets
96100

97-
if len(self._q_idx) > 1:
101+
# Check the variable joint
102+
if v is None:
103+
self._jtype = self.STATIC
104+
elif not isinstance(v, ETS):
105+
raise TypeError('v must be of type ETS')
106+
elif v[0].jtype is v[0].STATIC:
107+
raise ValueError('v must be a variable ETS')
108+
elif len(v) > 1:
98109
raise ValueError(
99110
"An elementary link can only have one joint variable")
100-
elif len(self._q_idx) == 0:
101-
self._jtype = self.STATIC
102-
self._q_idx = None
103111
else:
104112
self._jtype = self.VARIABLE
105-
self._q_idx = self._q_idx[0]
106113

114+
self._v = v
107115
self.qlim = qlim
108116
self.geometry = geometry
109117
self.collision = collision
@@ -120,6 +128,14 @@ def __init__(
120128
def __repr__(self):
121129
return self.name
122130

131+
@property
132+
def v(self):
133+
return self._v
134+
135+
@property
136+
def Ts(self):
137+
return self._Ts
138+
123139
@property
124140
def collision(self):
125141
return self._collision
@@ -192,10 +208,6 @@ def Tc(self):
192208
def G(self):
193209
return self._G
194210

195-
@property
196-
def q_idx(self):
197-
return self._q_idx
198-
199211
@collision.setter
200212
def collision(self, coll):
201213
new_coll = []
@@ -376,22 +388,25 @@ def A(self, q=None):
376388
377389
"""
378390

379-
j = 0
380-
tr = SE3()
391+
# j = 0
392+
# tr = SE3()
381393

382-
if self.q_idx is not None and q is None:
394+
if self.jtype == self.VARIABLE and q is None:
383395
raise ValueError("q is required for variable joints")
384396

385-
for k in range(self.M):
386-
if self.ets[k].jtype == self.ets[k].VARIABLE:
387-
T = self.ets[k].T(q)
388-
j += 1
389-
else:
390-
T = self.ets[k].T()
397+
# for k in range(self.M):
398+
# if self.ets[k].jtype == self.ets[k].VARIABLE:
399+
# T = self.ets[k].T(q)
400+
# j += 1
401+
# else:
402+
# T = self.ets[k].T()
391403

392-
tr = tr * T
404+
# tr = tr * T
393405

394-
return tr
406+
if self.v is not None:
407+
return self.Ts * self.v.T(q)
408+
else:
409+
return self.Ts
395410

396411
def islimit(self, q):
397412
"""

0 commit comments

Comments
 (0)