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
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 test_init_elink(self):
link1 = ELink(ETS.rx(), name='link1')
link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(),
name='link2',
parent=link1)
link3 = ELink(ETS.tx(1), name='ee_1', parent=link2)
robot = ERobot([link1, link2, link3])
self.assertEqual(robot.n, 2)
self.assertIsInstance(robot[0], ELink)
self.assertIsInstance(robot[1], ELink)
self.assertIsInstance(robot[2], ELink)
self.assertTrue(robot[0].isrevolute)
self.assertTrue(robot[1].isprismatic)
self.assertFalse(robot[2].isrevolute)
self.assertFalse(robot[2].isprismatic)
self.assertIs(robot[0].parent, None)
self.assertIs(robot[1].parent, robot[0])
self.assertIs(robot[2].parent, robot[1])
self.assertEqual(robot[0].children, [robot[1]])
self.assertEqual(robot[1].children, [robot[2]])
self.assertEqual(robot[2].children, [])
link1 = ELink(ETS.rx(), name='link1')
link2 = ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(),
name='link2',
parent='link1')
link3 = ELink(ETS.tx(1), name='ee_1', parent='link2')
robot = ERobot([link1, link2, link3])
self.assertEqual(robot.n, 2)
self.assertIsInstance(robot[0], ELink)
self.assertIsInstance(robot[1], ELink)
self.assertIsInstance(robot[2], ELink)
self.assertTrue(robot[0].isrevolute)
self.assertTrue(robot[1].isprismatic)
self.assertIs(robot[0].parent, None)
self.assertIs(robot[1].parent, robot[0])
self.assertIs(robot[2].parent, robot[1])
self.assertEqual(robot[0].children, [robot[1]])
self.assertEqual(robot[1].children, [robot[2]])
self.assertEqual(robot[2].children, [])
def __init__(self):
# Puma dimensions (m)
l1 = 0.672
l2 = 0.2337
l3 = 0.4318
l4 = -0.0837
l5 = 0.4318
l6 = 0.0203
e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() * ET.tz(l3) * \
ET.tx(l6) * ET.ty(l4) * ET.ry() * ET.tz(l5) * ET.rz() * \
ET.ry() * ET.rz() * ET.tx(0.2)
super().__init__(e,
name='Puma560',
manufacturer='Unimation',
comment='ETS-based model')
self.addconfiguration("qz", [0, 0, 0, 0, 0, 0])
self.addconfiguration("qbent", [0, -90, 90, 0, 0, 0], 'deg')
def test_init_elink_autoparent(self):
links = [
ELink(ETS.rx(), name='link1'),
ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.tz(), name='link2'),
ELink(ETS.tx(1), name='ee_1')
]
robot = ERobot(links)
self.assertEqual(robot.n, 2)
self.assertIsInstance(robot[0], ELink)
self.assertIsInstance(robot[1], ELink)
self.assertIsInstance(robot[2], ELink)
self.assertTrue(robot[0].isrevolute)
self.assertTrue(robot[1].isprismatic)
self.assertIs(robot[0].parent, None)
self.assertIs(robot[1].parent, robot[0])
self.assertIs(robot[2].parent, robot[1])
self.assertEqual(robot[0].children, [robot[1]])
self.assertEqual(robot[1].children, [robot[2]])
self.assertEqual(robot[2].children, [])
def __init__(self, s):
et_re = re.compile(r"([RT][xyz])\(([^)]*)\)")
super().__init__()
# self.data = []
for axis, eta in et_re.findall(s):
print(axis, eta)
if eta[0] == 'q':
eta = None
unit = None
else:
# eta can be given as a variable or a number
try:
# first attempt to create symbolic number
eta = sympy.Number(eta)
except:
# failing that, a symbolic variable
eta = sympy.symbols(eta)
if axis[0] == 'R':
# convert to degrees, assumed unit in input string
eta = sympy.simplify(eta * deg)
if axis == 'Rx':
e = ETS.rx(eta)
elif axis == 'Ry':
e = ETS.ry(eta)
elif axis == 'Rz':
e = ETS.rz(eta)
elif axis == 'Tx':
e = ETS.tx(eta)
elif axis == 'Ty':
e = ETS.ty(eta)
elif axis == 'Tz':
e = ETS.tz(eta)
self.data.append(e.data[0])
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
l6 = 0.4318
print(sol)
puma.plot(sol.q, block=False)
puma.ikine_a(T, config="lun")
# Puma dimensions (m), see RVC2 Fig. 7.4 for details
l1 = 0.672
l2 = -0.2337
l3 = 0.4318
l4 = 0.0203
l5 = 0.0837
l6 = 0.4318
e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() \
* ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.ry() \
* ET.tz(l6) * ET.rz() * ET.ry() * ET.rz()
robot = ERobot(e)
print(robot)
panda = models.URDF.Panda()
print(panda)
# ## B. Trajectories
traj = jtraj(puma.qz, puma.qr, 100)
qplot(traj.q)
sol = puma.ikine_LM(T)
print(sol)
puma.plot(sol.q, block=False)
puma.ikine_a(T, config="lun")
# Puma dimensions (m), see RVC2 Fig. 7.4 for details
l1 = 0.672
l2 = -0.2337
l3 = 0.4318
l4 = 0.0203
l5 = 0.0837
l6 = 0.4318
e = (ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() * ET.tz(l3) * ET.tx(l4) *
ET.ty(l5) * ET.ry() * ET.tz(l6) * ET.rz() * ET.ry() * ET.rz())
robot = ERobot(e)
print(robot)
panda = models.URDF.Panda()
print(panda)
# ## B. Trajectories
traj = jtraj(puma.qz, puma.qr, 100)
qplot(traj.q)
t = np.arange(0, 2, 0.010)
T0 = SE3(0.6, -0.5, 0.3)