Fabrik Inverse Kinematics solver

(Taken from the ganja.js coffeeshop)

In the inverse kinematics problem we need to calculate joint angles of a kinematic chain so its base remains fixed and its tip reaches a given target. This is a highly non-linear problem with many solutions. We implement a solver that tries to minimize the differences on all remaining degrees of freedom.

This algorithm readily translates to 3D, is efficient and very well received by artists in a cg animation context.

First we create an algebra. Feel free to try 3D instead of 2D!

from kingdon import Algebra
import numpy as np
from timeit import default_timer
from functools import wraps

def tonp(func):
    @wraps
    def wrapped_func(*args, **kwargs):
        return np.array(func(*args, **kwargs))
    return func

alg = Algebra(2, 0, 1, wrapper=tonp)

We then need to set the number of points in the chain, and we initialize points equally along the chain.

l = 6
d = 3 / l
points = [alg.vector(e0=1, e1=i * d - 1.5, e2=0, e3=0).dual()
               for i in range(l + 1)]

Now we define the actual IK solver. Last point in the chain c is the target. We set the tip as the target, then cycle to the base and back restoring original lengths.

def translator(line, dist):
    """ Translate along the line `line` by a distance `dist`. """
    e, e0 = alg.blades.e, alg.blades.e0
    return 1 - 0.5 * dist * (e0 * line.normalized()*e0.dual())

def inverse_kinematics(c):
    # Run four relaxation steps
    for j in range(4):
        # Set the tip to the target. (this will change the length of the last segment.)
        c[-2] = c[-1]
        # Run backwards to the base and restore the lengths along the chain.
        for i in range(l-2, 0, -1):
            c[i] = translator(c[i] & c[i + 1], d) >> c[i + 1]
        # Loop the other way from base to tip again restoring all lengths.
        for i in range(1, l):
            c[i] = translator(c[i - 1] & c[i], -d) >> c[i - 1]

def graph_func():
    inverse_kinematics(points)
    return [
        0x224488, f"Inverse Kinematics",
        0x008844, *zip(points[1:-1], points[:-2]), # Render line segments [[A,B],[B,C],..]
        0x880088, points[0], "Base",               # Render base
        0x00DD88, *points[1:l],                    # Render joint points. [A,B,C,..]
        0x880088, points[l], "Target",             # Render target in purple
    ]

g = alg.graph(
    graph_func, grid=True, lineWidth=6, labels=True,
)
g