-
Notifications
You must be signed in to change notification settings - Fork 12
/
Copy pathrototranslation.py
150 lines (117 loc) · 5.22 KB
/
rototranslation.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
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
import collections
from math import cos
from math import radians
from math import sin
import numpy as np
from numpy import matmul
Vector = collections.namedtuple('Vector', ['x', 'y', 'z'])
class RotoTranslation:
def __init__(self, rotation: Vector, translation: Vector, angle_unit: str, notation: str='XYZ'):
self.rotation = rotation
self.translation = translation
self.angle_unit = angle_unit
if self.angle_unit == 'degrees':
self.rotation = Vector(*[radians(alpha) for alpha in rotation])
self.R_x = None
self.R_y = None
self.R_z = None
self.T = None
self.matrix = None
self.notation = notation
self._update_matrix()
def _update_matrix(self):
# Convention: counter-clockwise rotation
self.R_x = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, cos(self.rotation.x), -sin(self.rotation.x), 0.0],
[0.0, sin(self.rotation.x), cos(self.rotation.x), 0.0],
[0.0, 0.0, 0.0, 1.0]])
self.R_y = np.array([[cos(self.rotation.y), 0.0, sin(self.rotation.y), 0.0],
[0.0, 1.0, 0.0, 0.0],
[-sin(self.rotation.y), 0.0, cos(self.rotation.y), 0.0],
[0.0, 0.0, 0.0, 1.0]])
self.R_z = np.array([[cos(self.rotation.z), -sin(self.rotation.z), 0.0, 0.0],
[sin(self.rotation.z), cos(self.rotation.z), 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
self.T = np.array([[1.0, 0.0, 0.0, self.translation.x],
[0.0, 1.0, 0.0, self.translation.y],
[0.0, 0.0, 1.0, self.translation.z],
[0.0, 0.0, 0.0, 1.0]])
if self.notation == 'XYZ':
self.R = matmul(self.R_z, matmul(self.R_y, self.R_x))
if self.notation == 'YXZ':
self.R = matmul(self.R_z, matmul(self.R_x, self.R_y))
if self.notation == 'YZX':
self.R = matmul(self.R_x, matmul(self.R_z, self.R_y))
if self.notation == 'XZY':
self.R = matmul(self.R_y, matmul(self.R_z, self.R_x))
if self.notation == 'ZYX':
self.R = matmul(self.R_x, matmul(self.R_y, self.R_z))
if self.notation == 'ZXY':
self.R = matmul(self.R_y, matmul(self.R_x, self.R_z))
self.matrix = matmul(self.T, self.R)
def __str__(self):
return RotoTranslation.pretty_string(self.matrix)
@staticmethod
def pretty_string(matrix, border=False, border_len=40):
pretty_str = '\n'.join([''.join(['{:9.3f}'.format(item) for item in row]) for row in matrix])
if border:
pretty_str = border_len * '*' + '\n' + pretty_str + '\n' + border_len * '*'
return pretty_str
@property
def alpha_x(self):
alpha_x = self.rotation.x
if self.angle_unit == 'degrees':
alpha_x = np.degrees(alpha_x)
return alpha_x
@property
def alpha_y(self):
alpha_y = self.rotation.y
if self.angle_unit == 'degrees':
alpha_y = np.degrees(alpha_y)
return alpha_y
@property
def alpha_z(self):
alpha_z = self.rotation.z
if self.angle_unit == 'degrees':
alpha_z = np.degrees(alpha_z)
return alpha_z
@property
def t_x(self):
return self.translation.x
@property
def t_y(self):
return self.translation.y
@property
def t_z(self):
return self.translation.z
@alpha_x.setter
def alpha_x(self, value):
if self.angle_unit == 'degrees':
value = np.radians(value)
self.rotation = Vector(value, self.rotation.y, self.rotation.z)
self._update_matrix()
@alpha_y.setter
def alpha_y(self, value):
if self.angle_unit == 'degrees':
value = np.radians(value)
self.rotation = Vector(self.rotation.x, value, self.rotation.z)
self._update_matrix()
@alpha_z.setter
def alpha_z(self, value):
if self.angle_unit == 'degrees':
value = np.radians(value)
self.rotation = Vector(self.rotation.x, self.rotation.y, value)
self._update_matrix()
@t_x.setter
def t_x(self, value):
self.translation = Vector(value, self.translation.y, self.translation.z)
self._update_matrix()
@t_y.setter
def t_y(self, value):
self.translation = Vector(self.translation.x, value, self.translation.z)
self._update_matrix()
@t_z.setter
def t_z(self, value):
self.translation = Vector(self.translation.x, self.translation.y, value)
self._update_matrix()