Skip to content

Commit e221ff1

Browse files
petercorkepetercorke
committed
Rework to support DHFactor
1 parent 2abc94d commit e221ff1

File tree

1 file changed

+115
-89
lines changed

1 file changed

+115
-89
lines changed

‎roboticstoolbox/robot/ETS.py

Lines changed: 115 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -18,29 +18,33 @@ class SuperETS(UserList, ABC):
1818
# ets_tuple = namedtuple('ETS3', 'eta axis_func axis joint T jindex flip')
1919

2020
def __init__(
21-
self, axis_func=None, axis=None, eta=None,
21+
self, axis=None, eta=None, axis_func=None,
2222
unit='rad', j=None, flip=False):
2323

2424
super().__init__() # init UserList superclass
2525

26-
if axis_func is None and axis is None and eta is None:
26+
if axis is None and eta is None and axis_func is None:
2727
# ET()
2828
# create instance with no values
2929
self.data = []
3030
return
3131

32-
elif isinstance(axis_func, ETS):
32+
elif isinstance(axis, ETS):
3333
# copy constructor
34-
e = axis_func
35-
axis_func = e.axis_func
36-
axis = e.axis
37-
# et = e.eta
38-
j = e.jindex
39-
flip = e.isflip
40-
joint = e.isjoint
41-
T = e.T
42-
43-
elif callable(axis_func):
34+
# e = axis_func
35+
# axis_func = e.axis_func
36+
# axis = e.axis
37+
# # et = e.eta
38+
# j = e.jindex
39+
# flip = e.isflip
40+
# joint = e.isjoint
41+
# T = e.T
42+
self.data = [copy.copy(axis.data[0])]
43+
return
44+
45+
if axis in ('Rx', 'Ry', 'Rz', 'tx', 'ty', 'tz'):
46+
# it's a regular axis
47+
4448
if eta is None:
4549
# no value, it's a variable joint
4650
if unit != 'rad':
@@ -51,6 +55,8 @@ def __init__(
5155

5256
else:
5357
# constant value specified
58+
if not callable(axis_func):
59+
raise ValueError('axis func must be callable')
5460
joint = False
5561
eta = getunit(eta, unit)
5662
T = axis_func(eta)
@@ -83,7 +89,7 @@ def __init__(
8389
joint = False
8490
axis_func = None
8591
else:
86-
raise ValueError('axis_func must be callable or ndarray')
92+
raise ValueError('bad axis specified')
8793

8894
# Save all the params in a named tuple
8995
e = SimpleNamespace(eta=eta, axis_func=axis_func, axis=axis, joint=joint, T=T, jindex=j, flip=flip)
@@ -94,10 +100,10 @@ def __init__(
94100
@property
95101
def eta(self):
96102
"""
97-
The transform constant
103+
Get the transform constant
98104
99105
:return: The constant η if set
100-
:rtype: float or None
106+
:rtype: float, symbolic or None
101107
102108
Example:
103109
@@ -116,6 +122,19 @@ def eta(self):
116122
"""
117123
return self.data[0].eta
118124

125+
@eta.setter
126+
def eta(self, value):
127+
"""
128+
Set the transform constant
129+
130+
:param value: The transform constant η
131+
:type value: float, symbolic or None
132+
133+
.. note:: No unit conversions are applied, it is assumed to be in
134+
radians.
135+
"""
136+
self.data[0].eta = value
137+
119138
@property
120139
def axis_func(self):
121140
return self.data[0].axis_func
@@ -895,7 +914,7 @@ def rx(cls, eta=None, unit='rad', **kwargs):
895914
:seealso: :func:`ETS`, :func:`isrevolute`
896915
:SymPy: supported
897916
"""
898-
return cls(trotx, axis='Rx', eta=eta, unit=unit, **kwargs)
917+
return cls(axis='Rx', eta=eta, axis_func=trotx, unit=unit, **kwargs)
899918

900919
@classmethod
901920
def ry(cls, eta=None, unit='rad', **kwargs):
@@ -922,7 +941,7 @@ def ry(cls, eta=None, unit='rad', **kwargs):
922941
:seealso: :func:`ETS`, :func:`isrevolute`
923942
:SymPy: supported
924943
"""
925-
return cls(troty, axis='Ry', eta=eta, unit=unit, **kwargs)
944+
return cls(axis='Ry', eta=eta, axis_func=troty, unit=unit, **kwargs)
926945

927946
@classmethod
928947
def rz(cls, eta=None, unit='rad', **kwargs):
@@ -949,7 +968,7 @@ def rz(cls, eta=None, unit='rad', **kwargs):
949968
:seealso: :func:`ETS`, :func:`isrevolute`
950969
:SymPy: supported
951970
"""
952-
return cls(trotz, axis='Rz', eta=eta, unit=unit, **kwargs)
971+
return cls(axis='Rz', eta=eta, axis_func=trotz, unit=unit, **kwargs)
953972

954973
@classmethod
955974
def tx(cls, eta=None, **kwargs):
@@ -984,7 +1003,7 @@ def axis_func(eta):
9841003
[0, 0, 0, 1]
9851004
])
9861005

987-
return cls(axis_func, axis='tx', eta=eta, **kwargs)
1006+
return cls(axis='tx', axis_func=axis_func, eta=eta, **kwargs)
9881007

9891008
@classmethod
9901009
def ty(cls, eta=None, **kwargs):
@@ -1017,7 +1036,7 @@ def axis_func(eta):
10171036
[0, 0, 0, 1]
10181037
])
10191038

1020-
return cls(axis_func, axis='ty', eta=eta, **kwargs)
1039+
return cls(axis='ty', eta=eta, axis_func=axis_func, **kwargs)
10211040

10221041
# return cls(SE3.Ty, axis='ty', eta=eta)
10231042

@@ -1052,7 +1071,7 @@ def axis_func(eta):
10521071
[0, 0, 0, 1]
10531072
])
10541073

1055-
return cls(axis_func, axis='tz', eta=eta, **kwargs)
1074+
return cls(axis='tz', axis_func=axis_func, eta=eta, **kwargs)
10561075

10571076
def jacob0(self, q=None, T=None):
10581077
r"""
@@ -1351,8 +1370,8 @@ def r(cls, eta=None, unit='rad', **kwargs):
13511370
13521371
:seealso: :func:`ETS`, :func:`isrevolute`
13531372
"""
1354-
return cls(
1355-
lambda theta: trot2(theta), axis='R', eta=eta, unit=unit, **kwargs)
1373+
return cls(axis='R', eta=eta,
1374+
axis_func=lambda theta: trot2(theta), unit=unit, **kwargs)
13561375

13571376
@classmethod
13581377
def tx(cls, eta=None, **kwargs):
@@ -1376,7 +1395,8 @@ def tx(cls, eta=None, **kwargs):
13761395
13771396
:seealso: :func:`ETS`, :func:`isprismatic`
13781397
"""
1379-
return cls(lambda x: transl2(x, 0), axis='tx', eta=eta, **kwargs)
1398+
return cls(axis='tx', eta=eta,
1399+
axis_func=lambda x: transl2(x, 0), **kwargs)
13801400

13811401
@classmethod
13821402
def ty(cls, eta=None, **kwargs):
@@ -1400,72 +1420,78 @@ def ty(cls, eta=None, **kwargs):
14001420
14011421
:seealso: :func:`ETS`
14021422
"""
1403-
return cls(lambda y: transl2(0, y), axis='ty', eta=eta, **kwargs)
1423+
return cls(axis='ty', eta=eta,
1424+
axis_func=lambda y: transl2(0, y), **kwargs)
14041425

14051426

14061427
if __name__ == "__main__":
14071428

1408-
print(ETS.rx(0.2))
1409-
print(ETS.rx(45, 'deg'))
1410-
print(ETS.tz(0.75))
1411-
e = ETS.rx(45, 'deg') * ETS.tz(0.75)
1412-
print(e)
1413-
print(e.eval())
1414-
1415-
from roboticstoolbox import ETS
1416-
e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
1417-
print(e.eval([0, 0]))
1418-
print(e.eval([90, -90], 'deg'))
1419-
a = e.pop()
1420-
print(a)
1421-
1422-
from spatialmath.base import symbol
1423-
1424-
theta, d = symbol('theta, d')
1425-
1426-
e = ETS.rx(theta) * ETS.tx(2) * ETS.rx(45, 'deg') * ETS.ry(0.2) * ETS.ty(d)
1427-
print(e)
1428-
1429-
e = ETS()
1430-
e *= ETS.rx()
1431-
e *= ETS.tz()
1429+
# print(ETS.rx(0.2))
1430+
# print(ETS.rx(45, 'deg'))
1431+
# print(ETS.tz(0.75))
1432+
# e = ETS.rx(45, 'deg') * ETS.tz(0.75)
1433+
# print(e)
1434+
# print(e.eval())
1435+
1436+
# from roboticstoolbox import ETS
1437+
# e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
1438+
# print(e.eval([0, 0]))
1439+
# print(e.eval([90, -90], 'deg'))
1440+
# a = e.pop()
1441+
# print(a)
1442+
1443+
# from spatialmath.base import symbol
1444+
1445+
# theta, d = symbol('theta, d')
1446+
1447+
# e = ETS.rx(theta) * ETS.tx(2) * ETS.rx(45, 'deg') * ETS.ry(0.2) * ETS.ty(d)
1448+
# print(e)
1449+
1450+
# e = ETS()
1451+
# e *= ETS.rx()
1452+
# e *= ETS.tz()
1453+
# print(e)
1454+
1455+
# print(e.__str__("θ{0}"))
1456+
# print(e.__str__("θ{1}"))
1457+
1458+
# e = ETS.rx() * ETS._CONST(SE3()) * ETS.tx(0.3)
1459+
# print(e)
1460+
1461+
# l1 = 0.672
1462+
# l2 = -0.2337
1463+
# l3 = 0.4318
1464+
# l4 = 0.0203
1465+
# l5 = 0.0837
1466+
# l6 = 0.4318
1467+
1468+
# e = ETS.tz(l1) * ETS.rz() * ETS.ry() * ETS.ty(l2) * ETS.tz(l3) * ETS.ry() \
1469+
# * ETS.tx(l4) * ETS.ty(l5) * ETS.tz(l6) * ETS.rz() * ETS.ry() * ETS.rz()
1470+
# print(e.joints())
1471+
# print(e.config)
1472+
# print(e.eval(np.zeros(6)))
1473+
# ec = e.compile()
1474+
# print(ec)
1475+
# print(ec.eval(np.zeros(6)))
1476+
1477+
# print(ETS.SE3(SE3.Rz(200, 'deg')))
1478+
1479+
# a = ETS.rx()
1480+
# b = ETS(a)
1481+
# print(b)
1482+
# a = ETS.tz()
1483+
# print(b)
1484+
# e = ETS.rz(j=5) * ETS.tx(1) * ETS.rx(j=7, flip=True) * ETS.tx(1)
1485+
# print(e)
1486+
1487+
# print(e.inv())
1488+
1489+
# q = [1, 2, 3, 4, 5, 6, 7, 8]
1490+
# print(e.eval(q))
1491+
# print(e.inv().eval(q))
1492+
# print(e.eval(q) * e.inv().eval(q))
1493+
1494+
1495+
e = ETS.rz() * ETS.tx(-1) * ETS.tx(1) * ETS.rz()
14321496
print(e)
1433-
1434-
print(e.__str__("θ{0}"))
1435-
print(e.__str__("θ{1}"))
1436-
1437-
e = ETS.rx() * ETS._CONST(SE3()) * ETS.tx(0.3)
1438-
print(e)
1439-
1440-
l1 = 0.672
1441-
l2 = -0.2337
1442-
l3 = 0.4318
1443-
l4 = 0.0203
1444-
l5 = 0.0837
1445-
l6 = 0.4318
1446-
1447-
e = ETS.tz(l1) * ETS.rz() * ETS.ry() * ETS.ty(l2) * ETS.tz(l3) * ETS.ry() \
1448-
* ETS.tx(l4) * ETS.ty(l5) * ETS.tz(l6) * ETS.rz() * ETS.ry() * ETS.rz()
1449-
print(e.joints())
1450-
print(e.config)
1451-
print(e.eval(np.zeros(6)))
1452-
ec = e.compile()
1453-
print(ec)
1454-
print(ec.eval(np.zeros(6)))
1455-
1456-
print(ETS.SE3(SE3.Rz(200, 'deg')))
1457-
1458-
a = ETS.rx()
1459-
b = ETS(a)
1460-
print(b)
1461-
a = ETS.tz()
1462-
print(b)
1463-
e = ETS.rz(j=5) * ETS.tx(1) * ETS.rx(j=7, flip=True) * ETS.tx(1)
1464-
print(e)
1465-
1466-
print(e.inv())
1467-
1468-
q = [1, 2, 3, 4, 5, 6, 7, 8]
1469-
print(e.eval(q))
1470-
print(e.inv().eval(q))
1471-
print(e.eval(q) * e.inv().eval(q))
1497+
print(e.compile())

0 commit comments

Comments
 (0)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy