def __init__(
self,
ets=ETS(),
v=None,
jindex=None,
**kwargs):
# process common options
super().__init__(**kwargs)
# check we have an ETS
if not isinstance(ets, ETS):
raise TypeError(
'The ets argument must be of type ETS')
self._ets = ets
if v is None and len(ets) > 0 and ets[-1].isjoint:
v = ets.pop()
if jindex is not None:
v.jindex = jindex
elif jindex is None and v.jindex is not None:
jindex = v.jindex
# Initialise the static transform representing the constant
# component of the ETS
self._init_Ts()
# Check the variable joint
if v is None:
self._joint = False
elif not isinstance(v, ETS):
raise TypeError('v must be of type ETS')
elif not v[0].isjoint:
raise ValueError('v must be a variable ETS')
elif len(v) > 1:
raise ValueError(
"An elementary link can only have one joint variable")
else:
self._joint = True
self._v = v
# Private variable, can be written to but never replaced!
# The c will adjust the inside of this array with a reference
# to this specific array. If replaced --> segfault
self._fk = np.eye(4)
self._init_fknm()
def ets(self):
ets = ETS()
if self.mdh:
# MDH format: a alpha theta d
if self.a != 0:
ets *= ETS.tx(self.a)
if self.alpha != 0:
ets *= ETS.rx(self.alpha)
if self.isrevolute:
if self.offset != 0:
ets *= ETS.rz(self.offset)
ets *= ETS.rz(flip=self.flip) # joint
if self.d != 0:
ets *= ETS.tz(self.d)
elif self.isprismatic:
if self.theta != 0:
ets *= ETS.rz(self.theta)
if self.offset != 0:
ets *= ETS.tz(self.offset)
ets *= ETS.tz(flip=self.flip) # joint
else:
# DH format: theta d a alpha
if self.isrevolute:
if self.offset != 0:
ets *= ETS.rz(self.offset)
ets *= ETS.rz(flip=self.flip)
if self.d != 0:
ets *= ETS.tz(self.d)
elif self.isprismatic:
if self.theta != 0:
ets *= ETS.rz(self.theta)
if self.offset != 0:
ets *= ETS.tz(self.offset)
ets *= ETS.tz(flip=self.flip)
if self.a != 0:
ets *= ETS.tx(self.a)
if self.alpha != 0:
ets *= ETS.rx(self.alpha)
return ets
def __init__(self, ets=ETS(), v=None, parent=None, jindex=None, **kwargs):
# process common options
super(ELink, self).__init__(**kwargs)
# check we have an ETS
if isinstance(ets, ETS):
self._ets = ets
else:
raise TypeError('The ets argument must be of type ETS')
if v is None and len(ets) > 0 and ets[-1].isjoint:
v = ets.pop()
if jindex is not None:
v.jindex = jindex
elif jindex is None and v.jindex is not None:
jindex = v.jindex
# TODO simplify this logic, can be ELink class or None
if isinstance(parent, list):
raise TypeError('Only one parent link can be present')
elif not isinstance(parent, ELink) and parent is not None:
raise TypeError('Parent must be of type ELink')
self._parent = parent
self._child = []
self._joint_name = None
self._jindex = jindex
# Initialise the static transform representing the constant
# component of the ETS
self._init_Ts()
# Check the variable joint
if v is None:
self._joint = False
elif not isinstance(v, ETS):
raise TypeError('v must be of type ETS')
elif not v[0].isjoint:
raise ValueError('v must be a variable ETS')
elif len(v) > 1:
raise ValueError(
"An elementary link can only have one joint variable")
else:
self._joint = True
self._v = v