# Прямая задача кинематики SCARA


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


```python
from kinematics import Vector, Quaternion, Transform
import graphics
```

## Решение прямой задачи кинематики для манипулятора SCARA

Манипулятор кинематической схемы SCARA обладает четыремя степенями подвижности.



Можно выделить высоту колонны, длины первого и второго звеньев.

Обобщенные координаты будут в радианах и метрах:

Обобщенная координата | Обозначение | Размерность
---|---|---
Вращение вокруг колонны | $q_0$ | радиан
Вращение в локте | $q_1$ | радиан
Вращение инструмента | $q_2$ | радиан
Перемещение инструмента | $q_3$ | метр


```python
def scara_chain(q, l, math_source=np):
    base = Transform.identity()
    column = base + Transform(
        Vector(0, 0, l[0]),
        Quaternion.from_angle_axis(q[0], Vector(0, 0, 1), math_source)
    )
    elbow = column + Transform(
        Vector(l[1], 0, 0),
        Quaternion.from_angle_axis(q[1], Vector(0, 0, 1), math_source)
    )
    tool = elbow + Transform(
        Vector(l[2], 0, 0),
        Quaternion.from_angle_axis(q[2], Vector(0, 0, 1), math_source)
    )
    flange = tool + Transform(
        Vector(0, 0, -q[3]),
        Quaternion.identity()
    )
    return [
        base,
        column,
        elbow,
        tool,
        flange
    ]
```

Зададим закон изменения обобщенных координат:


```python
def scara_q(t, total):
    omega = t / total * np.pi * 2
    return [
        np.pi / 4 * np.sin(omega),
        np.pi / 2,
        omega,
        3 + 3 * np.cos(omega)
    ]
```

Укажем длины звеньев:


```python
scara_l = [8, 4, 3]
```


```python
scara_fig = plt.figure()
ax = scara_fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
lines, = ax.plot([], [], [], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
r, g, b = graphics.axis(ax, Transform.identity(), 1)

total = 100

def animate(frame):
    chain = scara_chain(scara_q(frame, total), scara_l)
    (x, y, z) = graphics.chain_to_points(chain)
    lines.set_data_3d(x, y, z)
    global r, g, b
    r.remove(); g.remove(); b.remove()
    r, g, b = graphics.axis(ax, chain[-1], 0.5)

    
animate(0)
fps = 25
scara_ani = animation.FuncAnimation(
    scara_fig,
    animate,
    frames=total,
    interval=1000.0/fps
)
```


```python
HTML(scara_ani.to_jshtml())
```

## Отображение траектории


```python
t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = scara_chain(scara_q(t, t_end), scara_l)
x, y, z = graphics.chain_to_points(chain)

fig = plt.figure()
ax = fig.add_subplot(projection="3d")
ax.set_xlim(-6, 6); ax.set_ylim(-6, 6); ax.set_zlim(0, 12)
ax.plot(x[2], y[2], z[2], color="#c0c0c0")
ax.plot(x[3], y[3], z[3], color="#c0c0c0")
ax.plot(x[4], y[4], z[4], color="#000000")
graphics.axis(ax, Transform.identity(), 2)
fig.show()
```

Определим диапазон изменения $Z$ координаты фланца:


```python
print(
    "Z координата менялась в диапазоне от",
    np.min(z[-1]),
    "до",
    np.max(z[-1])
)
```

## Измерение скорости движения

Численно продиффиринцируем для всех точек траектории:


```python
t_end = 10
step = 0.01
t = np.arange(0, t_end, step)
chain = scara_chain(scara_q(t, t_end), scara_l)
end = chain[-1]
velocity_x = np.diff(end.translation.x) / step
velocity_y = np.diff(end.translation.y) / step
velocity_z = np.diff(end.translation.z) / step
velocity_total = (velocity_x ** 2 + velocity_y ** 2 + velocity_z ** 2) ** 0.5

fig = plt.figure()
ax = fig.add_subplot()

ax.plot(t[:-1], velocity_x, color="#ff0000", label="$V_x$")
ax.plot(t[:-1], velocity_y, color="#00ff00", label="$V_y$")
ax.plot(t[:-1], velocity_z, color="#0000ff", label="$V_z$")
ax.plot(t[:-1], velocity_total, color="#000000", label="$V$")
fig.legend()
fig.show()
```

Оценим максимальную скорость на траектории:


```python
index_max = np.argmax(velocity_total)
print(
    "Максимальная скорость была",
    velocity_total[index_max],
    "в момент",
    index_max * step,
    "сек"
)
```

## Аналитическое решение прямой задачи кинематики


```python
import sympy as sp
```


```python
l_0, l_1, l_2 = sp.symbols("l_0, l_1, l_2")
q_0, q_1, q_2, q_3 = sp.symbols("q_0, q_1, q_2, q_3")
```


```python
flange = scara_chain([q_0, q_1, q_2, q_3], [l_0, l_1, l_2], sp)[-1]
sp.simplify(flange.translation.x)
```


```python
sp.simplify(flange.translation.y)
```


```python
sp.simplify(flange.translation.z)
```

## Самостоятельные задания

Для своего закона изменения обобщенных координат

- Оцените диапазон $X$, $Y$ и $Z$ координат для данной траектории;
- Оцените максимальную скорость по осям;
- Оцените абсолютную максимальную скорость.

849ead03a6ceede7d66a7738e59b1d67060a6372
ipynb
Jupyter Notebook
1.1 - Forward SCARA.ipynb
red-hara/jupyter-kinematics-control
7f55476bef51f915676a1b2f82361fae564f47fa
MIT
2021-03-28T11:36:54.000Z
2022-03-25T04:53:11.000Z
1.1 - Forward SCARA.ipynb
red-hara/jupyter-kinematics-control
7f55476bef51f915676a1b2f82361fae564f47fa
MIT
1.1 - Forward SCARA.ipynb
red-hara/jupyter-kinematics-control
7f55476bef51f915676a1b2f82361fae564f47fa
MIT
2022-03-04T12:19:09.000Z
2022-03-04T12:19:09.000Z
Qwen/Qwen-72B
1. YES
2. YES