2D Inverse Kinematics Implementation

Question

I am trying to implement Inverse Kinematics on a 2D arm(made up of three sticks with joints). I am able to rotate the lowest arm to the desired position. Now, I have some questi

Answer 1

1. Instead of a rotation matrix, the rotation can be represented by its angle or by a complex number of the unit circle, but it's the same thing really. More importantly, you need a representation T of rigid body transformations, so that you can write stuff like t1 * t2 * t3 to compute the position and orientation of the third link.

2. Use atan2 to compute the angle between the vectors.

As the following Python example shows, those two things are enough to build a small IK solver.

from gameobjects.vector2 import Vector2 as V
from matrix33 import Matrix33 as T
from math import sin, cos, atan2, pi
import random

The gameobjects library does not have 2D transformations, so you have to write matrix33 yourself. Its interface is just like gameobjects.matrix44.

Define the forward kinematics function for the transformation from one joint to the next. We assume the joint rotates by angle and is followed by a fixed transformation joint:

def fk_joint(joint, angle): return T.rotation(angle) * joint

The transformation of the tool is tool == fk(joints, q) where joints are the fixed transformations and q are the joint angles:

def fk(joints, q):
    prev = T.identity()
    for i, joint in enumerate(joints):
        prev = prev * fk_joint(joint, q[i])
    return prev

If the base of the arm has an offset, replace the T.identity() transformation.

The OP is solving the IK problem for position by cyclic coordinate descent. The idea is to move the tool closer to the goal position by adjusting one joint variable at a time. Let q be the angle of a joint and prev be the transformation of the base of the joint. The joint should be rotated by the angle between the vectors to the tool and goal positions:

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 the joints and update the tool configuration for every change of a joint value:

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

Note that fk() and ccd_sweep() are the same for 3D; you just have to rewrite fk_joint() and ccd_step().

Construct an arm with n identical links and run cnt iterations of the CCD sweep, starting from a random arm configuration 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)

We can try out the solver and compare the rate of convergence for different numbers of links:

>>> 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