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