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