• Increase font size
  • Default font size
  • Decrease font size

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:

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  

 
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
Quote
 
 
0 #13 oerewllobamzu 2018-11-06 02:48
http://doxycycline-cheapbuy.site/ - doxycycline-cheapbuy.site.ankor onlinebuycytote c.site.ankor
Quote
 
 
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
Quote
 
 
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.
Quote
 
 
0 #10 Synonymizer en ligne 2018-09-22 08:50
Découvrez le générateur d'articles automatique en français.
ce programme en mesure de faire de la réécriture de texte vous fournit des articles de blog,
mais aussi des textes pour publier des avis ou faire de
la conception de site. Les outils de recherches aprécierons que vous
leur donnez du texte inégalable sans duplicate content alors n'attendez plus à
obtenir du texte au moyen d'un programme pensé afin de faire
accélerer les choses aux créateurs. Rendez vous sur http://www.content-spinning.fr
Quote
 
 
0 #9 XAlphonso 2018-08-31 19:45
Hi admin. It was hard to find this site in google.
It's not even in top10. You should focus on hq backlinks from high authority websites
in your niche. I know of a very effective free method to get
high quality links and instant traffic. The best thing about this
method is that you start getting clicks right away.
For more info search in google for: masitsu's tricks
Quote
 
 
0 #8 Plomberie Paris 2018-08-01 00:26
Comment procéder dans la pose, restauration et réparation de vos équipements ?
L'entreprise réparation plombier propose des pros confirmés pour traiter vos pannes dans toutes les situations urgentes dans paris et Ile-de-France
au 01 40 19 02 29.
Quote
 
 
0 #7 Plombier à Paris 2018-07-14 08:52
Sélectionnez un pro pour s'occuper de vos pannes
et toute réparation de fuite d'eau chez réparation dépanneur plombier amb.
Trouvez des pros près de votre résidence à paris et ses environs qui proposent des services de qualité au
meilleur prix.
Quote
 
 
0 #6 Agence Black Hat SEO 2018-07-09 19:01
Sympa ton site, je t'invite à visiter le mien www.black-hat-seo.org
Quote
 
 
0 #5 GenevaSmall 2018-02-21 12:13
I have checked your blog and i've found some duplicate content, that's why you don't rank high in google's search results,
but there is a tool that can help you to create 100% unique
articles, search for: Boorfe's tips unlimited content
Quote
 

Add comment

No bad words.


Security code
Refresh


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