def test_str(self): r0 = rp.models.DH.Puma560() r1 = rp.models.DH.Panda() str(r0) str(r1) l0 = rp.PrismaticDH(offset=1.0, qlim=[-1, 1]) l1 = rp.RevoluteDH(flip=True, offset=1.0, qlim=[-1, 1]) r2 = rp.DHRobot([l0, l1]) str(r2) l0 = rp.PrismaticMDH(offset=1.0, qlim=[-1, 1]) l1 = rp.RevoluteMDH(flip=True, offset=1.0, qlim=[-1, 1]) r3 = rp.DHRobot([l0, l1]) str(r3) l0 = rp.PrismaticDH(offset=1.0) l1 = rp.RevoluteDH(flip=True, offset=1.0) r4 = rp.DHRobot([l0, l1]) str(r4) l0 = rp.PrismaticMDH(offset=1.0) l1 = rp.RevoluteMDH(flip=True, offset=1.0) r5 = rp.DHRobot([l0, l1], base=sm.SE3.Tx(0.1), tool=sm.SE3.Tx(0.1)) str(r5)
def test_ikine_LM(self): panda = rp.models.DH.Panda() 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.RevoluteDH(d=2.0) l1 = rp.PrismaticDH(theta=1.0) l2 = rp.PrismaticDH(theta=1, qlim=[0, 2]) r0 = rp.DHRobot([l0, l1]) r1 = rp.DHRobot([l0, l2]) qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] sol1 = panda.ikine_LM(T) sol2 = panda.ikine_LM(Tt) sol3 = panda.ikine_LM(T, q0=[0, 1.4, 0, 1, 0, 0.5, 1]) # Untested sol5 = r0.ikine_LM(T.A, mask=[1, 1, 0, 0, 0, 0], transpose=5, ilimit=5) sol6 = r0.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0]) sol7 = r0.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1) sol8 = r1.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1) self.assertTrue(sol1.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) self.assertTrue(sol2[0].success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2[0].q)), 0, places=4) self.assertTrue(sol2[0].success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol2[1].q)), 0, places=4) self.assertTrue(sol3.success) self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4) with self.assertRaises(ValueError): panda.ikine_LM(T, q0=[1, 2]) with self.assertRaises(ValueError): r0.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1, search=True, slimit=1)
def test_offset(self): l0 = rp.PrismaticDH(offset=1.0) l1 = rp.RevoluteDH(offset=2.0) l2 = rp.PrismaticDH(offset=3.0) l3 = rp.RevoluteDH(offset=4.0) r0 = rp.DHRobot([l0, l1, l2, l3]) ans = [1.0, 2.0, 3.0, 4.0] self.assertEqual(r0.offset, ans)
def test_d(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH(d=2.0) l2 = rp.PrismaticDH() l3 = rp.RevoluteDH(d=4.0) r0 = rp.DHRobot([l0, l1, l2, l3]) ans = [0.0, 2.0, 0.0, 4.0] self.assertEqual(r0.d, ans)
def test_a(self): l0 = rp.PrismaticDH(a=1.0) l1 = rp.RevoluteDH(a=2.0) l2 = rp.PrismaticDH(a=3.0) l3 = rp.RevoluteDH(a=4.0) r0 = rp.DHRobot([l0, l1, l2, l3]) ans = [1.0, 2.0, 3.0, 4.0] self.assertEqual(r0.a, ans)
def test_theta(self): l0 = rp.PrismaticDH(theta=1.0) l1 = rp.RevoluteDH() l2 = rp.PrismaticDH(theta=3.0) l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3]) ans = [1.0, 0.0, 3.0, 0.0] self.assertEqual(r0.theta, ans)
def test_prismaticjoints(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH() l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3]) ans = [True, False, True, False] self.assertEqual(r0.prismaticjoints, ans)
def test_isrevolute(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH() l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3]) ans = [False, True, False, True] self.assertEqual(r0.isrevolute(), ans)
def test_todegrees(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH() l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3]) q = np.array([np.pi, np.pi, np.pi, np.pi / 2.0]) ans = np.array([np.pi, 180, np.pi, 90]) nt.assert_array_almost_equal(r0.todegrees(q), ans)
def test_isprismatic(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH() l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3]) self.assertEqual(r0.isprismatic(0), True) self.assertEqual(r0.isprismatic(1), False) self.assertEqual(r0.isprismatic(2), True) self.assertEqual(r0.isprismatic(3), False)
def test_r(self): r = np.array([[1], [2], [3]]) l0 = rp.PrismaticDH(r=r) l1 = rp.RevoluteDH(r=r) l2 = rp.PrismaticDH(r=r) l3 = rp.RevoluteDH(r=r) r0 = rp.DHRobot([l0, l1, l2, l3]) r1 = rp.DHRobot([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_qlim(self): qlim = [-1, 1] l0 = rp.PrismaticDH(qlim=qlim) l1 = rp.RevoluteDH(qlim=qlim) l2 = rp.PrismaticDH(qlim=qlim) l3 = rp.RevoluteDH(qlim=qlim) r0 = rp.DHRobot([l0, l1, l2, l3]) r1 = rp.DHRobot([l0]) ans = np.c_[qlim, qlim, qlim, qlim] nt.assert_array_almost_equal(r0.qlim, ans) nt.assert_array_almost_equal(r1.qlim, np.c_[qlim])
def test_r(self): r = np.r_[1, 2, 3] l0 = rp.PrismaticDH(r=r) l1 = rp.RevoluteDH(r=r) l2 = rp.PrismaticDH(r=r) l3 = rp.RevoluteDH(r=r) r0 = rp.DHRobot([l0, l1, l2, l3]) r1 = rp.DHRobot([l0]) ans = np.c_[r, r, r, r] nt.assert_array_almost_equal(r0.r, ans) nt.assert_array_almost_equal(r1.r, r.flatten())
def test_links(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH() l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3]) self.assertIs(r0[0], l0) self.assertIs(r0[1], l1) self.assertIs(r0[2], l2) self.assertIs(r0[3], l3)
def test_ikine(self): panda = rp.models.DH.Panda() 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.RevoluteDH(d=2.0) l1 = rp.PrismaticDH(theta=1.0) l2 = rp.PrismaticDH(theta=1, qlim=[0, 2]) r0 = rp.DHRobot([l0, l1]) r1 = rp.DHRobot([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((2, 7))) 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_fkine(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH(theta=2.0) l3 = rp.RevoluteDH() 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]]) r0 = rp.DHRobot([l0, l1, l2, l3]) nt.assert_array_almost_equal(r0.fkine(q).A, T1)
def test_jacob0(self): l0 = rp.PrismaticDH(theta=4) l1 = rp.RevoluteDH(a=2) l2 = rp.PrismaticDH(theta=2) l3 = rp.RevoluteDH() r0 = rp.DHRobot([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)
def test_add_error(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1]) with self.assertRaises(TypeError): r0 + 2
def test_ikine6s_rrp(self): l0 = rp.RevoluteDH(alpha=-np.pi / 2) l1 = rp.RevoluteDH(alpha=np.pi / 2) l2 = rp.PrismaticDH() l3 = rp.RevoluteDH(alpha=-np.pi / 2) l4 = rp.RevoluteDH(alpha=np.pi / 2) l5 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1, l2, l3, l4, l5]) r1 = rp.DHRobot([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 zadanie_1_variable( l1, l2): robot = rtb.DHRobot( [ rtb.RevoluteDH(d=l1, alpha=np.pi / 2), rtb.RevoluteDH(alpha=np.pi / 2, offset=np.pi / 2 ), rtb.PrismaticDH(offset=l2) ], name="Mr_Robot") return robot
def test_fkine_traj(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH(theta=2.0) l3 = rp.RevoluteDH() q = np.array([1, 2, 3, 4]) qq = np.r_[q, q, q, q] r0 = rp.DHRobot([l0, l1, l2, l3]) T1 = r0.fkine(q).A 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 zadanie_1(): l1 = symbol("l1") l2 = symbol("l2") robot = rtb.DHRobot( [ rtb.RevoluteDH(d=l1, alpha=pi() / 2), rtb.RevoluteDH(alpha=pi()/2, offset=pi()/2 ), rtb.PrismaticDH(offset=l2) ], name="Mr_Robot") return robot
def test_add_links(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH(theta=2.0) l3 = rp.RevoluteDH() r0 = rp.DHRobot([l0, l1]) r1 = rp.DHRobot([l1, l2, l3]) r3 = r0 + l2 + l3 r4 = l0 + 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) nt.assert_array_almost_equal(r4.fkine(q).A, T1)
def test_jacobev(self): l0 = rp.PrismaticDH(theta=4) l1 = rp.RevoluteDH(a=2) l2 = rp.PrismaticDH(theta=2) l3 = rp.RevoluteDH() r0 = rp.DHRobot([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_str(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() s0 = l0.__str__() s1 = l1.__str__() self.assertEqual( s0, "PrismaticDH: theta=0.0, d=q, a=0.0, " "alpha=0.0") self.assertEqual(s1, "RevoluteDH: theta=q, d=0.0, a=0.0, " "alpha=0.0")
def test_init(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() with self.assertRaises(TypeError): rp.DHRobot([l0, l1], keywords=1) with self.assertRaises(TypeError): rp.Robot(l0) with self.assertRaises(TypeError): rp.Robot([l0, 1])
def test_fkine_traj(self): l0 = rp.PrismaticDH() l1 = rp.RevoluteDH() l2 = rp.PrismaticDH(theta=2.0) l3 = rp.RevoluteDH() 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.DHRobot([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_twists(self): # TODO panda = rp.models.DH.Panda() q = [1, 2, 3, 4, 5, 6, 7] panda.q = q panda.twists() panda.twists(q) puma = rp.models.DH.Puma560() q = [1, 2, 3, 4, 5, 6] puma.q = q puma.twists() puma.twists(q) l0 = rp.PrismaticMDH() r = rp.DHRobot([l0]) r.twists() l0 = rp.PrismaticDH() l1 = rp.PrismaticDH() r = rp.DHRobot([l0, l1]) r.twists()
def test_ikine3(self): l0 = rp.RevoluteDH(alpha=np.pi / 2) l1 = rp.RevoluteDH(a=0.4318) l2 = rp.RevoluteDH(d=0.15005, a=0.0203, alpha=-np.pi / 2) l3 = rp.PrismaticDH() l4 = rp.PrismaticMDH() r0 = rp.DHRobot([l0, l1, l2]) r1 = rp.DHRobot([l3, l3]) r2 = rp.DHRobot([l3, l3, l3]) r3 = rp.DHRobot([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_bad_list(self): l0 = rp.PrismaticDH() with self.assertRaises(TypeError): rp.DHRobot([l0, 1])