ik

ik.py

import numpy as np


# forward kinematics
def forward(theta, l):
    ex = np.dot(l, np.cos(theta))
    ey = np.dot(l, np.sin(theta))
    return np.array([ex, ey])


def approx(f, x, l, pidx, eps=1e-5):
    a = np.zeros(len(x))
    a[pidx] = 1.0
    xi = eps * a

    return (f(x+xi, l) - f(x-xi, l)) / (2*eps)


def compute_J(theta, l):
    dex = approx(forward, theta, l, 0)
    dey = approx(forward, theta, l, 1)
    dez = approx(forward, theta, l, 2)
    return np.stack([dex, dey, dez])


def solver_pseudoinverse(J, e):
    Jinv = np.linalg.pinv(J)
    return np.dot(Jinv.T, e)


def solver_dampened(j, e, dampen=1.0):
    j_t = np.transpose(j)
    x = np.dot(j, j_t) + np.dot(dampen ** 2, np.eye(3))
    y = np.linalg.inv(x)
    z = np.dot(j_t, y)
    return np.dot(z.T, e)


target = np.array([1, 1])
epsilon = 0.1

# joint angles
theta = np.array([0.0, 0, 0.0])

# segment lengths
lengths = np.array([3.0, 2.0, 1.0])

err = np.Inf

MAX_ITERATIONS = 1000
iterations = 0

while err > 0.001 and iterations < MAX_ITERATIONS:
    e = forward(theta, lengths)
    J = compute_J(theta, lengths)

    dv = target - e
    err = np.linalg.norm(dv)
    dv = epsilon * dv / err

    #d_theta = solver_pseudoinverse(J, dv)
    d_theta = solver_dampened(J, dv)

    theta += d_theta

    iterations += 1


print(f"Solution discovered after {iterations} iterations")
print(f"Total error is: \t{err} ")
print(f"Joint angles: \t{np.rad2deg(theta)}")
print(f"Effector position: \t{e}")

segments = np.zeros((4, 2))
current = [0, 0]

for i in range(len(theta)):

    nx = current[0] + lengths[i] * np.cos(theta[i])
    ny = current[1] + lengths[i] * np.sin(theta[i])

    segments[i+1][0] = nx
    segments[i+1][1] = ny

    current[0] = nx
    current[1] = ny

print(f"Segment positions: {segments}")