# Inverse Kinematics

逆運動學（英語：Inverse kinematics）是決定要達成所需要的姿勢所要設置的關節可活動對象的參數的過程。例如，給定一個人體的三維模型，如何設置手腕和手肘的角度以便把手從放鬆位置變成揮手的姿勢？這個問題在機器人學中是很關鍵的，因為操縱機械手臂通過關節角度來控制。逆運動學在遊戲編程和三維建模中也很重要，雖然其重要性因為運動捕獲數據的大型資料庫越來越多的使用而降低了。 ---WiKi

## Problem description

$$E(\theta) = \frac{1}{2}|| f(\theta) - (X_t, Y_t)^T||^2$$

$$find\ \ \theta=argmin\ \frac{1}{2}|| f(\theta) - (X_t, Y_t)^T||^2$$

## Least square method

### 梯度下降


### Ex: 2 Dof 系統

-------------------------------------------------------------------------------------------------

$$ f(x) = ( l_1cos(\theta_1) + l_2cos(\theta_2), l_1sin(\theta_1) + l_2sin(\theta_2) )^T $$
$$E(\theta) = \frac{1}{2}(l_1cos(\theta_1) + l_2cos(\theta_2) - X_t)^2+\frac{1}{2}(l_1sin(\theta_1) + l_2sin(\theta_2) - Y)^2$$

$$ \nabla E(\theta) = J(\theta)^T*f(\theta)$$

$$J(\theta) = \begin{bmatrix} -l_1sin(\theta_1) & -l_2sin(\theta_2)\\
l_1cos(\theta_1) & l_2cos(\theta_2)\\
\end{bmatrix}
$$
$$\theta_k = \theta_{k-1} - \alpha * \nabla E(\theta_{k-1})$$

## Gauss–Newton method

$$ min\ \frac{1}{2}|| f(x) ||^2 <=> min\ E(\Delta x) = \frac{1}{2}|| f(x + \Delta x) ||^2$$

對$f(x + \Delta x)$泰勒展開進行線性化

$$ f(x + \Delta x) = f(x) + J(x) * \Delta x$$
$$ \frac{1}{2}|| f(x + \Delta x) ||^2 \thickapprox \frac{1}{2}|| f(x) + J(x) * \Delta x ||^2$$
$$ \frac{1}{2}|| f(x) + J(x) * \Delta x ||^2 = \frac{1}{2} (f(x)^Tf(x) + 2f(x)^TJ(x)\Delta x + \Delta x^TJ(x)^TJ(x)\Delta x)$$

對二次式求導得到

$$J(x)^TJ(x)\Delta x = -J(x)^T*f(x)$$
$$\Delta x = -(J(x)^TJ(x))^{-1}J(x)^T*f(x)$$

### Ex: 3 Dof 系統

-------------------------------------------------------------------------------------------------

$$ f(x) = ( l_1cos(\theta_1) + l_2cos(\theta_2) + l_3cos(\theta_3) - X_t, l_1sin(\theta_1) + l_2sin(\theta_2) + l_3sin(\theta_3) - Y_t )^T $$

$$J(\theta) = \begin{bmatrix} -l_1sin(\theta_1) & -l_2sin(\theta_2) & -l_3sin(\theta_3)\\
l_1cos(\theta_1) & l_2cos(\theta_2) & l_3cos(\theta_3)\\
\end{bmatrix}
$$
$$\Delta \theta_{k-1} = -(J(\theta_{k-1})^TJ(\theta_{k-1}))^{-1}J(\theta_{k-1})^T*f(\theta_{k-1})$$
$$\theta_k = \theta_{k-1} + \Delta \theta_{k-1}$$

## Pygame simulation

import pygame as pg
import numpy as np


def get_Jacobian(theta, length):
    return np.vstack((-length * np.sin(theta), length * np.cos(theta)))


def get_f(theta, length, target):
    out = np.array([np.sum(length * np.cos(theta)), np.sum(length * np.sin(theta))])
    return out - target


def IK(theta, length, target, iter=5):
    theta = theta
    for _ in range(iter):
        J = get_Jacobian(theta, length)
        f = get_f(theta, length, target)
        delta_theta = - 0.00001*np.linalg.pinv(J.T @ J + 0.01 * np.eye(3)) @ J.T @ f
        theta += delta_theta
    theta_moment = np.zeros_like(theta)
    stepsize = 0.0000001
    beta = 0.9
    beta_2 = beta * beta
    for _ in range(100):
        J = get_Jacobian(theta, length)
        f = get_f(theta, length, target)
        grad = J.T @ f
        s = -stepsize * grad
        delta_theta = (1 + beta) * s + beta_2 * theta_moment
        theta += delta_theta
        theta_moment = beta * theta_moment + s

    return theta


pg.init()

width, height = 640, 480
transform_coordinate = lambda x, y: (x + width / 2, -y + height / 2)
inverse_transform_coordinate = lambda x, y: (x - width / 2, -y + height / 2)
screen = pg.display.set_mode((width, height))
pg.display.set_caption("my game")

bg = pg.Surface(screen.get_size())
bg.convert()

Theta = np.array([np.deg2rad(0), np.deg2rad(90), np.deg2rad(0)])
Length = np.array([100, 100, 100])

clock = pg.time.Clock()
running = True
# line = pg.draw.line(bg, (0, 0, 255), (0, 0), (200, 200))
while running:
    clock.tick(60)
    for event in pg.event.get():
        if event.type == pg.QUIT:
            running = False
    mouses = pg.mouse.get_pos()  # 取得滑鼠坐標
    mouses = inverse_transform_coordinate(mouses[0], mouses[1])
    Theta = IK(Theta, Length, mouses)
    bg.fill((255, 255, 255))
    tmp_x, tmp_y = 0, 0
    for theta, l in zip(Theta, Length):
        pg.draw.line(bg, (0, 0, 255), transform_coordinate(0 + tmp_x, 0 + tmp_y),
                     transform_coordinate(tmp_x + l * np.cos(theta), tmp_y + l * np.sin(theta)), width=5)
        tmp_x += l * np.cos(theta)
        tmp_y += l * np.sin(theta)
    screen.blit(bg, (0, 0))
    pg.display.update()
pg.quit()

### Pygame

pg.init()

width, height = 640, 480
transform_coordinate = lambda x, y: (x + width / 2, -y + height / 2)
inverse_transform_coordinate = lambda x, y: (x - width / 2, -y + height / 2)
screen = pg.display.set_mode((width, height))
pg.display.set_caption("my game")

bg = pg.Surface(screen.get_size())
bg.convert()

Theta = np.array([np.deg2rad(0), np.deg2rad(90), np.deg2rad(0)])
Length = np.array([100, 100, 100])

clock = pg.time.Clock()
running = True

while running:
    clock.tick(60)
    for event in pg.event.get():
        if event.type == pg.QUIT:
            running = False
    mouses = pg.mouse.get_pos()  # 取得滑鼠坐標
    mouses = inverse_transform_coordinate(mouses[0], mouses[1])
    Theta = IK(Theta, Length, mouses)
    bg.fill((255, 255, 255))
    tmp_x, tmp_y = 0, 0
    for theta, l in zip(Theta, Length):
        pg.draw.line(bg, (0, 0, 255), transform_coordinate(0 + tmp_x, 0 + tmp_y),
                     transform_coordinate(tmp_x + l * np.cos(theta), tmp_y + l * np.sin(theta)), width=5)
        tmp_x += l * np.cos(theta)
        tmp_y += l * np.sin(theta)
    screen.blit(bg, (0, 0))
    pg.display.update()
pg.quit()

### Inverse kinematics

# ex: theta = np.array([ pi/4, pi/6, pi/3 ])
# ex: length = np.array([ 1, 2, 2 ])
# ex: target = np.array([ 3.5, -2.7 ])

def get_Jacobian(theta, length):
    return np.vstack((-length * np.sin(theta), length * np.cos(theta)))


def get_f(theta, length, target):
    out = np.array([np.sum(length * np.cos(theta)), np.sum(length * np.sin(theta))])
    return out - target


def IK(theta, length, target, iter=5):
    theta = theta
    # Gaussian Newton method with Regularized Least Squares
    for _ in range(iter):
        J = get_Jacobian(theta, length)
        f = get_f(theta, length, target)
        delta_theta = - 0.00001*np.linalg.pinv(J.T @ J + 0.01 * np.eye(3)) @ J.T @ f
        theta += delta_theta
    # Gradient with Dozat Nesterov Momentum
    theta_moment = np.zeros_like(theta)
    stepsize = 0.0000001
    beta = 0.9
    beta_2 = beta * beta
    for _ in range(100):
        J = get_Jacobian(theta, length)
        f = get_f(theta, length, target)
        grad = J.T @ f
        s = -stepsize * grad
        delta_theta = (1 + beta) * s + beta_2 * theta_moment
        theta += delta_theta
        theta_moment = beta * theta_moment + s

    return theta

### 效果
<iframe width="720" height="480" src="https://www.youtube.com/embed/ylpjxvxbTRc" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>