# Скорости

Возьмем стандартные матрицы преобразования:


```python
from sympy import *
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 = symbols("q_0, q_1", cls=Function)
t = symbols("t")
l0, l1 = symbols("l_0, l_1")
```


```python
pos = rz(q0(t)) * trs(l0, 0, 0) * rz(q1(t)) * trs(l1, 0, 0)
simplify(pos)
```

Продиффиринцируем положение такого механизма по оси $X$:


```python
vx = (pos * vec(0, 0, 0)).diff(t)[0]
simplify(vx)
```

И по оси $Y$:


```python
vy = (pos * vec(0, 0, 0)).diff(t)[1]
simplify(vy)
```

Тогда, из полученного результата можно вынести $\dot{q_0}$ и $\dot{q_1}$:

$$
V_x = -l_0 \sin(q_0) \dot{q_0} - l1 (\dot{q_0} + \dot{q_1}) \sin(q_0 + q_1)
$$
$$
V_x = -l_0 \sin(q_0) \cdot \dot{q_0} - l_1 \dot{q_0} \sin(q_0 + q_1) - l_1 \dot{q_1} \sin(q_0 + q_1)
$$

$$
V_x = \dot{q_0} (-l_0 \sin(q_0) - l_1 \sin(q_0 + q_1)) + \dot{q_1} (-l_1 \sin(q_0 + q_1))
$$
$$
V_y = \dot{q_0} (l_0 \cos(q_0) + l_1 \cos(q_0 + q_1)) + \dot{q_1} (l_1 \cos(q_0 + q_1))
$$

Заменим:

`Исходное` | замена
:---|---
`-l_0 \sin(q_0)` | $A$
`-l_1 \sin(q_0 + q_1)` | $B$
`l_0 \cos(q_0)` | $C$
`l_1 \cos(q_0 + q_1)` | $D$

$$
V_x = \dot{q_0} (A + B) + \dot{q_1} B
$$
$$
V_y = \dot{q_0} (C + D) + \dot{q_1} D
$$

Тогда такую запись можно предсавить в матричном виде:

$$
\begin{bmatrix}
    V_x \\
    V_y
\end{bmatrix} = 
\begin{bmatrix}
    A + B & B \\
    C + D & D
\end{bmatrix}
\begin{bmatrix}
    \dot{q_0} \\
    \dot{q_1}
\end{bmatrix}
$$

Если учесть угловую скорость $\omega$ получим:

$$
\omega = \dot{q_0} + \dot{q_1}
$$

$$
\begin{bmatrix}
    V_x \\
    V_y \\
    \omega
\end{bmatrix} = 
\begin{bmatrix}
    A + B & B \\
    C + D & D \\
    1 & 1
\end{bmatrix}
\begin{bmatrix}
    \dot{q_0} \\
    \dot{q_1}
\end{bmatrix}
=
J
\begin{bmatrix}
    \dot{q_0} \\
    \dot{q_1}
\end{bmatrix}
$$

Матрица $J$ называется _матрицей Якоби_ или _Якобианом_.
Она описывает взаимоотношение скорости обобщенных координат и скорости звена (например, фланца).

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