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