Skip to content

Commit f7fc838

Browse files
jhavljhavl
committed
Added tests for ETS class
1 parent 02f0cde commit f7fc838

File tree

1 file changed

+253
-0
lines changed

1 file changed

+253
-0
lines changed

‎test/test_ets.py

Lines changed: 253 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,253 @@
1+
#!/usr/bin/env python3
2+
# -*- coding: utf-8 -*-
3+
"""
4+
Created on Fri May 1 14:04:04 2020
5+
@author: Jesse Haviland
6+
"""
7+
8+
import numpy.testing as nt
9+
import numpy as np
10+
import roboticstoolbox as rp
11+
import unittest
12+
13+
14+
class TestETS(unittest.TestCase):
15+
16+
def test_str(self):
17+
panda = rp.Panda()
18+
19+
ans = '\nPanda (Franka Emika): 7 axis, RzRzRzRzRzRzRz, ETS\n'\
20+
'Elementary Transform Sequence:\n'\
21+
'[tz(0.333), Rz(q1), Rx(-90), Rz(q2), Rx(90), tz(0.316), '\
22+
'Rz(q3), tx(0.0825), Rx(90), Rz(q4), tx(-0.0825), Rx(-90), '\
23+
'tz(0.384), Rz(q5), Rx(90), Rz(q6), tx(0.088), Rx(90), '\
24+
'tz(0.107), Rz(q7)]\n'\
25+
'tool: t = (0, 0, 0.103), RPY/xyz = (0, 0, -45) deg'
26+
27+
self.assertEqual(str(panda), ans)
28+
29+
def test_str_ets(self):
30+
panda = rp.Panda()
31+
32+
ans = '[tz(0.333), Rz(q1), Rx(-90), Rz(q2), Rx(90), tz(0.316), '\
33+
'Rz(q3), tx(0.0825), Rx(90), Rz(q4), tx(-0.0825), Rx(-90), '\
34+
'tz(0.384), Rz(q5), Rx(90), Rz(q6), tx(0.088), Rx(90), '\
35+
'tz(0.107), Rz(q7)]'
36+
37+
self.assertEqual(str(panda.ets), ans)
38+
39+
def test_fkine(self):
40+
panda = rp.Panda()
41+
q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
42+
43+
ans = np.array([
44+
[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
45+
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
46+
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
47+
[0., 0., 0., 1.]
48+
])
49+
50+
nt.assert_array_almost_equal(panda.fkine(q), ans)
51+
52+
def test_jacob0(self):
53+
panda = rp.Panda()
54+
q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
55+
56+
ans = np.array([
57+
[-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
58+
3.35029257e-01, -1.07195463e-02, 1.03187865e-01,
59+
0.00000000e+00],
60+
[4.46822947e-01, 6.25741987e-01, 4.16474664e-01,
61+
-8.04745724e-02, 7.78257566e-02, -1.17720983e-02,
62+
0.00000000e+00],
63+
[0.00000000e+00, -2.35276631e-01, -8.20187641e-02,
64+
-5.14076923e-01, -9.98040745e-03, -2.02626953e-01,
65+
0.00000000e+00],
66+
[1.29458954e-16, -9.85449730e-01, 3.37672585e-02,
67+
-6.16735653e-02, 6.68449878e-01, -1.35361558e-01,
68+
6.37462344e-01],
69+
[9.07021273e-18, 1.69967143e-01, 1.95778638e-01,
70+
9.79165111e-01, 1.84470262e-01, 9.82748279e-01,
71+
1.83758244e-01],
72+
[1.00000000e+00, -2.26036604e-17, 9.80066578e-01,
73+
-1.93473657e-01, 7.20517510e-01, -1.26028049e-01,
74+
7.48247732e-01]
75+
])
76+
77+
nt.assert_array_almost_equal(panda.jacob0(q), ans)
78+
79+
def test_hessian0(self):
80+
panda = rp.Panda()
81+
q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
82+
83+
ans = np.array([
84+
[
85+
[-4.46822947e-01, -6.25741987e-01, -4.16474664e-01,
86+
8.04745724e-02, -7.78257566e-02, 1.17720983e-02,
87+
0.00000000e+00],
88+
[-6.25741987e-01, -3.99892968e-02, -1.39404950e-02,
89+
-8.73761859e-02, -1.69634134e-03, -3.44399243e-02,
90+
0.00000000e+00],
91+
[-4.16474664e-01, -1.39404950e-02, -4.24230421e-01,
92+
-2.17748413e-02, -7.82283735e-02, -2.81325889e-02,
93+
0.00000000e+00],
94+
[8.04745724e-02, -8.73761859e-02, -2.17748413e-02,
95+
-5.18935898e-01, 5.28476698e-03, -2.00682834e-01,
96+
0.00000000e+00],
97+
[-7.78257566e-02, -1.69634134e-03, -7.82283735e-02,
98+
5.28476698e-03, -5.79159088e-02, -2.88966443e-02,
99+
0.00000000e+00],
100+
[1.17720983e-02, -3.44399243e-02, -2.81325889e-02,
101+
-2.00682834e-01, -2.88966443e-02, -2.00614904e-01,
102+
0.00000000e+00],
103+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
104+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
105+
0.00000000e+00]
106+
],
107+
[
108+
[-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
109+
3.35029257e-01, -1.07195463e-02, 1.03187865e-01,
110+
0.00000000e+00],
111+
[1.07925929e-01, -2.31853293e-01, -8.08253690e-02,
112+
-5.06596965e-01, -9.83518983e-03, -1.99678676e-01,
113+
0.00000000e+00],
114+
[-3.41453006e-02, -8.08253690e-02, -3.06951191e-02,
115+
3.45709946e-01, -1.01688580e-02, 1.07973135e-01,
116+
0.00000000e+00],
117+
[3.35029257e-01, -5.06596965e-01, 3.45709946e-01,
118+
-9.65242924e-02, 1.45842251e-03, -3.24608603e-02,
119+
0.00000000e+00],
120+
[-1.07195463e-02, -9.83518983e-03, -1.01688580e-02,
121+
1.45842251e-03, -1.05221866e-03, 2.09794626e-01,
122+
0.00000000e+00],
123+
[1.03187865e-01, -1.99678676e-01, 1.07973135e-01,
124+
-3.24608603e-02, 2.09794626e-01, -4.04324654e-02,
125+
0.00000000e+00],
126+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
127+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
128+
0.00000000e+00]
129+
],
130+
[
131+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
132+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
133+
0.00000000e+00],
134+
[0.00000000e+00, -6.34981134e-01, -4.04611266e-01,
135+
2.23596800e-02, -7.48714002e-02, -5.93773551e-03,
136+
0.00000000e+00],
137+
[0.00000000e+00, -4.04611266e-01, 2.07481281e-02,
138+
-6.83089775e-02, 4.72662062e-03, -2.05994912e-02,
139+
0.00000000e+00],
140+
[0.00000000e+00, 2.23596800e-02, -6.83089775e-02,
141+
-3.23085806e-01, 5.69641385e-03, -1.00311930e-01,
142+
0.00000000e+00],
143+
[0.00000000e+00, -7.48714002e-02, 4.72662062e-03,
144+
5.69641385e-03, 5.40000550e-02, -2.69041502e-02,
145+
0.00000000e+00],
146+
[0.00000000e+00, -5.93773551e-03, -2.05994912e-02,
147+
-1.00311930e-01, -2.69041502e-02, -9.98142073e-02,
148+
0.00000000e+00],
149+
[0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
150+
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
151+
0.00000000e+00]
152+
],
153+
[
154+
[-9.07021273e-18, -2.77555756e-17, -2.77555756e-17,
155+
-1.11022302e-16, -2.77555756e-17, 0.00000000e+00,
156+
-2.77555756e-17],
157+
[-1.69967143e-01, -1.97756387e-17, 4.11786040e-17,
158+
-1.48932398e-16, -5.07612940e-17, -8.38219650e-17,
159+
-4.90138154e-17],
160+
[-1.95778638e-01, 1.66579116e-01, -1.38777878e-17,
161+
1.04083409e-17, -1.38777878e-17, 3.46944695e-18,
162+
0.00000000e+00],
163+
[-9.79165111e-01, -3.28841647e-02, -9.97525009e-01,
164+
-4.16333634e-17, -1.14491749e-16, 1.38777878e-17,
165+
-6.24500451e-17],
166+
[-1.84470262e-01, 1.22464303e-01, -3.97312016e-02,
167+
7.41195745e-01, -2.77555756e-17, 1.12757026e-16,
168+
2.77555756e-17],
169+
[-9.82748279e-01, -2.14206274e-02, -9.87832342e-01,
170+
6.67336352e-02, -7.31335770e-01, 2.08166817e-17,
171+
-6.07153217e-17],
172+
[-1.83758244e-01, 1.27177529e-01, -3.36043908e-02,
173+
7.68210453e-01, 5.62842325e-03, 7.58497864e-01,
174+
0.00000000e+00]
175+
],
176+
[
177+
[1.29458954e-16, -1.11022302e-16, 8.67361738e-17,
178+
-4.16333634e-17, 5.55111512e-17, 2.77555756e-17,
179+
5.55111512e-17],
180+
[-9.85449730e-01, -6.36381327e-17, -1.02735399e-16,
181+
-1.83043043e-17, -5.63484308e-17, 8.08886307e-18,
182+
1.07112702e-18],
183+
[3.37672585e-02, 9.65806345e-01, 8.32667268e-17,
184+
-2.55871713e-17, 1.07552856e-16, 2.08166817e-17,
185+
-5.20417043e-18],
186+
[-6.16735653e-02, -1.90658563e-01, -5.39111251e-02,
187+
-6.59194921e-17, -2.77555756e-17, 2.38524478e-17,
188+
-4.16333634e-17],
189+
[6.68449878e-01, 7.10033786e-01, 6.30795483e-01,
190+
-8.48905588e-02, 0.00000000e+00, 3.46944695e-17,
191+
2.77555756e-17],
192+
[-1.35361558e-01, -1.24194307e-01, -1.28407717e-01,
193+
1.84162966e-02, -1.32869389e-02, 2.77555756e-17,
194+
-2.08166817e-17],
195+
[6.37462344e-01, 7.37360525e-01, 5.99489263e-01,
196+
-7.71850655e-02, -4.08633244e-02, 2.09458434e-02,
197+
0.00000000e+00]
198+
],
199+
[
200+
[0.00000000e+00, -6.59521910e-17, -1.31033786e-16,
201+
-1.92457571e-16, 1.54134782e-17, -7.69804929e-17,
202+
1.11140361e-17],
203+
[0.00000000e+00, -2.77555756e-17, 7.15573434e-17,
204+
1.65666092e-16, 1.38777878e-17, -8.67361738e-18,
205+
3.46944695e-17],
206+
[0.00000000e+00, -1.98669331e-01, 8.67361738e-18,
207+
-1.46584134e-16, 6.02816408e-17, -3.12250226e-17,
208+
6.11490025e-17],
209+
[0.00000000e+00, -9.54435515e-01, 4.51380881e-02,
210+
1.38777878e-17, 1.08420217e-16, 3.46944695e-18,
211+
6.24500451e-17],
212+
[0.00000000e+00, -2.95400686e-01, -1.24639152e-01,
213+
-6.65899738e-01, -4.85722573e-17, -5.20417043e-18,
214+
-5.55111512e-17],
215+
[0.00000000e+00, -9.45442009e-01, 5.96856167e-02,
216+
7.19317248e-02, 6.81888149e-01, -2.77555756e-17,
217+
1.04083409e-17],
218+
[0.00000000e+00, -2.89432165e-01, -1.18596498e-01,
219+
-6.35513913e-01, 5.24032975e-03, -6.51338823e-01,
220+
0.00000000e+00]
221+
]
222+
])
223+
224+
nt.assert_array_almost_equal(panda.hessian0(q), ans)
225+
226+
def test_manipulability(self):
227+
panda = rp.Panda()
228+
q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
229+
230+
ans = 0.006559178039088341
231+
232+
nt.assert_array_almost_equal(panda.manipulability(q), ans)
233+
234+
def test_jacobm(self):
235+
panda = rp.Panda()
236+
q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
237+
238+
ans = np.array([
239+
[1.27080875e-17],
240+
[2.38242538e-02],
241+
[6.61029519e-03],
242+
[8.18202121e-03],
243+
[7.74546204e-04],
244+
[-1.10885380e-02],
245+
[0.00000000e+00]
246+
])
247+
248+
nt.assert_array_almost_equal(panda.jacobm(q), ans)
249+
250+
251+
if __name__ == '__main__':
252+
253+
unittest.main()

0 commit comments

Comments
 (0)