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 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.rx(45, 'deg') * ETS.tz(0.75)
print(e)
print(e.eval())
from roboticstoolbox import ETS
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
