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
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 = (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)
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]))