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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy