8000 split ik tests to test_Robot · liang324/robotics-toolbox-python@50e795c · GitHub
[go: up one dir, main page]
Skip to content

Commit 50e795c

Browse files
petercorkepetercorke
committed
split ik tests to test_Robot
1 parent cb70179 commit 50e795c

File tree

2 files changed

+115
-82
lines changed

2 files changed

+115
-82
lines changed

‎tests/test_DHRobot.py

Lines changed: 0 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -670,71 +670,6 @@ def test_fkine_all(self):
670670
# nt.assert_array_almost_equal(res3, tauB, decimal=4)
671671
# nt.assert_array_almost_equal(res4, tauB3, decimal=4)
672672

673-
def test_ikcon(self):
674-
panda = rp.models.DH.Panda()
675-
q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
676-
T = panda.fkine(q)
677-
Tt = sm.SE3([T, T, T])
678-
679-
qr = [7.69161412e-04, 9.01409257e-01, -1.46372859e-02,
680-
-6.98000000e-02, 1.38978915e-02, 9.62104811e-01,
681-
7.84926515e-01]
682-
683-
qa, success, err = panda.ikcon(T.A, q0=np.zeros(7))
684-
qa2, success, err = panda.ikcon(Tt)
685-
qa3, _, _ = panda.ikcon(Tt, q0=np.zeros((3, 7)))
686-
687-
nt.assert_array_almost_equal(qa, qr, decimal=4)
688-
nt.assert_array_almost_equal(qa2[0, :], qr, decimal=4)
689-
nt.assert_array_almost_equal(qa2[1, :], qr, decimal=4)
690-
nt.assert_array_almost_equal(qa3[0, :], qr, decimal=4)
691-
nt.assert_array_almost_equal(qa3[1, :], qr, decimal=4)
692-
693-
def test_ikine(self):
694-
panda = rp.models.DH.Panda()
695-
q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
696-
T = panda.fkine(q)
697-
Tt = sm.SE3([T, T])
698-
699-
l0 = rp.RevoluteDH(d=2.0)
700-
l1 = rp.PrismaticDH(theta=1.0)
701-
l2 = rp.PrismaticDH(theta=1, qlim=[0, 2])
702-
r0 = rp.DHRobot([l0, l1])
703-
r1 = rp.DHRobot([l0, l2])
704-
705-
qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489]
706-
707-
qa, success, err = panda.ikine(T)
708-
qa2, success, err = panda.ikine(Tt)
709-
qa3, success, err = panda.ikine(Tt, q0=np.zeros((2, 7)))
710-
qa4, success, err = panda.ikine(T, q0=np.zeros(7))
711-
712-
# Untested
713-
qa5, success, err = r0.ikine(
714-
T.A, mask=[1, 1, 0, 0, 0, 0],
715-
transpose=5, ilimit=5)
716-
qa5, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0])
717-
qa6, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1)
718-
qa7, success, err = r1.ikine(
719-
T, mask=[1, 1, 0, 0, 0, 0],
720-
ilimit=1, search=True, slimit=1)
721-
722-
nt.assert_array_almost_equal(qa, qr, decimal=4)
723-
nt.assert_array_almost_equal(qa2[0, :], qr, decimal=4)
724-
nt.assert_array_almost_equal(qa2[1, :], qr, decimal=4)
725-
nt.assert_array_almost_equal(qa3[1, :], qr, decimal=4)
726-
nt.assert_array_almost_equal(qa4, qr, decimal=4)
727-
728-
with self.assertRaises(ValueError):
729-
panda.ikine(Tt, q0=np.zeros(7))
730-
731-
with self.assertRaises(ValueError):
732-
r0.ikine(T)
733-
734-
with self.assertRaises(ValueError):
735-
r0.ikine(
736-
T, mask=[1, 1, 0, 0, 0, 0], ilimit=1,
737-
search=True, slimit=1)
738673

739674
def test_ikine3(self):
740675
l0 = rp.RevoluteDH(alpha=np.pi / 2)
@@ -947,24 +882,7 @@ def test_ikinem(self):
947882
self.assertTrue(
948883
np.sum(np.abs(T.A - puma.fkine(q1).A)) < 0.7)
949884

950-
def test_ikunc(self):
951-
puma = rp.models.DH.Puma560()
952-
q = puma.qr
953-
T = puma.fkine(q)
954-
Tt = sm.SE3([T, T])
955885

956-
q0, _, _ = puma.ikunc(Tt)
957-
q1, success, _ = puma.ikunc(T.A)
958-
q2, success, _ = puma.ikunc(T, ilimit=1)
959-
960-
nt.assert_array_almost_equal(
961-
T.A - puma.fkine(q0[0, :]).A, np.zeros((4, 4)), decimal=4)
962-
963-
nt.assert_array_almost_equal(
964-
T.A - puma.fkine(q0[1, :]).A, np.zeros((4, 4)), decimal=4)
965-
966-
nt.assert_array_almost_equal(
967-
T.A - puma.fkine(q1).A, np.zeros((4, 4)), decimal=4)
968886

969887
def test_rne(self):
970888
puma = rp.models.DH.Puma560()

‎tests/test_Robot.py

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
"""
2+
@author: Peter Corke
3+
"""
4+
5+
import numpy.testing as nt
6+
import numpy as np
7+
import roboticstoolbox as rp
8+
import spatialmath as sm
9+
import unittest
10+
11+
class TestRobot(unittest.TestCase):
12+
13+
def test_links(self):
14+
15+
l0 = rp.PrismaticDH()
16+
l1 = rp.RevoluteDH()
17+
l2 = rp.PrismaticDH()
18+
l3 = rp.RevoluteDH()
19+
20+
r0 = rp.DHRobot([l0, l1, l2, l3])
21+
22+
self.assertIs(r0[0], l0)
23+
self.assertIs(r0[1], l1)
24+
self.assertIs(r0[2], l2)
25+
self.assertIs(r0[3], l3)
26+
27+
28+
def test_ikine(self):
29+
panda = rp.models.DH.Panda()
30+
q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
31+
T = panda.fkine(q)
32+
Tt = sm.SE3([T, T])
33+
34+
l0 = rp.RevoluteDH(d=2.0)
35+
l1 = rp.PrismaticDH(theta=1.0)
36+
l2 = rp.PrismaticDH(theta=1, qlim=[0, 2])
37+
r0 = rp.DHRobot([l0, l1])
38+
r1 = rp.DHRobot([l0, l2])
39+
40+
qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489]
41+
42+
qa, success, err = panda.ikine(T)
43+
qa2, success, err = panda.ikine(Tt)
44+
qa3, success, err = panda.ikine(Tt, q0=np.zeros((2, 7)))
45+
qa4, success, err = panda.ikine(T, q0=np.zeros(7))
46+
47+
# Untested
48+
qa5, success, err = r0.ikine(
49+
T.A, mask=[1, 1, 0, 0, 0, 0],
50+
transpose=5, ilimit=5)
51+
qa5, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0])
52+
qa6, success, err = r0.ikine(T, mask=[1, 1, 0, 0, 0, 0], ilimit=1)
53+
qa7, success, err = r1.ikine(
54+
T, mask=[1, 1, 0, 0, 0, 0],
55+
ilimit=1, search=True, slimit=1)
56+
57+
nt.assert_array_almost_equal(qa, qr, decimal=4)
58+
nt.assert_array_almost_equal(qa2[0, :], qr, decimal=4)
59+
nt.assert_array_almost_equal(qa2[1, :], qr, decimal=4)
60+
nt.assert_array_almost_equal(qa3[1, :], qr, decimal=4)
61+
nt.assert_array_almost_equal(qa4, qr, decimal=4)
62+
63+
with self.assertRaises(ValueError):
64+
panda.ikine(Tt, q0=np.zeros(7))
65+
66+
with self.assertRaises(ValueError):
67+
r0.ikine(T)
68+
69+
with self.assertRaises(ValueError):
70+
r0.ikine(
71+
T, mask=[1, 1, 0, 0, 0, 0], ilimit=1,
72+
search=True, slimit=1)
73+
74+
def test_ikcon(self):
75+
panda = rp.models.DH.Panda()
76+
q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
77+
T = panda.fkine(q)
78+
Tt = sm.SE3([T, T, T])
79+
80+
qr = [7.69161412e-04, 9.01409257e-01, -1.46372859e-02,
81+
-6.98000000e-02, 1.38978915e-02, 9.62104811e-01,
82+
7.84926515e-01]
83+
84+
qa, success, err = panda.ikcon(T.A, q0=np.zeros(7))
85+
qa2, success, err = panda.ikcon(Tt)
86+
qa3, _, _ = panda.ikcon(Tt, q0=np.zeros((3, 7)))
87+
88+
nt.assert_array_almost_equal(qa, qr, decimal=4)
89+
nt.assert_array_almost_equal(qa2[0, :], qr, decimal=4)
90+
nt.assert_array_almost_equal(qa2[1, :], qr, decimal=4)
91+
nt.assert_array_almost_equal(qa3[0, :], qr, decimal=4)
92+
nt.assert_array_almost_equal(qa3[1, :], qr, decimal=4)
93+
94+
def test_ikunc(self):
95+
puma = rp.models.DH.Puma560()
96+
q = puma.qr
97+
T = puma.fkine(q)
98+
Tt = sm.SE3([T, T])
99+
100+
q0, _, _ = puma.ikunc(Tt)
101+
q1, success, _ = puma.ikunc(T.A)
102+
q2, success, _ = puma.ikunc(T, ilimit=1)
103+
104+
nt.assert_array_almost_equal(
105+
T.A - puma.fkine(q0[0, :]).A, np.zeros((4, 4)), decimal=4)
106+
107+
nt.assert_array_almost_equal(
108+
T.A - puma.fkine(q0[1, :]).A, np.zeros((4, 4)), decimal=4)
109+
110+
nt.assert_array_almost_equal(
111+
T.A - puma.fkine(q1).A, np.zeros((4, 4)), decimal=4)
112+
113+
if __name__ == '__main__':
114+
unittest.main()
115+
# pytest.main(['tests/test_SerialLink.py'])

0 commit comments

Comments
 (0)
0