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_dh_error(self): l0 = rp.PrismaticMDH() l1 = rp.RevoluteDH() r0 = rp.DHRobot([l0]) r1 = rp.DHRobot([l1]) with self.assertRaises(ValueError): rp.DHRobot([l0, l1]) with self.assertRaises(ValueError): r0 + r1 with self.assertRaises(ValueError): rp.DHRobot([l0, l1]) r0 + l1
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_str(self): puma = rp.models.DH.Puma560() l0 = rp.PrismaticMDH() r0 = rp.DHRobot([l0, l0, l0]) str(r0) res = """ Puma 560 (Unimation): 6 axis, RRRRRR, std DH Parameters: Revolute theta=q1 + 0.00, d= 0.67, a= 0.00, alpha= 1.57 Revolute theta=q2 + 0.00, d= 0.00, a= 0.43, alpha= 0.00 Revolute theta=q3 + 0.00, d= 0.15, a= 0.02, alpha=-1.57 Revolute theta=q4 + 0.00, d= 0.43, a= 0.00, alpha= 1.57 Revolute theta=q5 + 0.00, d= 0.00, a= 0.00, alpha=-1.57 Revolute theta=q6 + 0.00, d= 0.00, a= 0.00, alpha= 0.00 tool: t = (0, 0, 0), RPY/xyz = (0, 0, 0) deg""" self.assertEqual(str(puma), res)
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()