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 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 __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]))