<h1>Inverse Kinematics</h1>
<h3>Interactive Demo below</h3>

Based on <a href="https://ipvs.informatik.uni-stuttgart.de/mlr/marc/teaching/index.html">Marc Toussaint's robotics lecture</a>.


```python
%matplotlib inline
import numpy as np
import numpy.linalg
import numpy.random
import pandas as pd
import matplotlib.pyplot as py
import threading
import IPython.display as disp
from ipywidgets import widgets
```

kinematics differ from dynamics in that they control the generalized coordinates $q$ directly, instead of generalized accelerations $\ddot{q}$.

<h2>Kinematic Chain</h2>

A simple model is the kinematic chain (subset of a kinematic tree) where each joint is attached to the previous joint.
In this example I'm using simple rotational joints with two DoFs: the angle and extension $q = (\alpha,\ l)$. This is formulated using affine transformations: <br>
<br>
$\begin{align}
A(\alpha,\ l) &= R(\alpha)\ T(l) \\
\\
R(\alpha) &= \left\lgroup\matrix{
\cos(\alpha) & -\sin(\alpha) & 0 \\
\sin(\alpha) &  \cos(\alpha) & 0 \\
0 & 0 & 1}\right\rgroup \\
\\
T(l) &= \left\lgroup\matrix{
1 & 0 & l \\
0 & 1 & 0 \\
0 & 0 & 1}\right\rgroup
\end{align}$

This could be multiplied out but for readability in the code I'll keep them seperated. <br>
<br>
The chain <b>TODO:</b>


```python
# Simple rotational joint
def linkTransformation(angle, length):
    R = np.matrix([
        [np.cos(angle), -np.sin(angle), 0],
        [np.sin(angle),  np.cos(angle), 0],
        [0,              0,             1],
    ])
    
    T = np.matrix([
        [1, 0, length],
        [0, 1, 0],
        [0, 0, 1],
    ])
    
    return R * T
    
# Returns a set of transformations for a chain of joints
# attached to one another.
def kinematicChain(angles, lenghts):
    assert(len(angles) == len(lenghts))
    A = np.matrix(np.eye(3))
    trafos = np.zeros((len(angles), 3, 3))
    for i in range(0, len(angles)):
        linkA = linkTransformation(angles[i], lenghts[i])
        A = np.dot(A, linkA)
        trafos[i, :, :] = A
    return trafos

def effectorPositions(transformations):
    effPositions = np.zeros((len(transformations) + 1, 2))
    for i in range(0, len(transformations)):
        A = transformations[i]
        effPositions[i + 1, :] =  np.dot(A, [0, 0, 1])[:-1]
    return effPositions
```

<h2>Derivation of a Gauss-Newton IK Solver</h2>
<br>
Linearization of the transformation function (first order Taylor): <br>
$\begin{align}
\phi(q - q_0) &= y_0 + \frac{\partial \phi}{\partial q} \bigg\rvert_{q_0} (q - q_0) + \mathcal{O}((q-q_0)^2) \\
              &\approx y_0 + J\rvert_{q_0} (q - q_0)
\end{align}$
<br>
<br>
Here a Thikonov regularized iterative least squares solver is used. <br>

$\begin{align}
q^* &= \underset{q}{\text{argmin}} \ \lVert \phi(q) - y^* \rVert^2_C + \lVert q_0 - q \rVert^2_W &  \\
&= \underset{q}{\text{argmin}} \ (\phi(q) - y^*)^\top C (\phi(q) - y^*) + (q_0 - q)^\top W (q_0 - q) & \text{(Tikhonov regularization)} \\
&\approx \underset{q}{\text{argmin}} \ (y_0 + J\rvert_{q_0} (q - q_0) - y^*)^\top C (y_0 + J\rvert_{q_0} (q - q_0) - y^*) + (q_0 - q)^\top W (q_0 - q) &
\end{align}$ <br>

$\begin{align}
q^* = \underset{q}{\text{argmin}} \ L(q) &\Rightarrow \frac{\partial \ L(q^*)}{\partial q} = 0 & \text{(gradient vanishes at minimum)}
\end{align}$ <br>

$\begin{align}
\frac{\partial \ L(q)}{\partial q} &= \frac{\partial}{\partial q}
[y_0 + J (q - q_0) - y^*]^\top \ C \ [y_0 + J (q - q_0) - y^*]
+ (q_0 - q)^\top W (q_0 - q) \\
%
&= \frac{\partial}{\partial q}
[J (q - q_0) + (y_0 - y^*)]^\top \ C \ [J (q - q_0) + (y_0 - y^*)]
+ (q_0 - q)^\top W (q_0 - q) \\
%
&= \frac{\partial}{\partial q}
(q - q_0)^\top J^\top \ C \ J (q - q_0)
+ 2 (y_0 - y^*)^\top \ C \ J (q - q_0)
+ (q_0 - q)^\top W (q_0 - q) \\
%
&= 2(q - q_0)^\top J^\top \ C \ J
+  2 (y_0 - y^*)^\top \ C \ J
+  2 (q_0 - q)^\top W = 0
\end{align}$ <br>
<br>

$\begin{align}
2(q - q_0) J^\top \ C \ J  2 (q_0 - q)^\top W
&= -2 (y_0 - y^*)^\top \ C \ J \\
%
(J^\top \ C^\top \ J + W^\top) (q - q_0)
&= J^\top C^\top (y_0 - y^*) \\
%
q &= q_0 + (J^\top \ C^\top \ J + W^\top)^{-1} J^\top C^\top (y_0 - y^*)
\end{align}$ <br>
<br>
But since the whole thing is based on only a linear approximation evaluated at $q_0$ the result is expected to be off the farther away the optimal solution $q^*$ is from $q_0$.
A solution to alleviate this problem is to compute the solution incrementally in small steps and reevaluate the Jacobian at each step. A.k.a the <a href="https://en.wikipedia.org/wiki/Gaussâ€“Newton_algorithm">Gaussâ€“Newton algorithm</a> <br>

<div style="background-color: #F0F0F0">
$q_{t+1} = q_t + \ ({J_t}^\top \ C^\top \ J_t + W^\top)^{-1} {J_t}^\top C^\top \epsilon \ (y_t - y^*)$
</div>
<br>
with some $\epsilon < 1$ and make sure to recompute $J_t$ and $y_t$ at each time step. <br>

<h4> $\rightarrow$ The great thing about this method is that only the link transformations and the Jacobian have to be specified!</h4>
(But even the Jacobian could be computed using symbolic differntiation or approximated using finite differences.)

<h3>A slitghty faster method</h3>

The bottleneck of the above iteration equation is the matrix inversion which is in $\mathcal{O}(n^3)$ whereas direct linear system solvers are in $\mathcal{O}(n^2)$.
Therefore solving the linear system for the increment $\delta q_t = q_{t+1} - q_t$ is faster than actually computing the inverse and can be formulated as follows: <br>
<br>
$\underbrace{({J_t}^\top \ C^\top \ J_t + W^\top)}_{A} \underbrace{(q_{t+1} - q_t)}_{\delta q} = \underbrace{\ {J_t}^\top C^\top \epsilon \ (y_t - y^*)}_{b}$ <br>
$\Rightarrow$ solve $\ A \ \delta q_t = b \ $ then update $\ q_{t + 1} = q_t + \delta q_t \ \ \ \text{(see inverseKinematic() below)}$


```python
# TODO: make explicit that only for angles, not lengths 
def Jacobian(q):
    J = np.zeros((2, len(q['angles'])))
    trafos = kinematicChain(q['angles'], q['lenghts'])
    eff = effectorPositions(trafos)
    for i in range(0, len(q['angles'])):
        d = eff[i + 1] - eff[i]
        J[:, i] = [-d[1], d[0]]
    return J

# Approximate the Jacobian directly from the forward kinematics using central differences
def Jacobian_finiteDifferences(q, h=1.0e-6):
    J = np.zeros((2, len(q['angles'])))
    for d in range(0, len(q['angles'])):
        qAngle = np.copy(q['angles'])
        
        qAngle[d] += h
        trafos = kinematicChain(qAngle, q['lenghts'])
        effLeft = effectorPositions(trafos)[-1]
        
        qAngle[d] -= 2.0 * h
        trafos = kinematicChain(qAngle, q['lenghts'])
        effRight = effectorPositions(trafos)[-1]
        
        J[:, d] = (effLeft - effRight) / (2.0 * h)
    return J
```


```python
def inverseKinematic(q, J, endeffectorGoal, cVal = 50.0, wVal = 1.0, epsilon=0.2, maxSpeed=0.25):
    # TODO: check dimensions
    # TODO: check invertibility
    trafos = kinematicChain(q['angles'], q['lenghts'])
    eff = effectorPositions(trafos)

    diff = (endeffectorGoal - eff[-1])
    dist = np.sqrt(np.dot(diff, diff))
    
    C = cVal * np.eye(2)  
    W = wVal * np.eye(len(q['angles']))
    
    A = np.dot(np.dot(J.T, C.T), J) + W.T
    b = np.dot(J.T, np.dot(C.T, epsilon * (diff / dist) * min(maxSpeed, dist)))
    q['angles'] = q['angles'] + np.linalg.solve(A, b)
    
    return q
```

<h2>The Effects of $W$ and $C$</h2>

The Woodbury identity can be applied to make the effects of $W$ and $C$ more transparent:

$\begin{align}
q &= q_0 + (J^\top \ C^\top \ J + W^\top)^{-1} J^\top C^\top (y_0 - y^*) \\
  &= q_0 + W^{-1} J^\top (C^{-1} + J W^{-1} J^\top)^{-1} (y_0 - y^*)
\end{align}$

Choosing an invertible $C$ ensures the inverse can always be solved even if the target position is not even reachable. <br>
As $C$ approaches $\infty \ I$ and $W$ approaches $I$ the solution becomes the exact (left) pseudo inverse.

<h1> Interactive Demo: Inverse Kinematic </h1>
<i>(Requires Jupyter Notebook)</i>
The arm follows uses inverse kinematics to align the endeffector with the target position.
Use the mouse to specify the target position.
Use the sliders to try out the effect of the Tikhonov regularization parameters $C$ and $W$ as well as the iteration step size $\epsilon$ and see when this method becomes unstable.


```python
import IPython.display as disp
from ipywidgets import widgets
import json

js = '''


<!-- Interaction with the python code -->


<!-- CSS constants for node and links style -->
<style>
node {

}

link {

}
</style>

<!-- svg area and mouse hover event forward declaration -->

<svg id="canvas" onmousemove="svgMouseMove(evt)" width="800" height="600">
<rect id="bg" width="100%" height="100%" fill="#808080"/>
</svg>


'''

def toJson(data):
    return pd.DataFrame(data).to_json(orient='split')

class RobotState():
    def __init__(self, q):
        self.out = widgets.Output()
        self.q = q
        self.goal = [0.5, 0.5]
        self.cVal = 50.0
        self.wVal = 1.0
        self.epsVal = 0.25
    
    def mouse(self, parameters):
        with self.out:
            self.goal = np.asarray([parameters['xRelative'], parameters['yRelative']])
            return '{"x":%f,"y":%f}' % (self.goal [0], self.goal[1])
    
    def update(self, parameters):
        with self.out:
            if not self.goal is None:
                J = Jacobian(self.q)
                # alternatively the jacobian could be approximated
                #J = Jacobian_finiteDifferences(self.q)

                self.q = inverseKinematic(self.q, J, self.goal, self.cVal, self.wVal, self.epsVal)
            return toJson([])
    
    # computes and returns the current effector positions in JSON format
    def requestEffectors(self, parameters):
        with self.out:
            trafos = kinematicChain(self.q['angles'], self.q['lenghts'])
            eff = effectorPositions(trafos)
            return toJson(eff)

linkCount = 7
linkLength = 0.15
robot = RobotState(q={'angles': np.ones(linkCount) / linkCount, 'lenghts': np.ones(linkCount) * linkLength})

def cValSlider(C_Value):
    robot.cVal = C_Value
widgets.interact(cValSlider, C_Value=widgets.FloatSlider(min=0.01, max=100.0, value=robot.cVal))

def wValSlider(W_Value):
    robot.wVal = W_Value
widgets.interact(wValSlider, W_Value=widgets.FloatSlider(min=0.01, max=10.0, value=robot.wVal))

def epsValSlider(epsilon_Value):
    robot.epsVal = epsilon_Value
widgets.interact(epsValSlider, epsilon_Value=widgets.FloatSlider(min=0.01, max=1.0, value=robot.epsVal))

disp.display(robot.out)
disp.display(disp.HTML(js))
# set the initial goal
disp.display(disp.HTML('' % (robot.goal[0], robot.goal[1])))
```


<p>Failed to display Jupyter Widget of type <code>interactive</code>.</p>
<p>
  If you're reading this message in the Jupyter Notebook or JupyterLab Notebook, it may mean
  that the widgets JavaScript is still loading. If this message persists, it
  likely means that the widgets JavaScript library is either not installed or
  not enabled. See the <a href="https://ipywidgets.readthedocs.io/en/stable/user_install.html">Jupyter
  Widgets Documentation</a> for setup instructions.
</p>
<p>
  If you're reading this message in another frontend (for example, a static
  rendering on GitHub or <a href="https://nbviewer.jupyter.org/">NBViewer</a>),
  it may mean that your frontend doesn't currently support widgets.
</p>




<p>Failed to display Jupyter Widget of type <code>interactive</code>.</p>
<p>
  If you're reading this message in the Jupyter Notebook or JupyterLab Notebook, it may mean
  that the widgets JavaScript is still loading. If this message persists, it
  likely means that the widgets JavaScript library is either not installed or
  not enabled. See the <a href="https://ipywidgets.readthedocs.io/en/stable/user_install.html">Jupyter
  Widgets Documentation</a> for setup instructions.
</p>
<p>
  If you're reading this message in another frontend (for example, a static
  rendering on GitHub or <a href="https://nbviewer.jupyter.org/">NBViewer</a>),
  it may mean that your frontend doesn't currently support widgets.
</p>




<p>Failed to display Jupyter Widget of type <code>interactive</code>.</p>
<p>
  If you're reading this message in the Jupyter Notebook or JupyterLab Notebook, it may mean
  that the widgets JavaScript is still loading. If this message persists, it
  likely means that the widgets JavaScript library is either not installed or
  not enabled. See the <a href="https://ipywidgets.readthedocs.io/en/stable/user_install.html">Jupyter
  Widgets Documentation</a> for setup instructions.
</p>
<p>
  If you're reading this message in another frontend (for example, a static
  rendering on GitHub or <a href="https://nbviewer.jupyter.org/">NBViewer</a>),
  it may mean that your frontend doesn't currently support widgets.
</p>




<p>Failed to display Jupyter Widget of type <code>Output</code>.</p>
<p>
  If you're reading this message in the Jupyter Notebook or JupyterLab Notebook, it may mean
  that the widgets JavaScript is still loading. If this message persists, it
  likely means that the widgets JavaScript library is either not installed or
  not enabled. See the <a href="https://ipywidgets.readthedocs.io/en/stable/user_install.html">Jupyter
  Widgets Documentation</a> for setup instructions.
</p>
<p>
  If you're reading this message in another frontend (for example, a static
  rendering on GitHub or <a href="https://nbviewer.jupyter.org/">NBViewer</a>),
  it may mean that your frontend doesn't currently support widgets.
</p>







<!-- Interaction with the python code -->


<!-- CSS constants for node and links style -->
<style>
node {

}

link {

}
</style>

<!-- svg area and mouse hover event forward declaration -->

<svg id="canvas" onmousemove="svgMouseMove(evt)" width="800" height="600">
<rect id="bg" width="100%" height="100%" fill="#808080"/>
</svg>









41fb2aa556051f63c16859e26551d91d8519a2a4
ipynb
Jupyter Notebook
Inverse Kinematics.ipynb
patrickwick/jupyter_playground
e7785e7090c2fbde042f9e6ded994d74ccdcf709
MIT
Inverse Kinematics.ipynb
patrickwick/jupyter_playground
e7785e7090c2fbde042f9e6ded994d74ccdcf709
MIT
Inverse Kinematics.ipynb
patrickwick/jupyter_playground
e7785e7090c2fbde042f9e6ded994d74ccdcf709
MIT
Qwen/Qwen-72B
1. YES
2. YES