def test_init_elink_branched(self):
robot = ERobot([
ELink(ETS.rz(), name='link1'),
ELink(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(),
name='link2',
parent='link1'),
ELink(ETS.tx(1), name='ee_1', parent='link2'),
ELink(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(),
name='link3',
parent='link1'),
ELink(ETS.tx(1), name='ee_2', parent='link3')
])
self.assertEqual(robot.n, 3)
for i in range(5):
self.assertIsInstance(robot[i], ELink)
self.assertTrue(robot[0].isrevolute)
self.assertTrue(robot[1].isrevolute)
self.assertTrue(robot[3].isrevolute)
self.assertIs(robot[0].parent, None)
self.assertIs(robot[1].parent, robot[0])
self.assertIs(robot[2].parent, robot[1])
self.assertIs(robot[3].parent, robot[0])
self.assertIs(robot[4].parent, robot[3])
self.assertEqual(robot[0].children, [robot[1], robot[3]])
self.assertEqual(robot[1].children, [robot[2]])
self.assertEqual(robot[2].children, [])
self.assertEqual(robot[3].children, [robot[4]])
self.assertEqual(robot[2].children, [])
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])
:seealso: :func:`ETS`
"""
return cls(lambda y: transl2(0, y), axis='ty', eta=eta, **kwargs)
if __name__ == "__main__":
print(ETS.rx(0.2))
print(ETS.rx(45, 'deg'))
print(ETS.tz(0.75))
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()
# 7.4 - 2 link planar robot velocity ellipse import numpy as np import roboticstoolbox as rtb import matplotlib.pyplot as plt # Create the model p2 = rtb.models.DH.Planar2() # Assign some angles p2.q = [0.1, 0.2] # Plot and include the Velocity Ellipsoid #p2.plot(p2.q, vellipse=True) from roboticstoolbox import ETS as ets # Links a1 = 1 a2 = 1 # Angles q1 = 0 q2 = np.pi / 2 e = ets.rz(q1) * ets.tx(a1) * ets.rz(q2) * ets.tx(a2) print(e.eval([q1, q2])) e.plot([q1, q2], vellipse=True)
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)
