def test_rne(self):
puma = rp.Puma560()
puma.q = puma.qn
z = np.zeros(6)
o = np.ones(6)
fext = [1, 2, 3, 1, 2, 3]
tr0 = [-0.0000, 31.6399, 6.0351, 0.0000, 0.0283, 0]
tr1 = [29.1421, 56.5044, 16.3528, 1.2645, 1.1239, 0.5196]
tr2 = [32.4952, 60.8670, 17.7436, 1.4545, 1.2991, 0.7138]
tr3 = [29.7849, 53.9511, 15.0208, 4.0929, -0.8761, 3.5196]
t0 = puma.rne(z, z, puma.qn)
t1 = puma.rne(z, o, puma.qn)
puma.gravity = [0, 0, 9.81]
t2 = puma.rne(o, o, puma.qn)
t3 = puma.rne(z, z, grav=[0, 0, 9.81])
t4 = puma.rne(z, o, q=puma.qn, fext=fext)
nt.assert_array_almost_equal(t0, tr0, decimal=4)
nt.assert_array_almost_equal(t1, tr1, decimal=4)
nt.assert_array_almost_equal(t2, tr2, decimal=4)
nt.assert_array_almost_equal(t3, tr0, decimal=4)
nt.assert_array_almost_equal(t4, tr3, decimal=4)
def test_ikine6s_fail(self):
l0 = rp.Revolute(alpha=np.pi / 2)
l1 = rp.Revolute(d=1.0)
l2 = rp.Revolute(alpha=np.pi / 2)
l3 = rp.Revolute(alpha=-np.pi / 2)
l4a = rp.Revolute(alpha=np.pi / 2)
l4b = rp.Revolute()
l5 = rp.Revolute()
l6 = rp.Revolute(mdh=1)
r0 = rp.SerialLink([l0, l1, l2, l3, l4a, l5])
r1 = rp.SerialLink([l0, l1, l2, l3, l4b, l5])
r2 = rp.SerialLink([l1, l2, l3])
r3 = rp.SerialLink([l6, l6, l6, l6, l6, l6])
puma = rp.Puma560()
T = sm.SE3(0, 10, 10)
puma.ikine6s(T)
q = [1, 1, 1, 1, 1, 1]
T = r0.fkine(q)
with self.assertRaises(ValueError):
r0.ikine6s(T)
with self.assertRaises(ValueError):
r1.ikine6s(T)
with self.assertRaises(ValueError):
r2.ikine6s(T)
with self.assertRaises(ValueError):
r3.ikine6s(T)
def test_ikine6s_traj(self):
r0 = rp.Puma560()
q = r0.qr
T = r0.fkine(q)
Tt = sm.SE3([T, T, T])
qr0 = [0.2689, 1.5708, -1.4768, -3.1416, 0.0940, 2.8726]
q0, _ = r0.ikine6s(Tt)
nt.assert_array_almost_equal(q0[:, 0], qr0, decimal=4)
nt.assert_array_almost_equal(q0[:, 1], qr0, decimal=4)
nt.assert_array_almost_equal(q0[:, 2], qr0, decimal=4)
def test_jacob_dot(self):
puma = rp.Puma560()
puma.q = puma.qr
puma.qd = puma.qr
q = puma.qr
j0 = puma.jacob_dot(q, q)
j1 = puma.jacob_dot()
res = [-0.0000, -1.0654, -0.3702, 2.4674, 0, 0]
nt.assert_array_almost_equal(j0, res, decimal=4)
nt.assert_array_almost_equal(j1, res, decimal=4)
def test_rne_traj(self):
puma = rp.Puma560()
z = np.zeros(6)
o = np.ones(6)
tr0 = [-0.0000, 31.6399, 6.0351, 0.0000, 0.0283, 0]
tr1 = [32.4952, 60.8670, 17.7436, 1.4545, 1.2991, 0.7138]
t0 = puma.rne(np.c_[z, o], np.c_[z, o], np.c_[puma.qn, puma.qn])
nt.assert_array_almost_equal(t0[:, 0], tr0, decimal=4)
nt.assert_array_almost_equal(t0[:, 1], tr1, decimal=4)
def test_ikine6s_puma(self):
r0 = rp.Puma560()
q = r0.qr
T = r0.fkine(q)
qr0 = [0.2689, 1.5708, -1.4768, -3.1416, 0.0940, 2.8726]
qr1 = [0.0000, 1.5238, -1.4768, -0.0000, -0.0470, -0.0000]
q0, _ = r0.ikine6s(T)
q1, _ = r0.ikine6s(T, left=False, elbow_up=False, wrist_flip=True)
nt.assert_array_almost_equal(q0, qr0, decimal=4)
nt.assert_array_almost_equal(q1, qr1, decimal=4)
def test_rne_delete(self):
puma = rp.Puma560()
z = np.zeros(6)
tr0 = [-0.0000, 31.6399, 6.0351, 0.0000, 0.0283, 0]
t0 = puma.rne(z, z, puma.qn)
puma.delete_rne()
t1 = puma.rne(z, z, puma.qn)
nt.assert_array_almost_equal(t0, tr0, decimal=4)
nt.assert_array_almost_equal(t1, tr0, decimal=4)
def test_asada(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
m0, mx0 = puma.maniplty(method='asada')
m1, mx1 = puma.maniplty(q, method='asada')
m2, mx2 = puma.maniplty(np.c_[q, q], method='asada')
m3, mx3 = puma.maniplty(q, axes=[1, 1, 1, 0, 0, 0], method='asada')
m4, mx4 = puma.maniplty(q, axes=[0, 0, 0, 1, 1, 1], method='asada')
m5, mx5 = puma.maniplty(puma.qz, method='asada')
a0 = 0.0044
a2 = 0.2094
a3 = 0.1716
a4 = 0.0
ax0 = np.array([[17.2954, -2.7542, -9.6233, -0.0000, 0.2795, -0.0000],
[-2.7542, 12.1909, 1.2459, -0.3254, -0.0703, -0.9652],
[-9.6233, 1.2459, 13.3348, -0.0000, 0.2767, 0.0000],
[-0.0000, -0.3254, -0.0000, 0.1941, 0.0000, 0.1941],
[0.2795, -0.0703, 0.2767, 0.0000, 0.1713, 0.0000],
[-0.0000, -0.9652, 0.0000, 0.1941, 0.0000, 0.5791]])
ax1 = np.array([[17.2954, -2.7542,
-9.6233], [-2.7542, 12.1909, 1.2459],
[-9.6233, 1.2459, 13.3348]])
ax2 = np.array([[0.1941, 0.0000, 0.1941], [0.0000, 0.1713, 0.0000],
[0.1941, 0.0000, 0.5791]])
ax3 = np.zeros((6, 6))
nt.assert_almost_equal(m0, a0, decimal=4)
nt.assert_array_almost_equal(mx0, ax0, decimal=4)
nt.assert_almost_equal(m1, a0, decimal=4)
nt.assert_array_almost_equal(mx1, ax0, decimal=4)
nt.assert_almost_equal(m2[0], a0, decimal=4)
nt.assert_array_almost_equal(mx2[:, :, 0], ax0, decimal=4)
nt.assert_almost_equal(m2[1], a0, decimal=4)
nt.assert_array_almost_equal(mx2[:, :, 1], ax0, decimal=4)
nt.assert_almost_equal(m3, a2, decimal=4)
nt.assert_array_almost_equal(mx3, ax1, decimal=4)
nt.assert_almost_equal(m4, a3, decimal=4)
nt.assert_array_almost_equal(mx4, ax2, decimal=4)
nt.assert_almost_equal(m5, a4, decimal=4)
nt.assert_array_almost_equal(mx5, ax3, decimal=4)
def test_ikinem(self):
puma = rp.Puma560()
q = puma.qr
T = puma.fkine(q)
Tt = sm.SE3([T, T])
q0, _, _ = puma.ikinem(Tt)
q1, success, _ = puma.ikinem(T.A, qlimits=False)
q2, success, _ = puma.ikinem(T, qlimits=False, stiffness=0.1, ilimit=1)
print(np.sum(np.abs(T.A - puma.fkine(q0[:, 0]).A)))
self.assertTrue(np.sum(np.abs(T.A - puma.fkine(q0[:, 0]).A)) < 0.7)
self.assertTrue(np.sum(np.abs(T.A - puma.fkine(q0[:, 1]).A)) < 0.7)
self.assertTrue(np.sum(np.abs(T.A - puma.fkine(q1).A)) < 0.7)
def test_itorque(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
qdd = [1, 2, 3, 1, 2, 3]
tauir = [3.1500, 9.4805, 3.6189, 0.1901, 0.3519, 0.5823]
taui0 = puma.itorque(qdd, q)
taui1 = puma.itorque(np.c_[qdd, qdd], np.c_[q, q])
taui2 = puma.itorque(qdd)
nt.assert_array_almost_equal(taui0, tauir, decimal=4)
nt.assert_array_almost_equal(taui1[:, 0], tauir, decimal=4)
nt.assert_array_almost_equal(taui1[:, 1], tauir, decimal=4)
nt.assert_array_almost_equal(taui2, tauir, decimal=4)
def test_dyn(self):
puma = rp.Puma560()
s0 = puma.links[0].dyn()
self.assertEqual(
s0,
"m = 0.00 \n"
"r = 0.00 0.00 0.00 \n"
" | 0.00 0.00 0.00 | \n"
"I = | 0.00 0.35 0.00 | \n"
" | 0.00 0.00 0.00 | \n"
"Jm = 0.00 \n"
"B = 0.00 \n"
"Tc = 0.40(+) -0.43(-) \n"
"G = -62.61 \n"
"qlim = -2.79 to 2.79")
def test_accel(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
qd = [0.1, 0.2, 0.8, 0.2, 0.5, 1.0]
torque = [1.0, 3.2, 1.8, 0.1, 0.7, 4.6]
res = [-7.4102, -9.8432, -10.9694, -4.4314, -0.9881, 21.0228]
qdd0 = puma.accel(qd, torque, q)
qdd1 = puma.accel(np.c_[qd, qd], np.c_[torque, torque], np.c_[q, q])
qdd2 = puma.accel(qd, torque)
nt.assert_array_almost_equal(qdd0, res, decimal=4)
nt.assert_array_almost_equal(qdd1[:, 0], res, decimal=4)
nt.assert_array_almost_equal(qdd1[:, 1], res, decimal=4)
nt.assert_array_almost_equal(qdd2, res, decimal=4)
def test_inertia(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
Ir = [[3.6594, -0.4044, 0.1006, -0.0025, 0.0000, -0.0000],
[-0.4044, 4.4137, 0.3509, 0.0000, 0.0024, 0.0000],
[0.1006, 0.3509, 0.9378, 0.0000, 0.0015, 0.0000],
[-0.0025, 0.0000, 0.0000, 0.1925, 0.0000, 0.0000],
[0.0000, 0.0024, 0.0015, 0.0000, 0.1713, 0.0000],
[-0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.1941]]
I0 = puma.inertia(q)
I1 = puma.inertia(np.c_[q, q])
I2 = puma.inertia()
nt.assert_array_almost_equal(I0, Ir, decimal=4)
nt.assert_array_almost_equal(I1[:, :, 0], Ir, decimal=4)
nt.assert_array_almost_equal(I1[:, :, 1], Ir, decimal=4)
nt.assert_array_almost_equal(I2, Ir, decimal=4)
def test_gravload(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
grav = [0, 0, 9.81]
taur = [-0.0000, 31.6399, 6.0351, 0.0000, 0.0283, 0]
tau0 = puma.gravload(q)
tau1 = puma.gravload(np.c_[q, q])
tau2 = puma.gravload(q=np.c_[q, q], grav=np.c_[grav, grav])
tau3 = puma.gravload(grav=grav)
nt.assert_array_almost_equal(tau0, taur, decimal=4)
nt.assert_array_almost_equal(tau1[:, 0], taur, decimal=4)
nt.assert_array_almost_equal(tau1[:, 1], taur, decimal=4)
nt.assert_array_almost_equal(tau2[:, 0], taur, decimal=4)
nt.assert_array_almost_equal(tau2[:, 1], taur, decimal=4)
nt.assert_array_almost_equal(tau3, taur, decimal=4)
def test_cinertia(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
Mr = [[17.2954, -2.7542, -9.6233, -0.0000, 0.2795, 0.0000],
[-2.7542, 12.1909, 1.2459, -0.3254, -0.0703, -0.9652],
[-9.6233, 1.2459, 13.3348, -0.0000, 0.2767, -0.0000],
[-0.0000, -0.3254, -0.0000, 0.1941, 0.0000, 0.1941],
[0.2795, -0.0703, 0.2767, 0.0000, 0.1713, 0.0000],
[0.0000, -0.9652, -0.0000, 0.1941, 0.0000, 0.5791]]
M0 = puma.cinertia(q)
M1 = puma.cinertia(np.c_[q, q])
M2 = puma.cinertia()
nt.assert_array_almost_equal(M0, Mr, decimal=4)
nt.assert_array_almost_equal(M1[:, :, 0], Mr, decimal=4)
nt.assert_array_almost_equal(M1[:, :, 1], Mr, decimal=4)
nt.assert_array_almost_equal(M2, Mr, decimal=4)
def test_ikunc(self):
puma = rp.Puma560()
q = puma.qr
T = puma.fkine(q)
Tt = sm.SE3([T, T])
q0, _, _ = puma.ikunc(Tt)
q1, success, _ = puma.ikunc(T.A)
q2, success, _ = puma.ikunc(T, ilimit=1)
nt.assert_array_almost_equal(T.A - puma.fkine(q0[:, 0]).A,
np.zeros((4, 4)),
decimal=4)
nt.assert_array_almost_equal(T.A - puma.fkine(q0[:, 1]).A,
np.zeros((4, 4)),
decimal=4)
nt.assert_array_almost_equal(T.A - puma.fkine(q1).A,
np.zeros((4, 4)),
decimal=4)
def test_yoshi(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
m0 = puma.maniplty()
m1 = puma.maniplty(q)
m2 = puma.maniplty(np.c_[q, q])
m3 = puma.maniplty(q, axes=[1, 1, 1, 0, 0, 0])
m4 = puma.maniplty(q, axes=[0, 0, 0, 1, 1, 1])
a0 = 0.0786
a2 = 0.111181
a3 = 2.44949
nt.assert_almost_equal(m0, a0, decimal=4)
nt.assert_almost_equal(m1, a0, decimal=4)
nt.assert_almost_equal(m2[0], a0, decimal=4)
nt.assert_almost_equal(m2[1], a0, decimal=4)
nt.assert_almost_equal(m3, a2, decimal=4)
nt.assert_almost_equal(m4, a3, decimal=4)
def test_coriolis(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
qd = [1, 2, 3, 1, 2, 3]
Cr = [[-0.1735, -2.0494, -0.1178, -0.0002, -0.0045, 0.0001],
[0.6274, 1.1572, 1.9287, -0.0015, -0.0003, -0.0000],
[-0.3608, -0.7734, -0.0018, -0.0009, -0.0064, -0.0000],
[0.0011, 0.0005, -0.0001, 0.0002, 0.0002, -0.0001],
[-0.0002, 0.0028, 0.0046, -0.0002, -0.0000, -0.0000],
[0.0001, 0.0000, 0.0000, 0.0000, 0.0000, 0]]
C0 = puma.coriolis(qd, q)
C1 = puma.coriolis(np.c_[qd, qd], np.c_[q, q])
C2 = puma.coriolis(qd)
nt.assert_array_almost_equal(C0, Cr, decimal=4)
nt.assert_array_almost_equal(C1[:, :, 0], Cr, decimal=4)
nt.assert_array_almost_equal(C1[:, :, 1], Cr, decimal=4)
nt.assert_array_almost_equal(C2, Cr, decimal=4)
def test_str(self):
puma = rp.Puma560()
l0 = rp.Prismatic(mdh=1)
r0 = rp.SerialLink([l0, l0, l0])
str(r0)
res = ("\nPuma 560 (Unimation): 6 axis, RRRRRR, std DH\n"
"Parameters:\n"
"Revolute theta= 0.00 d= 0.00 a= 0.00"
" alpha= 1.57 offset= 0.00\n"
"Revolute theta= 0.00 d= 0.00 a= 0.43"
" alpha= 0.00 offset= 0.00\n"
"Revolute theta= 0.00 d= 0.15 a= 0.02"
" alpha=-1.57 offset= 0.00\n"
"Revolute theta= 0.00 d= 0.43 a= 0.00"
" alpha= 1.57 offset= 0.00\n"
"Revolute theta= 0.00 d= 0.00 a= 0.00"
" alpha=-1.57 offset= 0.00\n"
"Revolute theta= 0.00 d= 0.00 a= 0.00"
" alpha= 0.00 offset= 0.00\n\n"
"tool: t = (0, 0, 0), RPY/xyz = (0, 0, 0) deg")
self.assertEqual(str(puma), res)
def test_paycap(self):
puma = rp.Puma560()
puma.q = puma.qn
q = puma.qn
w = [1, 2, 1, 2, 1, 2]
tauR = np.ones((6, 2))
tauR[:, 1] = -1
res0 = [
1.15865438e+00, -3.04790052e+02, -5.00870095e+01, 6.00479950e+15,
3.76356072e+00, 1.93649167e+00
]
wmax0, joint = puma.paycap(w, tauR, q=q, frame=0)
wmax1, _ = puma.paycap(np.c_[w, w], tauR, q=np.c_[q, q], frame=0)
wmax2, _ = puma.paycap(w, tauR, frame=0)
nt.assert_allclose(wmax0, res0)
self.assertEqual(joint, 1)
nt.assert_allclose(wmax1[:, 0], res0)
nt.assert_allclose(wmax1[:, 1], res0)
nt.assert_allclose(wmax2, res0)
def test_perterb(self):
puma = rp.Puma560()
p2 = puma.perterb()
p3 = puma.perterb(0.8)
resI0 = np.zeros(puma.n)
resm0 = np.zeros(puma.n)
resI1 = np.zeros(puma.n)
resm1 = np.zeros(puma.n)
for i in range(puma.n):
resI0[i] = np.divide(
np.sum(np.abs(puma.links[i].I - p2.links[i].I)),
np.sum(np.abs(puma.links[i].I)))
if puma.links[i].m - p2.links[i].m != 0.0:
resm0[i] = np.abs(
np.divide((puma.links[i].m - p2.links[i].m),
puma.links[i].m))
else:
resm0[i] = 0
resI1[i] = np.divide(
np.sum(np.abs(puma.links[i].I - p3.links[i].I)),
np.sum(np.abs(puma.links[i].I)))
if puma.links[i].m - p3.links[i].m != 0.0:
resm1[i] = np.abs(
np.divide((puma.links[i].m - p3.links[i].m),
puma.links[i].m))
else:
resm1[i] = 0
self.assertTrue(resI0[i] < 0.1)
self.assertTrue(resm0[i] < 0.1 or np.isnan(resm0[i]))
self.assertTrue(resI1[i] < 0.8)
self.assertTrue(resm1[i] < 0.8 or np.isnan(resm1[i]))
def test_puma(self):
puma = rp.Puma560()
puma.qr
puma.qz
puma.qs
puma.qn
def test_maniplty_fail(self):
puma = rp.Puma560()
puma.q = puma.qn
with self.assertRaises(ValueError):
puma.maniplty(method='notamethod')
