# Проверка усвоения знаний


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


def dh(theta, d, alpha, a):
    return rz(theta) * trs(0, 0, d) * rx(alpha) * trs(a, 0, 0)
```


```python
def joint_spec(number):
    return [
        "вращение вокруг X",
        "вращение вокруг Y",
        "вращение вокруг Z",
        "перемещение вдоль X",
        "перемещение вдоль Y",
        "перемещение вдоль Z"
    ][number]

def bone_spec(number):
    return [
        "смещение вдоль X",
        "смещение вдоль Y",
        "смещение вдоль Z",
    ][number]
```


```python
def get_my_robot(my_id):
    my_id = my_id * 7919
    first_id = my_id
    second_id = my_id // 6
    third_id = second_id // 3
    fourth_id = third_id // 3
    fifth_id = fourth_id // 3
    sixth_id = fifth_id // 3
    
    first = first_id % 3
    second = (first + second_id + 1) % 6
    third = (second + third_id + 1) % 3
    fourth = (third + fourth_id + 1) % 3
    fifth = (fourth + fifth_id + 1) % 3
    sixth = (fifth + sixth_id + 1) % 3
    
    return "Мой робот состоит из следующих элементов:" +\
        "\n\t- " + bone_spec(third) +\
        "\n\t- " + joint_spec(first) +\
        "\n\t- " + bone_spec(fourth) +\
        "\n\t- " + joint_spec(second) +\
        "\n\t- " + bone_spec(fifth) +\
        "\n\t- " + joint_spec(sixth) +\
        "\n\t- " + bone_spec(2)
```

Введите номер вашего варианта:


```python
my_variant = 0
```


```python
print(
    get_my_robot(my_variant)
)
```

## Задание 0

Нарисуйте свой манипулятор в _нулевом_ положении: все обобщенные координаты равны $0$, все смещения совпадают с описанием.

## Задание 1

Для своего робота решите прямую задачу кинематики простым способом:


```python
q1, q2, q3 = symbols("q_1, q_2, q_3")
l1, l2, l3, l4, l5, l6, l7 = symbols("l_1, l_2, l_3, l_4, l_5, l_6, l_7")
```


```python
# FIX NEXT LINE
pos = trs(0, 0, l1) * rx(q1) * trs(l2, 0, 0) * ry(q2) * trs(0, l3, 0) * rz(q3) * trs(0, 0, l4)
```


```python
simplify(
    pos
)
```


```python
simplify(
    pos * vec(0, 0, 0)
)
```

## Задание 2

Воспользуйтесь DH-нотацией чтобы расставить системы координат, решите прямую задачу кинематики.
Помните что угол задается в радианах, например, как:
```
pi / 2
```

В данном примере в DH-нотации сначала идут поворот-смещение вокруг-вдоль $Z$, затем поворот-смещение вокруг-воль $X$.


```python
# FIX NEXT LINE, MAKE SURE IT IS FILLED WITH `dh` ONLY
pos_dh = dh(pi / 2, l1, pi / 2, 0) * dh(q1 + pi / 2, l2, pi / 2, 0) * dh(q2 + pi / 2, l3, pi / 2, 0) * dh(q3, l4, 0, 0)
```


```python
simplify(
    pos_dh
)
```


```python
simplify(
    pos_dh * vec(0, 0, 0)
)
```

7d721b1f515ab596bf6e3ad2bf3ee07345d3b68b
ipynb
Jupyter Notebook
5 - Check.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
5 - Check.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
5 - Check.ipynb
red-hara/jupyter-dh-notation
0ffd305b3e67ce7dd3c20f2d1c719b53251dbf58
MIT
Qwen/Qwen-72B
1. YES
2. YES