Системы координат и позы

Чтобы говорить о «где находится схват», нужно сначала договориться, относительно чего мы это меряем.

Поза (pose) — совокупность положения (где) и ориентации (как повёрнут) тела относительно выбранной системы координат.

В робототехнике координат много: мировая (привязана к полу/цеху), базовая (к основанию робота), локальные (к каждому звену), система схвата. Любой расчёт начинается с вопроса: «в какой системе мы считаем?».

Положение и ориентация

Положение точки на плоскости — пара $(x, y)$, в пространстве — тройка $(x, y, z)$. Ориентация на плоскости — один угол $\theta$, в пространстве — три (об этом в разделе про повороты). Поза на плоскости — это $(x, y, \theta)$.

Правая тройка осей

Договорённость: оси $x, y, z$ образуют правую тройку — если повернуть $x$ к $y$ по кратчайшему пути, большой палец правой руки указывает вдоль $z$. Положительный поворот — против часовой стрелки, если смотреть с конца оси $z$.

Локальные и мировые координаты

Датчик на схвате видит мир в своей локальной системе. Чтобы понять, где объект относительно стола, координаты надо пересчитать в мировую систему. Этот пересчёт — суть преобразований, которыми мы займёмся в следующем разделе. Пока сделаем простейший случай: схват сместили и повернули, посчитаем его позу.

import math

# база схвата в мире
base_x, base_y, base_theta = 1.0, 0.5, math.radians(30)

# точка в локальной системе схвата (10 см вперёд по локальной оси x)
lx, ly = 0.10, 0.0

# пересчёт в мир: поворот на base_theta + сдвиг
wx = base_x + lx * math.cos(base_theta) - ly * math.sin(base_theta)
wy = base_y + lx * math.sin(base_theta) + ly * math.cos(base_theta)

print("точка в мире: x =", round(wx, 4), "y =", round(wy, 4))

Вывод:

точка в мире: x = 1.0866 y = 0.55

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

Формула пересчёта — это поворот вектора на угол $\theta$ плюс перенос на $(base_x, base_y)$. Именно эту операцию мы в следующем разделе запишем компактно через однородную матрицу $4\times4$ (или $3\times3$ на плоскости), чтобы цепочки преобразований перемножались как матрицы.

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

Считать в градусах там, где функции math.sin/cos ждут радианы. Всегда переводите: math.radians(deg).
Менять порядок «сначала поворот, потом сдвиг» — результат зависит от порядка.
Терять, в какой системе записаны числа: одна и та же точка имеет разные координаты в разных системах.

Итог

Поза = положение + ориентация относительно конкретной системы координат.
В робототехнике сосуществуют мировая, базовая, локальные системы; всегда фиксируйте, в какой считаете.
Оси образуют правую тройку; положительный поворот — против часовой стрелки.
Пересчёт локальных координат в мировые — это поворот плюс перенос.