Трёхзвенный манипулятор

Добавляем третье звено и видим, как формулы FK обобщаются и зачем нужно «лишнее» звено.

Трёхзвенный плоский манипулятор (3R) имеет три DOF: он может привести схват в точку $(x, y)$ и задать его ориентацию $\phi$ — три параметра, три сустава.

Формулы FK просто наращивают слагаемые: каждое звено добавляет вектор под суммой всех предыдущих углов.

$$ x = L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2) + L_3 \cos(\theta_1 + \theta_2 + \theta_3) $$

$$ y = L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2) + L_3 \sin(\theta_1 + \theta_2 + \theta_3) $$

$$ \phi = \theta_1 + \theta_2 + \theta_3 $$

Зачем третье звено

Двухзвенник дотягивается до точки, но ориентацию схвата выбрать не может — она жёстко определена позицией. Третье звено даёт независимое управление углом $\phi$: можно подвести инструмент к точке под нужным наклоном (например, держать паяльник вертикально). Это и есть смысл «полного» плоского робота: 3 DOF на 3 параметра задачи.

Считаем

$\theta = (20^\circ, 30^\circ, 40^\circ)$, $L = (1.0, 0.7, 0.4)$.

import math

def fk3(thetas, lengths):
    x = y = 0.0
    acc = 0.0
    for th, L in zip(thetas, lengths):
        acc += th
        x += L * math.cos(acc)
        y += L * math.sin(acc)
    return x, y, acc

thetas = [math.radians(d) for d in (20, 30, 40)]
lengths = [1.0, 0.7, 0.4]
x, y, phi = fk3(thetas, lengths)
print("x =", round(x, 4))
print("y =", round(y, 4))
print("ориентация, град =", round(math.degrees(phi), 2))

Вывод:

x = 1.3896
y = 1.2783
ориентация, град = 90.0

Как работает под капотом

Цикл повторяет ту же векторную сумму, что и в двухзвеннике, накапливая суммарный угол acc. Этот код работает для любого числа звеньев — добавьте элементы в списки thetas и lengths. Так FK масштабируется от учебного 2R до промышленного 6R: меняется лишь длина списков.

Частые ошибки

Сбрасывать накопленный угол между звеньями — тогда теряется «прокрутка» поворотов.
Думать, что третье звено добавляет дальность: оно в первую очередь добавляет управление ориентацией.
Считать, что 3 DOF позволяют достать любую точку под любым углом — ограничения углов суставов всё равно сужают зону.

Итог

Трёхзвенник имеет 3 DOF: позиция $(x,y)$ плюс ориентация $\phi$.
FK обобщается циклом с накоплением суммарного угла.
Третье звено даёт независимое управление наклоном инструмента.
Тот же алгоритм масштабируется на любое число звеньев.