# Матрицы

Достаточно осведомленные знают, что систему линейных уравнений можно представить в матричной форме.
Действительно,

$$
X = a \cos(\alpha) - b \sin(\alpha) + x \\
Y = a \sin(\alpha) + b \cos(\alpha) + y \\
$$

можно представить как

$$
\begin{bmatrix}
    X \\
    Y
\end{bmatrix} =
\begin{bmatrix}
    \cos(\alpha) & -\sin(\alpha) \\
    \sin(\alpha) & \cos(\alpha)
\end{bmatrix}
\begin{bmatrix}
    a \\
    b
\end{bmatrix} +
\begin{bmatrix}
    x \\
    y
\end{bmatrix}
$$

или как

$$
\begin{bmatrix}
    X \\
    Y \\
    1
\end{bmatrix} =
\begin{bmatrix}
    \cos(\alpha) & -\sin(\alpha) & x \\
    \sin(\alpha) & \cos(\alpha) & y \\
    0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
    a \\
    b \\
    1
\end{bmatrix}
$$

Последний вариант удобен тем, что _комбинирование_ перемещений (смещений и поворотов) достигается простым перемножением таких матриц.


```python
from sympy import *
t = symbols("t")
x, y, z = symbols("x, y, z")
```

Рассмотрим пример комбинирования при работе в плоскости:


```python
a, b = symbols("a, b")
alpha, beta = symbols("alpha, beta")
x1, x2, y1, y2 = symbols("x_1, x_2, y_1, y_2")
X0, Y0 = symbols("X_0, Y_0")
```

Подставим начальное положение $(X_0, Y_0)$ в систему уравнений:


```python
X1 = X0 * cos(alpha) - Y0 * sin(alpha) + x1
X1
```


```python
Y1 = X0 * sin(alpha) + Y0 * cos(alpha) + y1
Y1
```

Подставим результаты во второе подобное уравнение:


```python
X2 = X1 * cos(beta) - Y1 * sin(beta) + x2
X2.simplify()
```


```python
Y2 = X1 * sin(beta) + Y1 * cos(beta) + y2
Y2.simplify()
```

Теперь запишем эти преобразования в матричном виде:


```python
XY = simplify(Matrix([
    [cos(beta), -sin(beta), x2],
    [sin(beta), cos(beta), y2],
    [0, 0, 1]
]) * Matrix([
    [cos(alpha), -sin(alpha), x1],
    [sin(alpha), cos(alpha), y1],
    [0, 0, 1]
]) * Matrix([
    [X0],
    [Y0],
    [1]
]))
XY
```

## Работа в трехмерном пространстве


```python
def rz(a):
    return Matrix([
        [cos(a), -sin(a), 0, 0],
        [sin(a), cos(a), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ])

def ry(a):
    return Matrix([
        [cos(a), 0, sin(a), 0],
        [0, 1, 0, 0],
        [-sin(a), 0, cos(a), 0],
        [0, 0, 0, 1]
    ])

def rx(a):
    return Matrix([
        [1, 0, 0, 0],
        [0, cos(a), -sin(a), 0],
        [0, sin(a), cos(a), 0],
        [0, 0, 0, 1]
    ])

def trs(x, y, z):
    return Matrix([
        [1, 0, 0, x],
        [0, 1, 0, y],
        [0, 0, 1, z],
        [0, 0, 0, 1]
    ])

def vec(x, y, z):
    return Matrix([
        [x],
        [y],
        [z],
        [1]
    ])
```

Рассмотрим манипулятор с шестью степенями подвижности:




```python
q0, q1, q2, q3, q4, q5 = symbols("q_0, q_1, q_2, q_3, q_4, q_5")
l0, l1, l2, l3 = symbols("l_0, l_1, l_2, l_3")
```


```python
base = Matrix([
    [1, 0, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
])
m0 = base * trs(0, 0, l0) * rz(q0)
m1 = m0 * ry(q1)
m2 = m1 * trs(0, 0, l1)
m3 = m2 * ry(q2)
m4 = m3 * trs(0, 0, l2)
m5 = m4 * rz(q3)
m6 = m5 * ry(q4)
m7 = m6 * trs(0, 0, l3)
m8 = m7 * rz(q5)
position = m8 * vec(0, 0, 0)
simplify(position)
```

e9316cad0548656f8550a2f765818d6d0e0da9ca
ipynb
Jupyter Notebook
2 - Matrices.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
2 - Matrices.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
2 - Matrices.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
Qwen/Qwen-72B
1. YES
2. YES