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