除了旋转矩阵,旋转可以用它的角度或单位圆的复数来表示,但实际上是一回事。更重要的是,您需要一个刚体变换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()
变换。
OP正在通过循环坐标下降来解决位置的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