8000 New tests for angular velocity Jacobians · Zebiao1998/spatialmath-python@6e9d83b · GitHub
[go: up one dir, main page]
Skip to content

Commit 6e9d83b

Browse files
petercorkepetercorke
committed
New tests for angular velocity Jacobians
1 parent 56d4e83 commit 6e9d83b

File tree

1 file changed

+152
-0
lines changed

1 file changed

+152
-0
lines changed

‎tests/base/test_velocity.py

Lines changed: 152 additions & 0 deletions
341A
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,152 @@
1+
#!/usr/bin/env python3
2+
# -*- coding: utf-8 -*-
3+
"""
4+
Created on Fri Apr 10 14:19:04 2020
5+
6+
@author: corkep
7+
8+
"""
9+
10+
11+
import numpy as np
12+
import numpy.testing as nt
13+
import unittest
14+
from math import pi
15+
import math
16+
from scipy.linalg import logm, expm
17+
18+
from spatialmath.base.transforms3d import *
19+
from spatialmath.base.numeric import *
20+
from spatialmath.base.transformsNd import isR, t2r, r2t, rt2tr
21+
22+
import matplotlib.pyplot as plt
23+
24+
25+
class TestVelocity(unittest.TestCase):
26+
27+
def test_numjac(self):
28+
29+
# test on algebraic example
30+
def f(X):
31+
x = X[0]
32+
y = X[1]
33+
return np.r_[x, x**2, x*y**2]
34+
35+
nt.assert_array_almost_equal(numjac(f, [2, 3]),
36+
np.array([
37+
[1, 0], # x, 0
38+
[4, 0], # 2x, 0
39+
[9, 12] # y^2, 2xy
40+
]))
41+
42+
# test on rotation matrix
43+
nt.assert_array_almost_equal(numjac(rotx, [0], SO=3),
44+
np.array([[1, 0, 0]]).T)
45+
46+
nt.assert_array_almost_equal(numjac(rotx, [pi / 2], SO=3),
47+
np.array([[1, 0, 0]]).T)
48+
49+
nt.assert_array_almost_equal(numjac(roty, [0], SO=3),
50+
np.array([[0, 1, 0]]).T)
51+
52+
nt.assert_array_almost_equal(numjac(rotz, [0], SO=3),
53+
np.array([[0, 0, 1]]).T)
54+
55+
def test_rpy2jac(self):
56+
57+
# ZYX order
58+
gamma = [0, 0, 0]
59+
nt.assert_array_almost_equal(rpy2jac(gamma), numjac(rpy2r, gamma, SO=3))
60+
gamma = [pi / 4, 0, -pi / 4]
61+
nt.assert_array_almost_equal(rpy2jac(gamma), numjac(rpy2r, gamma, SO=3))
62+
gamma = [-pi / 4, pi / 2, pi / 4]
63+
nt.assert_array_almost_equal(rpy2jac(gamma), numjac(rpy2r, gamma, SO=3))
64+
65+
# XYZ order
66+
f = lambda gamma: rpy2r(gamma, order='xyz')
67+
gamma = [0, 0, 0]
68+
nt.assert_array_almost_equal(rpy2jac(gamma, order='xyz'), numjac(f, gamma, SO=3))
69+
f = lambda gamma: rpy2r(gamma, order='xyz')
70+
gamma = [pi / 4, 0, -pi / 4]
71+
nt.assert_array_almost_equal(rpy2jac(gamma, order='xyz'), numjac(f, gamma, SO=3))
72+
f = lambda gamma: rpy2r(gamma, order='xyz')
73+
gamma = [-pi / 4, pi / 2, pi / 4]
74+
nt.assert_array_almost_equal(rpy2jac(gamma, order='xyz'), numjac(f, gamma, SO=3))
75+
76+
77+
def test_eul2jac(self):
78+
79+
# ZYX order
80+
gamma = [0, 0, 0]
81+
nt.assert_array_almost_equal(eul2jac(gamma), numjac(eul2r, gamma, SO=3))
82+
gamma = [pi / 4, 0, -pi / 4]
83+
nt.assert_array_almost_equal(eul2jac(gamma), numjac(eul2r, gamma, SO=3))
84+
gamma = [-pi / 4, pi / 2, pi / 4]
85+
nt.assert_array_almost_equal(eul2jac(gamma), numjac(eul2r, gamma, SO=3))
86+
87+
def test_exp2jac(self):
88+
89+
# ZYX order
90+
gamma = np.r_[1, 0, 0]
91+
nt.assert_array_almost_equal(exp2jac(gamma), numjac(exp2r, gamma, SO=3))
92+
gamma = np.r_[0.2, 0.3, 0.4]
93+
nt.assert_array_almost_equal(exp2jac(gamma), numjac(exp2r, gamma, SO=3))
94+
gamma = np.r_[0, 0, 0]
95+
nt.assert_array_almost_equal(exp2jac(gamma), numjac(exp2r, gamma, SO=3))
96+
97+
def test_rot2jac(self):
98+
99+
gamma = [0.1, 0.2, 0.3]
100+
R = rpy2r(gamma, order='zyx')
101+
A = rot2jac(R, representation='rpy/zyx')
102+
self.assertEqual(A.shape, (6,6))
103+
A3 = np.linalg.inv(A[3:6,3:6])
104+
nt.assert_array_almost_equal(A3, rpy2jac(gamma, order='zyx'))
105+
106+
gamma = [0.1, 0.2, 0.3]
107+
R = rpy2r(gamma, order='xyz')
108+
A = rot2jac(R, representation='rpy/xyz')
109+
self.assertEqual(A.shape, (6,6))
110+
A3 = np.linalg.inv(A[3:6,3:6])
111+
nt.assert_array_almost_equal(A3, rpy2jac(gamma, order='xyz'))
112+
113+
gamma = [0.1, 0.2, 0.3]
114+
R = eul2r(gamma)
115+
A = rot2jac(R, representation='eul')
116+
self.assertEqual(A.shape, (6,6))
117+
A3 = np.linalg.inv(A[3:6,3:6])
118+
nt.assert_array_almost_equal(A3, eul2jac(gamma))
119+
120+
121+
def test_angvelxform(self):
122+
123+
gamma = [0.1, 0.2, 0.3]
124+
A = angvelxform(gamma, full=False, representation='rpy/zyx')
125+
Ai = angvelxform(gamma, full=False, inverse=True, representation='rpy/zyx')
126+
nt.assert_array_almost_equal(Ai, rpy2jac(gamma, order='zyx'))
127+
nt.assert_array_almost_equal(A @ Ai, np.eye(3))
128+
129+
gamma = [0.1, 0.2, 0.3]
130+
A = angvelxform(gamma, full=False, representation='rpy/xyz')
131+
Ai = angvelxform(gamma, full=False, inverse=True, representation='rpy/xyz')
132+
nt.assert_array_almost_equal(Ai, rpy2jac(gamma, order='xyz'))
133+
nt.assert_array_almost_equal(A @ Ai, np.eye(3))
134+
135+
gamma = [0.1, 0.2, 0.3]
136+
A = angvelxform(gamma, full=False, representation='eul')
137+
Ai = angvelxform(gamma, full=False, inverse=True, representation='eul')
138+
nt.assert_array_almost_equal(Ai, eul2jac(gamma))
139+
nt.assert_array_almost_equal(A @ Ai, np.eye(3))
140+
141+
# def test_angvelxform_dot(self):
142+
143+
# gamma = [0.1, 0.2, 0.3]
144+
# options = dict(full=False, representation='rpy/zyx')
145+
146+
# f = lambda gamma: angvelxform(gamma, options)
147+
148+
# nt.assert_array_almost_equal(angvelxform_dot(gamma, options), numjac(f))
149+
# ---------------------------------------------------------------------------------------#
150+
if __name__ == '__main__':
151+
152+
unittest.main()

0 commit comments

Comments
 (0)
0