# Пример с $\Delta$-кинематикой

## Прямая задача


```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]
    ])
```

Дельта-принтер:

[источник изображения](https://www.3dprintersonlinestore.com/delta-3d-printer-kit)

Возьмем $0$ в центре, $X$ направлено вправо, $Y$ - вглубь, $Z$ - вверх.

Для описания положения "ползунов" будем отчситывать их высоту относительно основания. Первый ползун - в глубине, второй - слева, третий - справа.


```python
q1, q2, q3 = symbols("q_1, q_2, q_3")
```

Обратим внимание, что плоский перенос тяг вдоль оси, проходящей через центр и ползун не будет влиять на решение кинематики.
За счет этого мысленно сместим тяги в центр подвижной платформы, и определим проекцию расстояния от крепления тяги до центра на горизонтальную плоскость.
Это расстояние, например, в прошивке Marlin называется `delta radius` и обозначается $R$:


```python
delta_radius = symbols("R")
```

Длину тяг (`diagonal rod`) обозначим как $L$:


```python
diagonal_rod = symbols("L")
```

Тогда, положение ползунов будет следующим:


```python
slider1 = vec(0, delta_radius, q1)
slider2 = rz(2*pi/3) * vec(0, delta_radius, q2)
slider3 = rz(4*pi/3) * vec(0, delta_radius, q3)
```

Положение центра подвижной платформы зададим как вектор-столбец:
$$
p = 
\begin{bmatrix}
    x \\
    y \\
    z \\
    1
\end{bmatrix}
$$


```python
x, y, z = symbols("x, y, z")
p = vec(x, y, z)
```

Тогда, "очевидно" что расстояние от центра платформы до каждого ползуна должно равняться длине тяги, а квадрат расстояния - квадрату длины тяги:


```python
def distance_sq(a, b):
    return (a[0] - b[0]) ** 2 +\
        (a[1] - b[1]) ** 2 +\
        (a[2] - b[2]) ** 2
```


```python
condition1 = Eq(distance_sq(p, slider1), diagonal_rod**2)
condition2 = Eq(distance_sq(p, slider2), diagonal_rod**2)
condition3 = Eq(distance_sq(p, slider3), diagonal_rod**2)
```


```python
import time
start = time.time()
fk_solution = solve(
    [
        condition1,
        condition2,
        condition3
    ],
    [
        x,
        y,
        z
    ],
)
end = time.time()
print("Прошло секунд:", end - start)
```

Должно быть два решения:


```python
len(fk_solution)
```

Рассмотрим первое:


```python
fk_solution[0][0]
```


```python
fk_solution[0][1]
```


```python
fk_solution[0][2]
```

Теперь нарисуем какой-нибудь принтер и посмотрим на его движение:


```python
from matplotlib import pyplot as plt
from matplotlib import animation
import numpy as np
from IPython.display import HTML
%matplotlib notebook
```


```python
def fk(r, l, pos1, pos2, pos3):
    x = fk_solution[0][0].evalf(subs={
        delta_radius: r,
        diagonal_rod: l,
        q1: pos1,
        q2: pos2,
        q3: pos3
    })
    y = fk_solution[0][1].evalf(subs={
        delta_radius: r,
        diagonal_rod: l,
        q1: pos1,
        q2: pos2,
        q3: pos3
    })
    z = fk_solution[0][2].evalf(subs={
        delta_radius: r,
        diagonal_rod: l,
        q1: pos1,
        q2: pos2,
        q3: pos3
    })
    return (x, y, z)
```


```python
def generalized(t, total):
    omega = t / total * 2 * np.pi
    q1 = 350 + 25 * np.sin(omega)
    q2 = 350 + 25 * np.sin(omega + 2/3*np.pi)
    q3 = 350 + 25 * np.sin(omega + 4/3*np.pi)
    return (q1, q2, q3)
```


```python
delta_radius_ex = 124
diagonal_rod_ex = 337

fig = plt.figure()
ax = fig.add_subplot(projection="3d")

ax.set_xlim([-delta_radius_ex, delta_radius_ex])
ax.set_ylim([-delta_radius_ex, delta_radius_ex])
ax.set_zlim([0, 2 * delta_radius_ex])
ax.plot([0]*2, [delta_radius_ex] * 2, [0, 400], color="#000000")
ax.plot([-np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color="#000000")
ax.plot([np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color="#000000")
rod1, = ax.plot([], [], [], color="#ff0000")
rod2, = ax.plot([], [], [], color="#00ff00")
rod3, = ax.plot([], [], [], color="#0000ff")

total = 36

def animate(frame):
    q1, q2, q3 = generalized(frame, total)
    x, y, z = fk(delta_radius_ex, diagonal_rod_ex, q1, q2, q3)
    rod1.set_data_3d([0, x], [delta_radius_ex, y], [q1, z])
    rod2.set_data_3d([-np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q2, z])
    rod3.set_data_3d([np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q3, z])

ani = animation.FuncAnimation(
    fig,
    animate,
    frames=total,
    interval=1000.0/25
)
```

## Обратная задача


```python
start = time.time()
ik_solution = solve(
    [
        condition1,
        condition2,
        condition3
    ],
    [
        q1,
        q2,
        q3
    ],
)
end = time.time()
print("Прошло секунд:", end - start)
```

Решений должно быть $2^3$:


```python
len(ik_solution)
```

Рассмотрим восьмое:


```python
ik_solution[7][0]
```


```python
ik_solution[7][1]
```


```python
ik_solution[7][2]
```

Помоделируем наше решение:


```python
def ik(r, l, pos1, pos2, pos3):
    q1 = ik_solution[7][0].evalf(subs={
        delta_radius: r,
        diagonal_rod: l,
        x: pos1,
        y: pos2,
        z: pos3
    })
    q2 = ik_solution[7][1].evalf(subs={
        delta_radius: r,
        diagonal_rod: l,
        x: pos1,
        y: pos2,
        z: pos3
    })
    q3 = ik_solution[7][2].evalf(subs={
        delta_radius: r,
        diagonal_rod: l,
        x: pos1,
        y: pos2,
        z: pos3
    })
    return (q1, q2, q3)
```


```python
def position(t, total):
    omega = t / total
    r = delta_radius_ex / 2
    h = 50
    if omega < 0.25:
        tau = omega/0.25
        return (
            -r + r*tau*2,
            -r,
            h
        )
    elif omega < 0.5:
        tau = (omega - 0.25)/0.25
        return (
            r,
            -r + r*tau*2,
            h
        )
    elif omega < 0.75:
        tau = (omega - 0.5)/0.25
        return (
            r - r*tau*2,
            r,
            h
        )
    else:
        tau = (omega - 0.75)/0.25
        return (
            -r,
            r - r*tau*2,
            h
        )
```


```python
fig = plt.figure()
bx = fig.add_subplot(projection="3d")

bx.set_xlim([-delta_radius_ex, delta_radius_ex])
bx.set_ylim([-delta_radius_ex, delta_radius_ex])
bx.set_zlim([0, 2 * delta_radius_ex])
bx.plot([0]*2, [delta_radius_ex] * 2, [0, 400], color="#000000")
bx.plot([-np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color="#000000")
bx.plot([np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color="#000000")
rod1, = bx.plot([], [], [], color="#ff0000")
rod2, = bx.plot([], [], [], color="#00ff00")
rod3, = bx.plot([], [], [], color="#0000ff")

total = 100

def animate2(frame):
    x, y, z = position(frame, total)
    q1, q2, q3 = ik(delta_radius_ex, diagonal_rod_ex, x, y, z)
    rod1.set_data_3d([0, x], [delta_radius_ex, y], [q1, z])
    rod2.set_data_3d([-np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q2, z])
    rod3.set_data_3d([np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q3, z])

animate2(0)
ani2 = animation.FuncAnimation(
    fig,
    animate2,
    frames=total,
    interval=1000.0/25
)
```

f8f0c1903f40a54855fa23749ceaebe8883bee66
ipynb
Jupyter Notebook
6 - Delta Example.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
6 - Delta Example.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
6 - Delta Example.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
Qwen/Qwen-72B
1. YES
2. YES