Вместо матрицы вращения, вращение может быть представлено его углом или комплексным числом единичного круга , но на самом деле это то же самое. Что еще более важно, вам нужно представление T из преобразований твердого тела , чтобы вы могли писать такие вещи, как t1 * t2 * t3, для вычисления положения и ориентации третьей ссылки.
Используйте atan2 для вычисления угла между векторами .
Как показывает следующий пример Python, этих двух вещей достаточно для создания небольшого IK-решателя.
from gameobjects.vector2 import Vector2 as V
from matrix33 import Matrix33 as T
from math import sin, cos, atan2, pi
import random
Библиотека gameobjects не имеет 2D-преобразований, поэтому вы должны написать matrix33 самостоятельно. Его интерфейс похож на gameobjects.matrix44.
Определить функцию прямой кинематики для преобразования из одного соединения в другое. Мы предполагаем, что соединение вращается на angle и сопровождается фиксированным преобразованием joint:
def fk_joint(joint, angle): return T.rotation(angle) * joint
Преобразование инструмента: tool == fk(joints, q), где joints - фиксированные преобразования, а q - углы соединения:
def fk(joints, q):
prev = T.identity()
for i, joint in enumerate(joints):
prev = prev * fk_joint(joint, q[i])
return prev
Если основание рычага имеет смещение, замените преобразование T.identity().
ОП решает проблему IK для положения с помощью циклического спуска координат. Идея состоит в том, чтобы переместить инструмент ближе к позиции цели, регулируя одну переменную соединения за раз. Пусть q будет углом сустава, а prev будет трансформацией основания сустава. Сустав должен быть повернут на угол между векторами к инструменту и позициям цели:
def ccd_step(q, prev, tool, goal):
a = tool.get_position() - prev.get_position()
b = goal - prev.get_position()
return q + atan2(b.get_y(), b.get_x()) - atan2(a.get_y(), a.get_x())
Обходите соединения и обновляйте конфигурацию инструмента при каждом изменении значения соединения:
def ccd_sweep(joints, tool, q, goal):
prev = T.identity()
for i, joint in enumerate(joints):
next = prev * fk_joint(joint, q[i])
q[i] = ccd_step(q[i], prev, tool, goal)
prev = prev * fk_joint(joint, q[i])
tool = prev * next.get_inverse() * tool
return prev
Обратите внимание, что fk() и ccd_sweep() одинаковы для 3D; вам просто нужно переписать fk_joint() и ccd_step().
Создайте плечо из n идентичных звеньев и выполните cnt итераций развертки CCD, начиная со случайной конфигурации плеча q:
def ccd_demo(n, cnt):
q = [random.uniform(-pi, pi) for i in range(n)]
joints = [T.translation(0, 1)] * n
tool = fk(joints, q)
goal = V(0.9, 0.75) # Some arbitrary goal.
print "i Error"
for i in range(cnt):
tool = ccd_sweep(joints, tool, q, goal)
error = (tool.get_position() - goal).get_length()
print "%d %e" % (i, error)
Мы можем опробовать решатель и сравнить скорость сходимости для разного количества ссылок:
>>> ccd_demo(3, 7)
i Error
0 1.671521e-03
1 8.849190e-05
2 4.704854e-06
3 2.500868e-07
4 1.329354e-08
5 7.066271e-10
6 3.756145e-11
>>> ccd_demo(20, 7)
i Error
0 1.504538e-01
1 1.189107e-04
2 8.508951e-08
3 6.089372e-11
4 4.485040e-14
5 2.601336e-15
6 2.504777e-15