def test_ikine6s_rrp(self): l0 = rp.Revolute(alpha=-np.pi / 2) l1 = rp.Revolute(alpha=np.pi / 2) l2 = rp.Prismatic() l3 = rp.Revolute(alpha=-np.pi / 2) l4 = rp.Revolute(alpha=np.pi / 2) l5 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3, l4, l5]) r1 = rp.SerialLink([l1, l0, l2, l3, l4, l5]) q = [1, 1, 1, 1, 1, 1] T1 = r0.fkine(q) T2 = r1.fkine(q) qr0 = [1.0000, -2.1416, -1.0000, -1.0000, -2.1416, 1.0000] qr1 = [-2.1416, -1.0000, 1.0000, -2.1416, 1.0000, 1.0000] qr2 = [1.0000, 1.0000, 1.0000, -2.1416, -1.0000, -2.1416] qr3 = [-2.1416, 2.1416, -1.0000, -1.0000, 2.1416, -2.1416] q0, _ = r0.ikine6s(T1) q1, _ = r0.ikine6s(T1, left=False, elbow_up=False, wrist_flip=True) q2, _ = r1.ikine6s(T2) q3, _ = r1.ikine6s(T2, 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) nt.assert_array_almost_equal(q2, qr2, decimal=4) nt.assert_array_almost_equal(q3, qr3, decimal=4)
def test_ikine6s_offset(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) l4 = rp.Revolute(alpha=np.pi / 2) l5 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3, l4, l5]) r1 = rp.SerialLink([l2, l1, l0, l3, l4, l5]) q = [1, 1, 1, 1, 1, 1] T1 = r0.fkine(q) T2 = r1.fkine(q) qr0 = [1.0000, 3.1416, -0.0000, -1.1675, -0.8786, 2.6811] qr1 = [1.0000, -1.1059, 2.6767, 0.8372, 1.2639, 1.3761] qr2 = [1.0000, 3.1416, -3.1416, -0.8053, -1.3811, 1.8933] qr3 = [1.0000, -1.1059, -0.4649, 1.8311, 2.3192, -2.6398] q0, _ = r0.ikine6s(T1.A) q1, _ = r0.ikine6s(T1, left=False, elbow_up=False, wrist_flip=True) q2, _ = r1.ikine6s(T2) q3, _ = r1.ikine6s(T2, 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) nt.assert_array_almost_equal(q2, qr2, decimal=4) nt.assert_array_almost_equal(q3, qr3, decimal=4)
def test_ikine6s_simple(self): l0 = rp.Revolute(alpha=-np.pi / 2) l1 = rp.Revolute() l2 = rp.Revolute(alpha=np.pi / 2) l3 = rp.Revolute(alpha=-np.pi / 2) l4 = rp.Revolute(alpha=np.pi / 2) l5 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3, l4, l5]) r1 = rp.SerialLink([l2, l1, l0, l3, l4, l5]) q = [1, 1, 1, 1, 1, 1] T1 = r0.fkine(q) T2 = r1.fkine(q) qr0 = [0, 0, 0, -0.9741, -2.2630, -0.4605] qr1 = [0, 0, 0, 0.1947, -1.3811, 1.8933] qr2 = [0, 0, 0, 2.1675, 2.2630, 2.6811] qr3 = [0, 0, 0, -2.9468, 1.3811, -1.2483] q0, _ = r0.ikine6s(T1) q1, _ = r0.ikine6s(T1, left=False, elbow_up=False, wrist_flip=True) q2, _ = r1.ikine6s(T2) q3, _ = r1.ikine6s(T2, left=False, elbow_up=False, wrist_flip=True) nt.assert_array_almost_equal(q0, qr0, decimal=4) nt.assert_array_almost_equal(q1, qr2, decimal=4) nt.assert_array_almost_equal(q2, qr1, decimal=4) nt.assert_array_almost_equal(q3, qr3, 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_qlim(self): qlim = [-1, 1] l0 = rp.Prismatic(qlim=qlim) l1 = rp.Revolute(qlim=qlim) l2 = rp.Prismatic(qlim=qlim) l3 = rp.Revolute(qlim=qlim) r0 = rp.SerialLink([l0, l1, l2, l3]) r1 = rp.SerialLink([l0]) ans = np.c_[qlim, qlim, qlim, qlim] nt.assert_array_almost_equal(r0.qlim, ans) nt.assert_array_almost_equal(r1.qlim, qlim)
def test_isspherical(self): l0 = rp.Revolute() l1 = rp.Revolute(alpha=-np.pi / 2) l2 = rp.Revolute(alpha=np.pi / 2) l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3]) r1 = rp.SerialLink([l0, l1]) r2 = rp.SerialLink([l1, l2, l3, l0]) self.assertTrue(r0.isspherical()) self.assertFalse(r1.isspherical()) self.assertFalse(r2.isspherical())
def test_r(self): r = np.array([[1], [2], [3]]) l0 = rp.Prismatic(r=r) l1 = rp.Revolute(r=r) l2 = rp.Prismatic(r=r) l3 = rp.Revolute(r=r) r0 = rp.SerialLink([l0, l1, l2, l3]) r1 = rp.SerialLink([l0]) ans = np.c_[r, r, r, r] nt.assert_array_almost_equal(r0.r, ans) nt.assert_array_almost_equal(r1.r, r)
def test_ikine(self): panda = rp.PandaMDH() q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) T = panda.fkine(q) Tt = sm.SE3([T, T]) l0 = rp.Revolute(d=2.0) l1 = rp.Prismatic(theta=1.0) l2 = rp.Prismatic(theta=1, qlim=[0, 2]) r0 = rp.SerialLink([l0, l1]) r1 = rp.SerialLink([l0, l2]) qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] qa, success, err = panda.ikine(T) qa2, success, err = panda.ikine(Tt) qa3, success, err = panda.ikine(Tt, q0=np.zeros((7, 2))) qa4, success, err = panda.ikine(T, q0=np.zeros(7)) # Untested qa5, success, err = r0.ikine(T.A, mask=[1, 1, 0, 0, 0, 0], transpose=5, ilimit=5) qa5, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0]) qa6, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1) qa7, success, err = r1.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1) nt.assert_array_almost_equal(qa, qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 0], qr, decimal=4) nt.assert_array_almost_equal(qa2[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa3[:, 1], qr, decimal=4) nt.assert_array_almost_equal(qa4, qr, decimal=4) with self.assertRaises(ValueError): panda.ikine(Tt, q0=np.zeros(7)) with self.assertRaises(ValueError): r0.ikine(T) with self.assertRaises(ValueError): r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1)
def test_dh_error(self): l0 = rp.Prismatic(mdh=1) l1 = rp.Revolute() r0 = rp.SerialLink([l0]) r1 = rp.SerialLink([l1]) with self.assertRaises(ValueError): rp.SerialLink([l0, l1]) with self.assertRaises(ValueError): r0 + r1 with self.assertRaises(ValueError): rp.SerialLink([l0, l1]) r0 + l1
def test_add_error(self): l0 = rp.Prismatic() l1 = rp.Revolute() r0 = rp.SerialLink([l0, l1]) with self.assertRaises(TypeError): r0 + 2
def test_add_seriallink(self): l0 = rp.Prismatic() l1 = rp.Revolute() l2 = rp.Prismatic(theta=2.0) l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1]) r1 = rp.SerialLink([l2, l3]) r3 = r0 + r1 q = np.array([1, 2, 3, 4]) T1 = np.array([[-0.14550003, -0.98935825, 0, 0], [0.98935825, -0.14550003, 0, 0], [0, 0, 1, 4], [0, 0, 0, 1]]) nt.assert_array_almost_equal(r3.fkine(q).A, T1)
def test_friction(self): l0 = rp.Revolute(d=2, B=3, G=2, Tc=[2, -1]) qd = [1, 2, 3, 4] r0 = rp.SerialLink([l0, l0, l0, l0]) tau = np.array([-16, -28, -40, -52]) nt.assert_array_almost_equal(r0.friction(qd), tau)
def test_offset(self): l0 = rp.Prismatic(offset=1.0) l1 = rp.Revolute(offset=2.0) l2 = rp.Prismatic(offset=3.0) l3 = rp.Revolute(offset=4.0) r0 = rp.SerialLink([l0, l1, l2, l3]) ans = [1.0, 2.0, 3.0, 4.0] self.assertEqual(r0.offset, ans)
def test_theta(self): l0 = rp.Prismatic(theta=1.0) l1 = rp.Revolute() l2 = rp.Prismatic(theta=3.0) l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3]) ans = [1.0, 0.0, 3.0, 0.0] self.assertEqual(r0.theta, ans)
def test_a(self): l0 = rp.Prismatic(a=1.0) l1 = rp.Revolute(a=2.0) l2 = rp.Prismatic(a=3.0) l3 = rp.Revolute(a=4.0) r0 = rp.SerialLink([l0, l1, l2, l3]) ans = [1.0, 2.0, 3.0, 4.0] self.assertEqual(r0.a, ans)
def test_d(self): l0 = rp.Prismatic() l1 = rp.Revolute(d=2.0) l2 = rp.Prismatic() l3 = rp.Revolute(d=4.0) r0 = rp.SerialLink([l0, l1, l2, l3]) ans = [0.0, 2.0, 0.0, 4.0] self.assertEqual(r0.d, ans)
def test_ikine3(self): l0 = rp.Revolute(alpha=np.pi / 2) l1 = rp.Revolute(a=0.4318) l2 = rp.Revolute(d=0.15005, a=0.0203, alpha=-np.pi / 2) l3 = rp.Prismatic() l4 = rp.Prismatic(mdh=1) r0 = rp.SerialLink([l0, l1, l2]) r1 = rp.SerialLink([l3, l3]) r2 = rp.SerialLink([l3, l3, l3]) r3 = rp.SerialLink([l4, l4, l4]) q = [1, 1, 1] r0.q = q T = r0.fkine(q) # T2 = r1.fkine(q) Tt = sm.SE3([T, T]) res = [2.9647, 1.7561, 0.2344] res2 = [1.0000, 0.6916, 0.2344] res3 = [2.9647, 2.4500, 3.1762] res4 = [1.0000, 1.3855, 3.1762] q0 = r0.ikine3(T.A) q1 = r0.ikine3(Tt) q2 = r0.ikine3(T, left=False, elbow_up=False) q3 = r0.ikine3(T, elbow_up=False) q4 = r0.ikine3(T, left=False) nt.assert_array_almost_equal(q0, res, decimal=4) nt.assert_array_almost_equal(q1[:, 0], res, decimal=4) nt.assert_array_almost_equal(q1[:, 1], res, decimal=4) nt.assert_array_almost_equal(q2, res2, decimal=4) nt.assert_array_almost_equal(q3, res3, decimal=4) nt.assert_array_almost_equal(q4, res4, decimal=4) with self.assertRaises(ValueError): r1.ikine3(T) with self.assertRaises(ValueError): r2.ikine3(T) with self.assertRaises(ValueError): r3.ikine3(T)
def test_isrevolute(self): l0 = rp.Prismatic() l1 = rp.Revolute() l2 = rp.Prismatic() l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3]) ans = [False, True, False, True] self.assertEqual(r0.isrevolute(), ans)
def test_gravjac(self): l0 = rp.Revolute(d=2, B=3, G=2, Tc=[2, -1], alpha=0.4, a=0.2, r=[0.1, 0.2, 0.05], m=0.5) l1 = rp.Prismatic(theta=0.1, B=3, G=2, Tc=[2, -1], a=0.2, r=[0.1, 0.2, 0.05], m=0.5) r0 = rp.SerialLink([l0, l0, l0, l0]) r1 = rp.SerialLink([l0, l0, l0, l1]) q = [0.3, 0.4, 0.2, 0.1] qT = np.c_[q, q] r0.q = q grav = [0.3, 0.5, 0.7] tauB = [0, 4.6280, 3.1524, 0.9324] tauB2 = [1.9412, 1.1374, 0.3494, -0.0001] tauB3 = [0, 3.2819, 2.0195, 1.9693] res0 = r0.gravjac(qT) res1 = r0.gravjac(q) res2 = r0.gravjac(q, grav) res3 = r0.gravjac() res4 = r1.gravjac(q) nt.assert_array_almost_equal(res0[:, 0], tauB, decimal=4) nt.assert_array_almost_equal(res0[:, 1], tauB, decimal=4) nt.assert_array_almost_equal(res1, tauB, decimal=4) nt.assert_array_almost_equal(res2, tauB2, decimal=4) nt.assert_array_almost_equal(res3, tauB, decimal=4) nt.assert_array_almost_equal(res4, tauB3, decimal=4)
def test_toradians(self): l0 = rp.Prismatic() l1 = rp.Revolute() l2 = rp.Prismatic() l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3]) q = np.array([np.pi, 180, np.pi, 90]) r0.q = q ans = np.array([np.pi, np.pi, np.pi, np.pi / 2.0]) nt.assert_array_almost_equal(r0.toradians(q), ans)
def test_jacob0(self): l0 = rp.Prismatic(theta=4) l1 = rp.Revolute(a=2) l2 = rp.Prismatic(theta=2) l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3]) q = [1, 2, 3, 4] r0.q = q J0 = np.array([[0, 0.5588, 0, 0], [0, 1.9203, 0, 0], [1.0000, 0, 1.0000, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 1.0000, 0, 1.0000]]) nt.assert_array_almost_equal(r0.jacob0(q), J0, decimal=4) nt.assert_array_almost_equal(r0.jacob0(), J0, decimal=4)
def test_fkine_traj(self): l0 = rp.Prismatic() l1 = rp.Revolute() l2 = rp.Prismatic(theta=2.0) l3 = rp.Revolute() q = np.array([1, 2, 3, 4]) qq = np.c_[q, q, q, q] T1 = np.array([[-0.14550003, -0.98935825, 0, 0], [0.98935825, -0.14550003, 0, 0], [0, 0, 1, 4], [0, 0, 0, 1]]) r0 = rp.SerialLink([l0, l1, l2, l3]) TT = r0.fkine(qq) nt.assert_array_almost_equal(TT[0].A, T1) nt.assert_array_almost_equal(TT[1].A, T1) nt.assert_array_almost_equal(TT[2].A, T1) nt.assert_array_almost_equal(TT[3].A, T1)
def test_jacobev(self): l0 = rp.Prismatic(theta=4) l1 = rp.Revolute(a=2) l2 = rp.Prismatic(theta=2) l3 = rp.Revolute() r0 = rp.SerialLink([l0, l1, l2, l3]) q = [1, 2, 3, 4] r0.q = q J = np.array([ [0.8439, -0.5366, 0, 0, 0, 0], [0.5366, 0.8439, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 0.8439, -0.5366, 0], [0, 0, 0, 0.5366, 0.8439, 0], [0, 0, 0, 0, 0, 1], ]) nt.assert_array_almost_equal(r0.jacobev(q), J, decimal=4) nt.assert_array_almost_equal(r0.jacobev(), J, decimal=4)
def test_nofriction(self): l0 = rp.Link(Tc=2, B=3) l1 = rp.Link(Tc=2, B=3) l2 = rp.Link(Tc=2, B=3) l3 = rp.Link(Tc=2, B=3) L = [l0, l1, l2, l3] r0 = rp.SerialLink(L) n0 = r0.nofriction() n1 = r0.nofriction(viscous=True) n2 = r0.nofriction(coulomb=False) for i in range(4): nt.assert_array_almost_equal(n0.links[i].B, L[i].B) nt.assert_array_almost_equal(n0.links[i].Tc, [0, 0]) nt.assert_array_almost_equal(n1.links[i].B, 0) nt.assert_array_almost_equal(n1.links[i].Tc, [0, 0]) nt.assert_array_almost_equal(n2.links[i].B, L[i].B) nt.assert_array_almost_equal(n2.links[i].Tc, L[i].Tc)
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_seriallink(self): l0 = rp.Link() rp.SerialLink([l0])
def test_teach2_basic(self): l0 = rp.Link(d=2) r0 = rp.SerialLink([l0, l0]) e = r0.teach2(False) e.step() e.close()
def test_links(self): l0 = rp.Prismatic() with self.assertRaises(TypeError): rp.SerialLink(l0)
def test_bad_list(self): l0 = rp.Prismatic() with self.assertRaises(TypeError): rp.SerialLink([l0, 1])