En lugar de una matriz de rotación, la rotación puede ser representado por su ángulo o por un complex number of the unit circle, pero es la misma cosa realmente. Más importante aún, necesita una representación T de rigid body transformations, de modo que pueda escribir cosas como t1 * t2 * t3 para calcular la posición y la orientación del tercer enlace.
Usa atan2 para calcular el angle between the vectors.
Como muestra el siguiente ejemplo de Python, esas dos cosas son suficientes para construir un pequeño solucionador de IK.
from gameobjects.vector2 import Vector2 as V
from matrix33 import Matrix33 as T
from math import sin, cos, atan2, pi
import random
La biblioteca gameobjects no tiene transformaciones 2D, por lo que tienen que escribir matrix33 mismo. Su interfaz es como gameobjects.matrix44.
Define la función cinemática hacia adelante para la transformación de una unión a la siguiente. Suponemos que gira el conjuntas por angle y es seguido por una transformación fijo joint:
def fk_joint(joint, angle): return T.rotation(angle) * joint
La transformación de la herramienta es tool == fk(joints, q) donde joints son las transformaciones fijos y q son los ángulos de las articulaciones:
def fk(joints, q):
prev = T.identity()
for i, joint in enumerate(joints):
prev = prev * fk_joint(joint, q[i])
return prev
Si la base del brazo tiene un desplazamiento, reemplace la transformación T.identity().
El OP está resolviendo el problema de IK para la posición mediante descenso de coordenadas cíclicas. La idea es acercar la herramienta a la posición objetivo ajustando una variable conjunta a la vez. Deje que q sea el ángulo de una junta y prev sea la transformación de la base de la junta.La unión debe ser girado por el ángulo entre los vectores a las posiciones de la herramienta y el objetivo:
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())
Traverse las articulaciones y actualizar la configuración de la herramienta para cada cambio de un valor conjunto:
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
Nota que fk() y ccd_sweep() son lo mismo para 3D; solo tiene que volver a escribir fk_joint() y ccd_step().
Construir un brazo con n enlaces idénticos y ejecutar cnt iteraciones del barrido CCD, a partir de una configuración de brazo al azar 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)
Podemos probar el solucionador y comparar la tasa de convergencia para diferentes números de enlaces:
>>> 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