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 __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
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):
deg = np.pi/180
mm = 1e-3
tool_offset = (103)*mm
l0 = ELink(
ETS.tz(0.333) * ETS.rz(),
name='link0',
parent=None
)
l1 = ELink(
ETS.rx(-90*deg) * ETS.rz(),
name='link1',
parent=l0
)
l2 = ELink(
ETS.rx(90*deg) * ETS.tz(0.316) * ETS.rz(),
name='link2',
parent=l1
)
l3 = ELink(
ETS.tx(0.0825) * ETS.rx(90, 'deg') * ETS.rz(),
name='link3',
parent=l2
)
l4 = ELink(
ETS.tx(-0.0825) * ETS.rx(-90, 'deg') * ETS.tz(0.384) * ETS.rz(),
name='link4',
parent=l3
)
l5 = ELink(
ETS.rx(90, 'deg') * ETS.rz(),
name='link5',
parent=l4
)
l6 = ELink(
ETS.tx(0.088) * ETS.rx(90, 'deg') * ETS.tz(0.107) * ETS.rz(),
name='link6',
parent=l5
)
ee = ELink(
ETS.tz(tool_offset) * ETS.rz(-np.pi/4),
name='ee',
parent=l6
)
elinks = [l0, l1, l2, l3, l4, l5, l6, ee]
super(Panda, self).__init__(
elinks,
name='Panda',
manufacturer='Franka Emika')
self.addconfiguration(
"qz", np.array([0, 0, 0, 0, 0, 0, 0]))
self.addconfiguration(
"qr", np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]))
def __init__(self):
deg = np.pi / 180
mm = 1e-3
tool_offset = (103) * mm
link_length = 0.5
l0 = ELink(ETS.tz(link_length) * ETS.rz(), name='link0', parent=None)
l1 = ELink(ETS.ry(), name='link1', parent=l0)
l2 = ELink(ETS.tz(link_length) * ETS.rz(), name='link2', parent=l1)
l3 = ELink(ETS.ry(), name='link3', parent=l2)
l4 = ELink(ETS.tz(link_length) * ETS.rz(), name='link4', parent=l3)
l5 = ELink(ETS.ry(), name='link5', parent=l4)
l6 = ELink(ETS.tx(link_length) * ETS.rz(), name='link6', parent=l5)
ee = ELink(ETS.tz(-link_length), name='ee', parent=l6)
elinks = [l0, l1, l2, l3, l4, l5, l6, ee]
# elinks = [l0, l1, l2, l3, l4, l5, ee]
super(GenericSeven, self).__init__(elinks,
name='Generic Seven',
manufacturer="Jesse's Imagination")
def __init__(self):
deg = np.pi / 180
mm = 1e-3
tool_offset = 1
# trunk of the tree
l0 = ELink(ETS.rz(), name='link0', jindex=0, parent=None)
l1 = ELink(ETS.tx(1) * ETS.rz(), name='link1', jindex=1, parent=l0)
# branch 1
l2 = ELink(ETS.tx(1) * ETS.rz(), name='link2a', jindex=2, parent=l1)
l3 = ELink(ETS.tx(1) * ETS.rz(), name='link3a', jindex=3, parent=l2)
eea = ELink(ETS.tz(tool_offset), name='eea', parent=l3)
# branch 2
l4 = ELink(ETS.tx(1) * ETS.rz(), name='link2b', jindex=4, parent=l1)
l5 = ELink(ETS.tx(1) * ETS.rz(), name='link3b', jindex=5, parent=l4)
eeb = ELink(ETS.tz(tool_offset), name='eeb', parent=l5)
elinks = [l0, l1, l2, l3, l4, l5, eea, eeb]
super().__init__(elinks, name='Planar-Y')
self.addconfiguration("qz", [0, 0, 0, 0, 0, 0])
self.addconfiguration("qy", [0, 0, np.pi / 4, 0, -np.pi / 4, 0])
def __init__(self):
deg = np.pi / 180
mm = 1e-3
tool_offset = (103) * mm
b0 = ELink(v=ETS.rz(), name='base0', parent=None)
b1 = ELink(v=ETS.tx(), name='base1', parent=b0)
l0 = ELink(ETS.tz(0.333), ETS.rz(), name='link0', parent=b1)
l1 = ELink(ETS.rx(-90 * deg), ETS.rz(), name='link1', parent=l0)
l2 = ELink(ETS.rx(90 * deg) * ETS.tz(0.316),
ETS.rz(),
name='link2',
parent=l1)
l3 = ELink(ETS.tx(0.0825) * ETS.rx(90 * deg),
ETS.rz(),
name='link3',
parent=l2)
l4 = ELink(ETS.tx(-0.0825) * ETS.rx(-90 * deg) * ETS.tz(0.384),
ETS.rz(),
name='link4',
parent=l3)
l5 = ELink(ETS.rx(90 * deg), ETS.rz(), name='link5', parent=l4)
l6 = ELink(ETS.tx(0.088) * ETS.rx(90 * deg) * ETS.tz(0.107),
ETS.rz(),
name='link6',
parent=l5)
ee = ELink(ETS.tz(tool_offset) * ETS.rz(-np.pi / 4),
name='ee',
parent=l6)
elinks = [b0, b1, l0, l1, l2, l3, l4, l5, l6, ee]
super(Frankie, self).__init__(elinks,
name='Frankie',
manufacturer='Franka Emika, Omron',
keywords=('mobile', ))
self.addconfiguration("qz", np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]))
self.addconfiguration(
"qr",
np.array([0, 0, 0, -90, -90, 90, 0, -90, 90]) * deg)