Прямая кинематика двухзвенника

Самая базовая задача: знаем углы в суставах — находим, где оказался схват.

Прямая кинематика (FK) — вычисление положения и ориентации схвата по заданным суставным координатам $q$.

Для плоского двухзвенника с длинами $L_1, L_2$ и углами $\theta_1, \theta_2$ положение схвата находится сложением проекций звеньев. Важно: угол второго звена отсчитывается относительно первого, поэтому в формуле стоит сумма $\theta_1 + \theta_2$:

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

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

Ориентация схвата на плоскости — это просто сумма углов: $\phi = \theta_1 + \theta_2$.

Считаем

$\theta_1 = 30^\circ$, $\theta_2 = 45^\circ$, $L_1 = 1.0$, $L_2 = 0.7$.

import math

def fk(theta1, theta2, L1=1.0, L2=0.7):
    x = L1 * math.cos(theta1) + L2 * math.cos(theta1 + theta2)
    y = L1 * math.sin(theta1) + L2 * math.sin(theta1 + theta2)
    phi = theta1 + theta2
    return x, y, phi

t1, t2 = math.radians(30), math.radians(45)
x, y, phi = fk(t1, t2)
print("x =", round(x, 4))
print("y =", round(y, 4))
print("ориентация схвата, град =", round(math.degrees(phi), 2))

Вывод:

x = 1.0472
y = 1.1761
ориентация схвата, град = 75.0

Заметьте: те же числа $(1.0472, 1.1761)$ мы получили в уроке про однородные матрицы — это подтверждает, что обе записи (явные формулы и произведение матриц) дают один результат.

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

Каждое звено вносит вектор длины $L_i$ под суммарным углом всех предыдущих суставов. FK — это просто векторная сумма этих звеньев «голова к хвосту». Поэтому метод обобщается на любое число звеньев: добавляйте слагаемые $L_i \cos(\theta_1 + \ldots + \theta_i)$.

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

Брать $\theta_2$ абсолютным, а не относительным — забыть прибавить $\theta_1$ внутри второго косинуса.
Считать в градусах в math.cos без перевода в радианы.
Путать $x$ и $y$: косинус — горизонталь, синус — вертикаль.

Итог

FK переводит углы суставов в положение схвата.
Углы звеньев накапливаются: второе звено идёт под углом $\theta_1 + \theta_2$.
Ориентация схвата на плоскости — сумма суставных углов.
Явные формулы и произведение однородных матриц дают одинаковый ответ.