# Using KDL Inverse Kinematics for Producibility Check?

Submitted by benjaminbihler on Tue, 2012-08-14 07:44 |

Hello,

I have been using KDL::ChainIkSolverPos_NR_JL as inverse kinematics solver

for my robot chain. As described here

http://www.orocos.org/forum/orocos/orocos-users/kdl-ik-position-solver

there can be the problem, that an iterative solver does not find a solution

if the starting point is somehow unfriendly. By choosing different starting

points (perhaps randomly) the probability to find a solution can be raised.

Now my question: if I do not know for sure whether the robot can reach a

certain position and if I want to use the inverse kinematics to answer this

question, is KDL a good tool for this at all? How can I distinguish whether

there is no solution for the inverse kinematics problem or whether the

solver just has not found it yet?

Did anyone already use KDL for producibility checks? Which solver did you

use?

Thank you very much,

Benjamin

## Using KDL Inverse Kinematics for Producibility Check?

Just for other users having the same problems as me: I have again made a

mistake. When I create the LMA solver like this

KDL::ChainIkSolverPos_LMA lmaSolver(chain, 1e-5 * 1e3, 500, 1e-15 * 1e3 );

the solver accepts iterates as solution even though they are no real

solutions to the problem (return value >=0, but the result is wrong).

Therefore I have adjusted my kinematic chain and enter all distances as

meter values now. Then I use the default values of the LMA constructor and

the solver can compute the correct solutions.

## Using KDL Inverse Kinematics for Producibility Check?

Hello Benjamin,

On Tue, Aug 14, 2012 at 9:44 AM, Benjamin Bihler

<benjamin [dot] bihler [..] ...> wrote:

> Hello,

>

>

> I have been using KDL::ChainIkSolverPos_NR_JL as inverse kinematics solver

> for my robot chain. As described here

> http://www.orocos.org/forum/orocos/orocos-users/kdl-ik-position-solver

> there can be the problem, that an iterative solver does not find a solution

> if the starting point is somehow unfriendly. By choosing different starting

> points (perhaps randomly) the probability to find a solution can be raised.

>

> Now my question: if I do not know for sure whether the robot can reach a

> certain position and if I want to use the inverse kinematics to answer this

> question, is KDL a good tool for this at all? How can I distinguish whether

> there is no solution for the inverse kinematics problem or whether the

> solver just has not found it yet?

Currently it is not possible to distinguish between these two. Did you

already tried the new Levenberg-Marquardt based ik solver?

> Did anyone already use KDL for producibility checks? Which solver did you

> use?

>

Not that I know of. Typically generic numeric recursive solvers are

really bad at this. Closed-form solutions are in this case a much

better option.

>

> Thank you very much,

> Benjamin

>

Ruben

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

## Using KDL Inverse Kinematics for Producibility Check?

Please keep the list in CC!

On Tue, Aug 14, 2012 at 1:54 PM, Benjamin Bihler

<benjamin [dot] bihler [..] ...> wrote:

> On Tue, Aug 14, 2012 at 11:53 AM, Ruben Smits <ruben [dot] smits [..] ...>

> wrote:

>

> Hi Ruben,

>

>> Currently it is not possible to distinguish between these two. Did you

>> already tried the new Levenberg-Marquardt based ik solver?

>

> yes, I have. I have used the constructor providing the default weight

> matrix. But this solver cannot solve even one of the inverse kinematic

> problems.

Are you sure the destination is reachable. Do not forget that these

are 6D solvers which means that the rotations are also accounted for.

ruben

> I have used it like this:

>

> KDL::ChainIkSolverPos_LMA lmaSolver(chain);

> int ret = lmaSolver.CartToJnt(q_init, F_dest, q);

>

> The return value is always -3 (number of iterations is exceeded). Is it

> wrong to use the default weight matrix?

>

> Thank you and Ciao,

> Benjamin

>

## Using KDL Inverse Kinematics for Producibility Check?

Hi,

the LMA solver works now when I adjust eps and eps_joints as written before.

Still there is a problem when trying to use it for a producibility check:

the LMA solver now always finds a solution, even in the case of positions

the robot cannot reach, since the LMA solver does not care for upper and

lower limits for joint angles like ChainIkSolverPos_NR_JL does.

Are there any assumptions I can make, like "If LMA finds a solution outside

the legal joint angles than there is no solution inside."? Probably not!?

Is there anyone else trying to use Orocos KDL for a producibility check? Or

are there any documents describing how to deal with several solutions of the

inverse kinematic problem and how to decide which is the one that the robot

will choose?

Thanks and Bye,

Benjamin

## Using KDL Inverse Kinematics for Producibility Check?

Hi Ruben,

On Tue, Aug 14, 2012 at 2:00 PM, Ruben Smits <ruben.smits at

intermodalics.eu>

wrote:

> Are you sure the destination is reachable. Do not forget that these

> are 6D solvers which means that the rotations are also accounted for.

The behavior of the LMA solver is strongly depending on the numbers eps and

eps_joints. If I call the constructor like this:

KDL::ChainIkSolverPos_LMA lmaSolver(chain, 1e-5 * 1e3, 500, 1e-15 * 1e3 );

the LMA solver also finds a solution in the millimeter case of the example I

have posted before. I will keep on playing with these numbers...

Bye,

Benjamin

## Using KDL Inverse Kinematics for Producibility Check?

To answer my own question...

We use the LMA solver now for producibility checks. If it finds a solution

inside the joint limits, the point is obviously reachable. And if the found

solution lies outside the joint limits we count it as unreachable. This

seems to be quite realistic. At least some points we have checked with the

robot were really not reachable if the solution found by the LMA solver was

outside the joint limits.

## Using KDL Inverse Kinematics for Producibility Check?

Hi Ruben,

On Tue, Aug 14, 2012 at 2:00 PM, Ruben Smits <ruben [dot] smits [..] ...>

wrote:

> Are you sure the destination is reachable. Do not forget that these

> are 6D solvers which means that the rotations are also accounted for.

I have more hints now. The solvers seem to depend on the order of magnitude

of the numbers. Please check the source code below. If I enter the chain

dimensions in meter, both solvers find solutions, but if I enter the

dimensions in millimeter (i.e. multiply everything by 1000), only the

iterative solver is able to find at least some solution.

Is this a general rule for building up kinematic chains? It does not make a

very good impression, but of course I could divide everything by 1000 as a

quick workaround.

Thank you again for your help! Any answer is greatly appreciated.

Benjamin

Sourcecode:

#include <chain.hpp>

#include <chainfksolver.hpp>

#include <chainfksolverpos_recursive.hpp>

#include <chainiksolverpos_lma.hpp>

#include <chainiksolvervel_pinv.hpp>

#include <chainiksolverpos_nr.hpp>

#include <chainiksolverpos_nr_jl.hpp>

#include <frames_io.hpp>

#include <stdio.h>

#include <iostream>

using namespace KDL;

void solve(KDL::Chain& chain)

{

// Create solver based on kinematic chain

ChainFkSolverPos_recursive fksolver =

ChainFkSolverPos_recursive(chain);

// Create joint array

unsigned int nj = chain.getNrOfJoints();

KDL::JntArray jointpositions = JntArray(nj);

// Assign some values to the joint positions

for (unsigned int i = 0; i < nj; i++)

{

jointpositions(i) = (double) i;

}

// Create the frame that will contain the results

KDL::Frame cartpos;

// Calculate forward position kinematics

bool kinematics_status;

kinematics_status = fksolver.JntToCart(jointpositions, cartpos);

if (kinematics_status >= 0)

{

std::cout << cartpos << std::endl;

printf("%s \n", "Succes, thanks KDL!");

}

else

{

printf("%s \n", "Error: could not calculate forward

kinematics :(");

}

KDL::JntArray q_init = JntArray(nj);

for (unsigned int index = 0; index < nj; ++index)

{

q_init(index) = 0.0;

}

KDL::JntArray q = JntArray(nj);

//Creation of the solvers:

KDL::ChainIkSolverVel_pinv iksolver1v(chain); //Inverse velocity

solver

KDL::ChainIkSolverPos_NR iksolver1(chain, fksolver, iksolver1v);

KDL::ChainIkSolverPos_LMA lmaSolver(chain);

int returnIterativeSolver = iksolver1.CartToJnt(q_init, cartpos, q);

std::cout << "Iterative Solver: " << returnIterativeSolver <<

std::endl;

std::cout << "q:" << std::endl;

for (int index = 0; index < 6; ++index)

{

std::cout << q(index) << std::endl;

}

int returnLmaSolver = lmaSolver.CartToJnt(q_init, cartpos, q);

std::cout << "LMASolver: " << returnLmaSolver << std::endl;

std::cout << "q:" << std::endl;

for (int index = 0; index < 6; ++index)

{

std::cout << q(index) << std::endl;

}

}

int main(int argc, char** argv)

{

{

//Definition of a kinematic chain & add segments to the

chain

KDL::Chain chain;

chain.addSegment(

Segment(Joint(Joint::RotZ, -1.0),

Frame(Vector(0.0, 0.0,

1.020))));

chain.addSegment(

Segment(Joint(Joint::RotX),

Frame(Vector(0.0, 0.0, 0.480))));

chain.addSegment(

Segment(Joint(Joint::RotX, -1.0),

Frame(Vector(0.0, 0.0,

0.645))));

chain.addSegment(Segment(Joint(Joint::RotZ)));

chain.addSegment(

Segment(Joint(Joint::RotX),

Frame(Vector(0.0, 0.0, 0.120))));

chain.addSegment(

Segment(Joint(Joint::RotZ),

KDL::Frame(KDL::Vector(0.010, 0.020, 0.030))));

std::cout << std::endl << "Lengths in m:" << std::endl;

solve(chain);

}

{

//Definition of a kinematic chain & add segments to the

chain

KDL::Chain chain2;

chain2.addSegment(

Segment(Joint(Joint::RotZ, -1.0),

Frame(Vector(0.0, 0.0,

1020.0))));

chain2.addSegment(

Segment(Joint(Joint::RotX),

Frame(Vector(0.0, 0.0, 480.0))));

chain2.addSegment(

Segment(Joint(Joint::RotX, -1.0),

Frame(Vector(0.0, 0.0,

645.0))));

chain2.addSegment(Segment(Joint(Joint::RotZ)));

chain2.addSegment(

Segment(Joint(Joint::RotX),

Frame(Vector(0.0, 0.0, 120.0))));

chain2.addSegment(

Segment(Joint(Joint::RotZ),

KDL::Frame(KDL::Vector(10.0,

20.0, 30.0))));

std::cout << std::endl << "Lengths in mm:" << std::endl;

solve(chain2);

}

}

## Using KDL Inverse Kinematics for Producibility Check?

On Tue, Aug 14, 2012 at 2:00 PM, Ruben Smits <ruben [dot] smits [..] ...>

wrote:

> Are you sure the destination is reachable. Do not forget that these are 6D

solvers which means that the rotations are also accounted for.

Yes, I am. The iterative solver finds solutions. With 6D solver you mean,

that (in KUKA-speak) not only X, Y and Z, but also A, B, C are part of the

solution, right? Then: yes, the solutions found by the iterative solver are

real solutions, but the LMA solver does not find them.

Benjamin