forked from WillianPaiva/uerobotics
-
Notifications
You must be signed in to change notification settings - Fork 0
/
test_matrices.py
30 lines (20 loc) · 841 Bytes
/
test_matrices.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
from __future__ import print_function, division
import unittest
import sympy
import matrices
class TestMatrices(unittest.TestCase):
def test_hm_inverse_x_sorting(self):
theta = sympy.Symbol('theta')
alpha = sympy.Symbol('alpha')
r = sympy.Symbol('r')
d = sympy.Symbol('d')
hm = matrices.HomMatrix(alpha, r, d, theta)
self.assertEqual(sympy.trigsimp(hm.m*hm.m_1), sympy.Matrix.eye(4))
def test_transformation(self):
theta = sympy.Symbol('theta')
trans = matrices.TTransX( theta)
trans_1 = matrices.TTransX(-theta)
self.assertEqual(sympy.trigsimp(trans.m*trans_1.m), sympy.Matrix.eye(4))
rot = matrices.TRotX( theta)
rot_1 = matrices.TRotX(-theta)
self.assertEqual(sympy.trigsimp(rot.m*rot_1.m), sympy.Matrix.eye(4))