Norm

\[\|\mathbf{x}\|_{p}=\left(\sum_{i=1}^{N}\left|x_{i}\right|^{p}\right)^{\frac{1}{p}}\]

  • One-norm :
    \[\|x\|_{1}=\sum_{i=1}^{N}\left|x_{i}\right|\]

  • Euclidean norm(L2-norm) :
    \[\|x\|_{2}=\left(\sum_{i=1}^{N} x_{i}^{2}\right)^{\frac{1}{2}}\]

  • Maximum norm: \[\|x\|_{∞}=\max _{i}\left|x_{i}\right|\]

Spatial Descriptions

Rotation matrix

\[ ^A_BR = \left[ ^AX_B, ^AY_B, ^AZ_B \right] = \begin{bmatrix} X_B \cdot X_A & Y_B \cdot X_A & Z_B \cdot X_A \\ X_B \cdot Y_A & Y_B \cdot Y_A & Z_B \cdot Y_A \\ X_B \cdot Z_A & Y_B \cdot Z_A & Z_B \cdot Z_A \\ \end{bmatrix} \]

Homogeneous transform

\[ \begin{bmatrix} ^AP \\ 1 \end{bmatrix} = \begin{bmatrix} \begin{array}{ccc|c} &^A_BR& & ^AP_{BORG} \\ \hline 0& 0& 0 & 1 \\ \end{array} \end{bmatrix} \begin{bmatrix} ^BP \\ 1 \end{bmatrix} \]

\[ ^A_BT = \begin{bmatrix} \begin{array}{ccc|c} &^A_BR& & ^AP_{BORG} \\ \hline 0& 0& 0 & 1 \\ \end{array} \end{bmatrix} \]

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
import sympy as sp

def translate_matrix(dx, dy, dz):
    return sp.Matrix([[1, 0, 0, dx],
              [0, 1, 0, dy],
              [0, 0, 1, dz],
              [0, 0, 0, 1]])

def rotate_matrix_by_fixed_angles(roll, pitch, yaw):
    '''
    Step1: Rx
    Step2: Ry
    Step3: Rz
    '''
    Rx = sp.Matrix([[1, 0, 0, 0],
            [0, sp.cos(roll), -sp.sin(roll), 0],
            [0, sp.sin(roll), sp.cos(roll), 0],
            [0, 0, 0, 1]])
    
    Ry = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch), 0],
            [0, 1, 0, 0],
            [-sp.sin(pitch), 0, sp.cos(pitch), 0],
            [0, 0, 0, 1]])
    
    Rz = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0, 0],
            [sp.sin(yaw), sp.cos(yaw), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])
    
    return Rz * Ry * Rx

def rotate_by_euler_angles(gamma, beta, alpha):
    '''
    Step1: Rz
    Step2: Ry
    Step3: Rx
    '''
    Rx = sp.Matrix([[1, 0, 0, 0],
            [0, sp.cos(gamma), -sp.sin(gamma), 0],
            [0, sp.sin(gamma), sp.cos(gamma), 0],
            [0, 0, 0, 1]])
    
    Ry = sp.Matrix([[sp.cos(beta), 0, sp.sin(beta), 0],
            [0, 1, 0, 0],
            [-sp.sin(beta), 0, sp.cos(beta), 0],
            [0, 0, 0, 1]])
    
    Rz = sp.Matrix([[sp.cos(alpha), -sp.sin(alpha), 0, 0],
            [sp.sin(alpha), sp.cos(alpha), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]])
    
    return Rz * Ry * Rx

dx, dy, dz = sp.symbols('dx dy dz')
roll, pitch, yaw = sp.symbols('gamma beta alpha')
gamma, beta, alpha = sp.symbols('gamma beta alpha')

# Shift
translation_matrix_symbolic = translate_matrix(dx, dy, dz)

# Rotaion
rotation_matrix_by_fixed_angles = rotate_matrix_by_fixed_angles(roll, pitch, yaw)
rotation_matrix_by_euler_angles = rotate_by_euler_angles(gamma, beta, alpha)

# combine
transformation_matrix_fixed_angles = translation_matrix_symbolic * rotation_matrix_by_fixed_angles
transformation_matrix_euler_angles = translation_matrix_symbolic * rotation_matrix_by_euler_angles

# input_values
input_values = {dx: 1, dy: 2, dz: 3, roll: sp.rad(30), pitch: sp.rad(30), yaw: sp.rad(60),\
                gamma: sp.rad(30), beta: sp.rad(30), alpha: sp.rad(60)}

# calculate
result_fixed_angles = rotation_matrix_by_fixed_angles.subs(input_values)
result_euler_angles = rotation_matrix_by_euler_angles.subs(input_values)


print("Transformation Matrix (Fixed Angles):")
sp.pprint(transformation_matrix_fixed_angles, use_unicode=True)
print('----------------')

print("\nTransformation Matrix (Euler Angles):")
sp.pprint(transformation_matrix_euler_angles, use_unicode=True)
print('----------------')

print("\nTransformation Matrix (Fixed Angles, Result):")
sp.pprint(result_fixed_angles, use_unicode=True)
print('----------------')
print("\nTransformation Matrix (Euler Angles, Result):")
sp.pprint(result_euler_angles, use_unicode=True)
print('----------------')
  • Result
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
Transformation Matrix (Fixed Angles):
⎡cos(α)⋅cos(β)  -sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)  sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ)  dx   ⎤
⎢                                                                                              ⎥
⎢sin(α)⋅cos(β)  sin(α)⋅sin(β)⋅sin(γ) + cos(α)⋅cos(γ)   sin(α)⋅sin(β)⋅cos(γ) - sin(γ)⋅cos(α)  dy   ⎥
⎢                                                                                              ⎥
⎢   -sin(β)                 sin(γ)⋅cos(β)                         cos(β)⋅cos(γ)              dz ⎥
⎢                                                                                              ⎥
⎣      0                          0                                     0                    1 ⎦
----------------

Transformation Matrix (Euler Angles):
⎡cos(α)⋅cos(β)  -sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)  sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ)  dx   ⎤
⎢                                                                                              ⎥
⎢sin(α)⋅cos(β)  sin(α)⋅sin(β)⋅sin(γ) + cos(α)⋅cos(γ)   sin(α)⋅sin(β)⋅cos(γ) - sin(γ)⋅cos(α)  dy   ⎥
⎢                                                                                              ⎥
⎢   -sin(β)                 sin(γ)⋅cos(β)                         cos(β)⋅cos(γ)              dz ⎥
⎢                                                                                              ⎥
⎣      0                          0                                     0                    1 ⎦
----------------

Transformation Matrix (Fixed Angles, Result):
⎡ √3         3⋅√3   ⎤
⎢ ──   -5/8  ────  0⎥
⎢ 4           8     ⎥
⎢                   ⎥
⎢      3⋅√3         ⎥
⎢3/4   ────  1/8   0⎥
⎢       8           ⎥
⎢                   ⎥
⎢       √3          ⎥
⎢-1/2   ──   3/4   0⎥
⎢       4           ⎥
⎢                   ⎥
⎣ 0     0     0    1⎦
----------------

Transformation Matrix (Euler Angles, Result):
⎡ √3         3⋅√3   ⎤
⎢ ──   -5/8  ────  0⎥
⎢ 4           8     ⎥
⎢                   ⎥
⎢      3⋅√3         ⎥
⎢3/4   ────  1/8   0⎥
⎢       8           ⎥
⎢                   ⎥
⎢       √3          ⎥
⎢-1/2   ──   3/4   0⎥
⎢       4           ⎥
⎢                   ⎥
⎣ 0     0     0    1⎦
----------------