## Inverse kinematics

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:

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

0 #20 Plomberie Paris 12 2019-01-18 10:12
Votre entreprise de plomberie à paris 12 est libres pour tout
dépannage sur le secteur Paris 12. Nos plombiers se déplacent en urgence pour tous
vos soucis de plomberie. Elle prend également en charge vos travaux d'installations à paris 12.

Contactez https://www.depannage-plombier-amb.fr/depannage-plomberie-paris/

0 #19 CodyBig 2019-01-02 19:43
Hello. I have checked your concordia.sk and i see you've got some duplicate content so probably
it is the reason that you don't rank hi in google. But you can fix this issue fast.

There is a tool that rewrites content like human, just
search in google: miftolo's tools

0 #18 ModestoBig 2018-12-10 19:30
Hello. I have checked your concordia.sk and i see you've got
some duplicate content so probably it is the reason that you don't
rank hi in google. But you can fix this issue fast.

There is a tool that rewrites articles like human, just search in google: miftolo's tools

0 #17 BestJoseph 2018-11-20 14:21
I see you don't monetize your blog, don't waste your traffic, you
can earn extra cash every month. You can use the best adsense alternative for any type of website (they approve all websites),
for more info simply search in gooogle: boorfe's tips monetize your website

0 #16 NellieJuicy 2018-11-20 13:32
Hello. I see that you don't update your site too often. I know that writing content is
boring and time consuming. But did you know that
there is a tool that allows you to create new posts using existing content (from article directories
or other pages from your niche)? And it does it very well.
The new articles are high quality and pass the copyscape test.
Search in google and try: miftolo's tools

0 #15 youngSara 2018-11-16 06:16
I see you don't monetize your page, but you can earn additional bucks
every day. It is very easy even for noobs, if you are interested simply search in gooogle: pandatsor's tools

0 #14 BestKelle 2018-11-13 20:24
I see you don't monetize your site, don't waste your traffic, you can earn additional cash every month.
You can use the best adsense alternative for any type of website (they approve all websites), for more info simply search in gooogle:
boorfe's tips monetize your website

0 #13 oerewllobamzu 2018-11-06 02:48
http://doxycycline-cheapbuy.site/ - doxycycline-cheapbuy.site.ankor onlinebuycytote c.site.ankor

0 #12 BestPhilomena 2018-10-21 09:29
I see you don't monetize your blog, don't waste your traffic, you can earn extra bucks every month.
You can use the best adsense alternative for any type
of website (they approve all websites), for more details simply search in gooogle:
boorfe's tips monetize your website

0 #11 plombiers paris 2018-10-12 17:36
Découvrez une entreprise qualifiée experte dans l'entretien de
chaudière à paris et en Ile-de-France à prix juste chez réparation artisan plombier amb.
Elle propose des solutions irréprochables d'un côté privé et aussi pour l'industrie, n'hésitez
plus d'appeler le 01 40 19 02 29.

#### Add comment

No bad words.

Refresh

Design by i-cons.ch / etosha-namibia.ch