def __getitem__(self, i):
"""
Index or slice an ETS
:param i: the index or slince
:type i: int or slice
:return: Elementary transform (sequence)
:rtype: ETS
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
>>> e[0]
>>> e[1]
>>> e[1:3]
"""
item = ETS()
data = self.data[i] # can be [2] or slice, eg. [3:5]
# ensure that data is always a list
if isinstance(data, list):
item.data = data
else:
item.data = [data]
return item
def substitute_to_z(self):
# substitute all non Z joint transforms according to rules
nchanges = 0
out = ETS()
for e in self:
if e.isjoint:
out *= e
else:
# do a substitution
if e.axis == 'Rx':
new = ETS.ry(pi2) * ETS.rz(e.eta) * ETS.ry(-pi2)
elif e.axis == 'Ry':
new = ETS.rx(-pi2) * ETS.rz(e.eta) * ETS.rx(pi2)
elif e.axis == 'tx':
new = ETS.ry(pi2) * ETS.tz(e.eta) * ETS.ry(-pi2)
elif e.axis == 'ty':
new = ETS.rx(-pi2) * ETS.tz(e.eta) * ETS.rx(pi2)
else:
out *= e
continue
out *= new
nchanges += 1
self.data = out.data
return nchanges
def add(this, that):
if this.isjoint and not that.isjoint:
out = ETS(this)
if out.eta is None:
out.eta = that.eta
else:
out.eta += that.eta
elif not this.isjoint and that.isjoint:
out = ETS(that)
if out.eta is None:
out.eta = this.eta
else:
out.eta += this.eta
else:
raise ValueError('both ET cannot be joint variables')
return out
def pop(self, i=-1):
"""
Pop value
:param i: item in the list to pop, default is last
:type i: int
:return: the popped value
:rtype: instance of same type
:raises IndexError: if there are no values to pop
Removes a value from the value list and returns it. The original
instance is modified.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
>>> tail = e.pop()
>>> tail
>>> e
"""
item = ETS()
item.data = [super().pop(i)]
return item
def __mul__(self, rest):
"""
Overloaded ``*`` operator
:return: [description]
:rtype: [type]
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e1 = ETS.rz()
>>> len(e1)
>>> e2= ETS.tx(2)
>>> len(e2)
>>> e = e1 * e2
>>> len(e)
.. note:: The ``*`` operator implies composition, but actually the
result is a new ETS instance that contains the concatenation of
the left and right operands in an internal list. In this example
we see the length of the product is 2.
"""
prod = ETS()
prod.data = self.data + rest.data
return prod
def inv(self):
r"""
Inverse of ETS
:return: [description]
:rtype: ETS instance
The inverse of a given ETS. It is computed as the inverse of the
individual ETs in the reverse order.
.. math::
(\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} )
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz(j=2) * ETS.tx(1) * ETS.rx(j=3,flip=True) * ETS.tx(1)
>>> print(e)
>>> print(e.inv())
>>> q = [1,2,3,4]
>>> print(e.eval(q) * e.inv().eval(q))
.. note:: It is essential to use explicit joint indices to account for
the reversed order of the transforms.
""" # noqa
inv = ETS()
for ns in reversed(self.data):
# get the namespace from the list
# clone it, and invert the elements to create an inverse
nsi = copy.copy(ns)
if nsi.joint:
nsi.flip ^= True # toggle flip status
elif nsi.axis[0] == 'C':
nsi.T = trinv(nsi.T)
elif nsi.eta is not None:
nsi.T = trinv(nsi.T)
nsi.eta = -nsi.eta
et = ETS() # create a new ETS instance
et.data = [nsi] # set it's data from the dict
inv *= et
return inv
def inv(self):
r"""
Inverse of ETS
:return: [description]
:rtype: ETS instance
The inverse of a given ETS. It is computed as the inverse of the
individual ETs in the reverse order.
.. math::
(\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} )
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> e = ETS.rz(j=2) * ETS.tx(1) * ETS.rx(j=3,flip=True) * ETS.tx(1)
>>> print(e)
>>> print(e.inv())
>>> q = [1,2,3,4]
>>> print(e.eval(q) * e.inv().eval(q))
.. note:: It is essential to use explicit joint indices to account for
the reversed order of the transforms.
"""
inv = ETS()
for e in reversed(self.data):
# get the named tuple from the list, and convert to a dict
etdict = e._asdict()
# update the dict to make this an inverse
if etdict['joint']:
etdict['flip'] ^= True # toggle flip status
elif etdict['axis'][0] == 'C':
etdict['T'] = trinv(etdict['T'])
elif etdict['eta'] is not None:
etdict['T'] = trinv(etdict['T'])
etdict['eta'] = -etdict['eta']
et = ETS() # create a new ETS instance
et.data = [self.ets_tuple(**etdict)] # set it's data from the dict
inv *= et
return inv
def SE3(cls, t, rpy=None, tol=100):
"""
Convert an SE3 to an ETS
:param t: Translation vector, or an SE(3) matrix
:type t: array_like(3) or SE3 instance
:param rpy: roll-pitch-yaw angles in XYZ order
:type rpy: array_like(3)
:param tol: Elements small than this many eps are considered as
being zero, defaults to 100
:type tol: int, optional
:return: ET sequence
:rtype: ETS instance
Create an ETS from the non-zero translational and rotational
components.
- ``SE3(t, rpy)`` convert translation ``t`` and rotation given by XYZ
roll-pitch-yaw angles ``rpy`` into an ETS.
- ``SE3(X)`` as above but convert from an SE3 instance ``X``.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> ETS.SE3(SE3(1,2,3))
>>> ETS.SE3(SE3.Rx(90, 'deg'))
.. warning:: ``SE3.rpy()`` is used to determine rotation about the x-,
y- and z-axes. For a y-axis rotation with magnitude greater than
180° this will result in a non-minimal representation with non-zero
amounts of x- and z-axis rotation.
:seealso: :func:`~SE3.rpy`
"""
if isinstance(t, SE3):
T = t
t = removesmall(T.t, tol)
rpy = removesmall(T.rpy(order='zyx'))
ets = ETS()
if t[0] != 0:
ets *= ETS.tx(t[0])
if t[1] != 0:
ets *= ETS.ty(t[1])
if t[2] != 0:
ets *= ETS.tz(t[2])
if rpy is not None:
if rpy[2] != 0:
ets *= ETS.rz(rpy[2])
if rpy[1] != 0:
ets *= ETS.ry(rpy[1])
if rpy[0] != 0:
ets *= ETS.rx(rpy[0])
return ets
def compile(self):
"""
Compile an ETS
:return: optimised ETS
:rtype: ETS
Perform constant folding for faster evaluation. Consecutive constant
ETs are compounded, leading to a constant ET which is denoted by
``Ci`` when displayed.
Example:
.. runblock:: pycon
>>> from roboticstoolbox import ETS
>>> robot = rtb.models.ETS.Panda()
>>> ets = robot.ets()
>>> ets
>>> ets.compile()
:seealso: :func:`isconstant`
"""
const = None
ets = ETS()
for et in self:
if et.isjoint:
# a joint
if const is not None:
# flush the constant
if not iseye(const):
ets *= ETS._CONST(const)
const = None
ets *= et # emit the joint ET
else:
# not a joint
if const is None:
const = et.T()
else:
const = const @ et.T()
if const is not None:
# flush the constant, tool transform
if not iseye(const):
ets *= ETS._CONST(const)
return ets
def eliminate_y(self):
nchanges = 0
out = ETS()
jointyet = False
def eliminate(prev, this):
if this.isjoint or prev.isjoint:
return None
new = None
if prev.axis == 'Rx' and this.axis == 'ty': # RX.TY -> TZ.RX
new = ETS.tx(prev.eta) * prev
elif prev.axis == 'Rx' and this.axis == 'tz': # RX.TZ -> TY.RX
new = ETS.ty(-prev.eta) * prev
elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TX-> TZ.RY
new = ETS.tz(-prev.eta) * prev
elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TZ-> TX.RY
new = ETS.tx(prev.eta) * prev
elif prev.axis == 'ty' and this.axis == 'Rx': # TY.RX -> RX.TZ
new = this * ETS.tz(-this.eta)
elif prev.axis == 'tx' and this.axis == 'Rz': # TX.RZ -> RZ.TY
new = this * ETS.tz(this.eta)
elif prev.axis == 'Ry' and this.axis == 'Rx': # RY(Q).RX -> RX.RZ(-Q)
new = this * ETS.Rz(-prev.eta)
elif prev.axis == 'Rx' and this.axis == 'Ry': # RX.RY -> RZ.RX
new = ETS.Rz(this.eta) * prev
elif prev.axis == 'Rz' and this.axis == 'Rx': # RZ.RX -> RX.RY
new = this * ETS.Ry(this.eta)
return new
for i in range(len(self)):
this = self[i]
jointyet = this.isjoint
if i == 0 or not jointyet:
continue
prev = self[i - 1]
new = eliminate(prev, this)
if new is not None:
self[i - 1:i] = new
nchanges += 1
return nchanges
def merge(self):
def canmerge(prev, this):
return prev.axis == this.axis and not (prev.isjoint
and this.isjoint)
out = ETS()
while len(self.data) > 0:
this = self.pop(0)
if len(self.data) > 0:
next = self[0]
if canmerge(this, next):
new = DHFactor.add(this, next)
out *= new
self.pop(0) # remove next from the queue
print(f"Merging {this * next} to {new}")
else:
out *= this
self.data = out.data
def __imul__(self, rest):
prod = ETS()
prod.data = self.data + rest.data
return prod
from roboticstoolbox import ETS
e = ETS.rz() * ETS.tx(1) * ETS.rz() * ETS.tx(1)
print(e.eval([0, 0]))
print(e.eval([90, -90], 'deg'))
a = e.pop()
print(a)
from spatialmath.base import symbol
theta, d = symbol('theta, d')
e = ETS.rx(theta) * ETS.tx(2) * ETS.rx(45, 'deg') * ETS.ry(0.2) * ETS.ty(d)
print(e)
e = ETS()
e *= ETS.rx()
e *= ETS.tz()
print(e)
print(e.__str__("θ{0}"))
print(e.__str__("θ{1}"))
e = ETS.rx() * ETS._CONST(SE3()) * ETS.tx(0.3)
print(e)
l1 = 0.672
l2 = -0.2337
l3 = 0.4318
l4 = 0.0203
l5 = 0.0837