Suppose you have an equation (describing for example a robot arm) that gives you a position of the end-effector (for example fingers of a robotic arm) when input like joint angles is provided. This would be **Forward kinematics (FK)**. Opposite problem when position of the end-effector (goal) serves as input and joint angles are on output is called **Inverse kinematics (IK)**.

While FK is relatively easy to compute (you may use coordinate systems described here) the IK is more challenging. There are often several solutions available but some problems do not have any one (for example when the goal is out of the arm's reach).

There are several algorithms to solve the IK problem. The **Jacobian matrix** will be used here.

The Jacobian matrix is the matrix of all first-order partial derivatives of a vector-valued function. Or sometimes referred to as matrix of partial derivatives of entire system.

\begin{aligned} J & = \begin{bmatrix} \frac{\partial f1}{\partial x1} & \frac{\partial f1}{\partial x2} \\ \frac{\partial f2}{\partial x1} & \frac{\partial f2}{\partial x2}\end{bmatrix} \qquad (1) \end{aligned}

Note 1: The Jacobian has also been used in the article "LTI systems 2".

Note 2: Sometimes the Jacobian matrix is called "Functional matrix".

To get some intuition of the Jacobian matrix, view on it as a linear approximation of a (usually non-linear) function $f()$ at a point $\bar{p}$.

In our case,

- the function $f()$ is the function that returns position of the end-effector and takes robot arm join angles (or maybe it takes as input number of ticks in case there are servo motors, or current, or ... it does not matter what).

- the point $\bar{p}$ is an actual position of the end-effector (probably out of the goal, but may also be right at the goal).

- the Jacobian defines how the position of end-effector changes relative to tiny change (limit to zero) of the input (of the system).

It is sometimes possible to compute Jacobian analytically (it is difficult though) but numerical computation servers good as well.

**Some math**

Let's define:

- end-effector position as vector $\bar{e}$

- goal position as vector $\bar{g}$

- joint angles as vector $\bar{\theta}$

Then FK gives us:

\begin{aligned} \bar{e} = f(\bar{\theta})\qquad (2) \end{aligned}

In other words we give joint angles on input and get end-effector position on output.

Let's do it from the opposite site and get joint angles on output and end-effector position on input. In order to do this simply move f() to the other site by inverting it:

\begin{aligned} \bar{\theta} = f^{-1}(\bar{e}) \qquad (3) \end{aligned}

Usually function $f()$ representing position of end-effector is non-linear and can not be inverted (not due to non-linearity). But what can be done is this. We can compute Jacobian matrix of the $f()$ (that is of the system) and afterward we can invert the matrix. This gives us something very close to $ f^{-1}(\bar{e})$.

Jacobian (1) can be also written like this (I leave out 'bars' because we already know $e$ and $\theta$ are vectors):

\begin{aligned} J = \frac{de}{d{\theta}}\qquad (4) \end{aligned}

After small mathematical manipulation we get:

\begin{aligned} de = J*d{\theta}\qquad (5) \end{aligned}

which is related (at least look similar to :)) to the (2).

Since point (5) is all about matrix and vector manipulation we can also move J to the other side by inverting it:

\begin{aligned} d{\theta} = J^{-1}*de\qquad (6) \end{aligned}

and that is actually end of this math road because now we have derivated form of (3).

**Pseudocode**

Below you can see a pseudocode of the IK which basically has 4 repeating steps.

*while (distance(e, g) > threshold){ // e - end-effector position, g - goal position*

*(1) compute the Jacobian matrix $J$*

*(2) compute the inverse of the $J$: $J^{-1}$*

*(3) compute change in joint angles $\theta$: **$\Delta{\theta}$ **= $J^{-1}$ * $\Delta e$ // **$\Delta e$** is **distance between end-effector and the goal position*

*(4) apply = move by a small step *$\alpha\Delta{\theta}$*: $\theta$ = *$\theta$ + $\alpha$ * $\Delta{\theta}$

*}*

**(1) compute the Jacobian matrix J**

As written above, it is sometimes possible to compute Jacobian matrix analytically (it is difficult though) but numerical computation servers good as well. As far as I know numerically can be computed always. Big advantage of numerical way is its robustness. During computation you look at $f()$ as on a black box. For example when you implement $f()$ that returns position of an end-effector (taking joint angles on input) you can use coordinate systems that use matrix manipulation or simply add angles, compute sin/cos of it and multiply it by arm length. Both implementations basically return the same - a vector of end-effector position and that is the reason why one can numerically compute the Jacobian from both of them without any change in code.

There are a lot of resources out there on this topic:

- http://www.maths.lth.se/na/courses/FMN081/FMN081-06/lecture7.pdf

- http://www.iue.tuwien.ac.at/phd/khalil/node14.html

- http://youngmok.com/numerical-jacobian-matrix-matlab/

However the main idea is as follows.

Let's rewrite definition of Jacobian matrix (1) with therms $\theta$ and $e$ - recall they are vectors.

\begin{aligned} J & = \begin{bmatrix} \frac{\partial ex}{\partial \theta 1} & \frac{\partial ex}{\partial \theta 2} \\ \frac{\partial ey}{\partial \theta 1} & \frac{\partial ey}{\partial \theta 2} \end{bmatrix} \qquad (7) \end{aligned}

Use $f(\theta)$ and compute $e$,

Now add small $\Delta \theta$ to every $\theta i$ and compute $\hat{e}i$ (here subscript 'i' means separately = if I had arm with 2 joints I would use as input $(\theta1+\Delta\theta,\theta2)$ and afterwards $(\theta1,\theta2+\Delta\theta)$).

We get:

\begin{aligned} \Delta e = \hat{e} - e \qquad (8) \end{aligned}

which basically tells us how the end-effector moves when an joint angle slightly changes.

Note: I left out subscripts 'i' here because we need to move in our thinking from single elements to vectors and matrices.

Since this approximation is valid:

\begin{aligned} \frac{\partial e}{\partial \theta} \approx \frac{\Delta e}{\Delta\theta} \qquad (9) \end{aligned}

we can now fill in the Jacobian matrix by numerical way.

My Java code of numerical computation of Jacobian matrix:

Matrix computeJacobian(Matrix mTheta){ Matrix mTheta_; Matrix mOut1 = computeArmPosition(mTheta); Matrix mOut2 = mOut1.copy(); Matrix mJ = new Matrix(2, 3); float fValue = 0.0; float d = 0.01; for (int i = 0; i < 2; i++){ for (int j = 0; j < 3; j++){ mTheta_ = mTheta.copy(); //create copy of mTheta mTheta_.set(0, j, mTheta_.get(0, j) + d); mOut2 = computeArmPosition( mTheta_ ); fValue = ( (float)mOut2.get(i, 0) - (float)mOut1.get(i, 0) )/ d; mJ.set(i, j, fValue); } } return mJ; }

**(2) compute the inverse of the J: $J^{-1}$**

How to inverse Jacobian?

1. Use pseudoinverse $J^+$. It is nearly impossible to inverse matrix in a standard way:

- often not a square matrix,

- changes over time hence its properties change as well.

\begin{aligned} J^+ = (J^T J)^{-1}J^T \qquad (10) \end{aligned}

2. Simply transpose the Jacobian matrix.

**(3) compute change in joints $\Delta{\theta}$**

In my java code I have:

//Compute delta(theta) mDeltaTheta = mJacobian.times(mDeltaE);

where variable mJacobian contains inverted (pseudoinverted or transposed) Jacobian matrix.

Look at the $\Delta{\theta}$ as on a direction to the goal given by a map (inverted Jacobian) and your distance to the goal (error).

**(4) apply = move by a small step **$\alpha\Delta{\theta}$

To get to the goal the algorithm must run in a loop and repeat steps of computing a Jacobian and then taking a small step towards the goal until the end-effector is not close enough.

In my java code I have:

//Add delta(theta) to theta mDeltaTheta = mDeltaTheta.times(fAlpha); mTheta.plusEquals(mDeltaTheta.transpose());

Note: bigger value of Alpha means faster convergence = the arm reaches the goal faster, but also may cause oscillations (arm jitters).

**Sample applet - IK with ****pseudoinverse **** of Jacobian**

- place the goal (circle) by doubleclick

**Sample applet - IK with ****transpose ****of Jacobian**

- place the goal (circle) by doubleclick

**Notes from developing the applets above**

- since applets differ in the way the Jacobian is being inversed (transpose vs. pseudoinverse) both applets have to use different value of alpha.

- smaller number of alpha means slower convergence (it takes longer to reach the goal)

- in applet 1 where pseudoinverse is in use I mentioned that goal out of the arm's reach causes that the arm jitters.

**Reference**

http://www.cs.cmu.edu/~./15464-s13/lectures/lecture6/IK.pdf

http://mathworld.wolfram.com/Jacobian.html

## Comments

What could you suggest about your publish that you simply

made some days ago? Anny positive?

My webpage Making You Smile: http://www.makingusmile.net,http://www.makingusmile.net/the-ultimate-list-of-career-limiting-moves/

RSS feed for comments to this post