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_ikine6s_offset(self):
self.skipTest("error introduced with DHLink change")
l0 = rp.RevoluteDH(alpha=-np.pi / 2)
l1 = rp.RevoluteDH(d=1.0)
l2 = rp.RevoluteDH(alpha=np.pi / 2)
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([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_fail(self):
l0 = rp.RevoluteDH(alpha=np.pi / 2)
l1 = rp.RevoluteDH(d=1.0)
l2 = rp.RevoluteDH(alpha=np.pi / 2)
l3 = rp.RevoluteDH(alpha=-np.pi / 2)
l4a = rp.RevoluteDH(alpha=np.pi / 2)
l4b = rp.RevoluteDH()
l5 = rp.RevoluteDH()
l6 = rp.RevoluteMDH()
r0 = rp.DHRobot([l0, l1, l2, l3, l4a, l5])
r1 = rp.DHRobot([l0, l1, l2, l3, l4b, l5])
r2 = rp.DHRobot([l1, l2, l3])
r3 = rp.DHRobot([l6, l6, l6, l6, l6, l6])
puma = rp.models.DH.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_simple(self):
l0 = rp.RevoluteDH(alpha=-np.pi / 2)
l1 = rp.RevoluteDH()
l2 = rp.RevoluteDH(alpha=np.pi / 2)
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([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_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 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_isspherical(self):
l0 = rp.RevoluteDH()
l1 = rp.RevoluteDH(alpha=-np.pi / 2)
l2 = rp.RevoluteDH(alpha=np.pi / 2)
l3 = rp.RevoluteDH()
r0 = rp.DHRobot([l0, l1, l2, l3])
r1 = rp.DHRobot([l0, l1])
r2 = rp.DHRobot([l1, l2, l3, l0])
self.assertTrue(r0.isspherical())
self.assertFalse(r1.isspherical())
self.assertFalse(r2.isspherical())
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_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_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_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_add_error(self):
l0 = rp.PrismaticDH()
l1 = rp.RevoluteDH()
r0 = rp.DHRobot([l0, l1])
with self.assertRaises(TypeError):
r0 + 2
def test_add_DHRobot(self):
l0 = rp.PrismaticDH()
l1 = rp.RevoluteDH()
l2 = rp.PrismaticDH(theta=2.0)
l3 = rp.RevoluteDH()
r0 = rp.DHRobot([l0, l1])
r1 = rp.DHRobot([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 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_friction(self):
l0 = rp.RevoluteDH(d=2, B=3, G=2, Tc=[2, -1])
qd = [1, 2, 3, 4]
r0 = rp.DHRobot([l0, l0, l0, l0])
tau = np.array([-16, -28, -40, -52])
nt.assert_array_almost_equal(r0.friction(qd), tau)
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_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_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_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_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_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_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_copy(self):
l0 = rp.RevoluteDH()
r = rp.DHRobot([l0])
l1 = l0.copy()
l0.m = 4
l0.r[1] = 5
self.assertEqual(l1.m, 0)
self.assertEqual(l1.r[1], 0)
self.assertIs(l0._robot, r)
self.assertIs(l1._robot, r)
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_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_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_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 __add__(self, L):
if isinstance(L, DHLink):
return rp.DHRobot([self, L])
elif isinstance(L, rp.DHRobot):
nlinks = [self]
# TODO - Should I do a deep copy here a physically copy the Links
# and not just the references?
# Copy Link references to new list
for i in range(L.n):
nlinks.append(L.links[i])
return rp.DHRobot(nlinks,
name=L.name,
manufacturer=L.manufacturer,
base=L.base,
tool=L.tool,
gravity=L.gravity)
else:
raise TypeError("Cannot add a Link with a non Link object")
