# KDL IK Position Solver

Submitted by AndreaLuzzana on Mon, 2012-05-07 15:00 |

Hi all,

i am developing a kinematic component that, once loaded an urdf

description of a kinematic chain, let the user to obtain direct and

inverse position and velocity kinematics.

I observed that the NR solver for the inverse position kinematics is

strongly affected by the initial guess of joint coordinates passed as

first parameter to the JntToCart method. In some cases the algorithm

finds very incorrect solutions, in other cases the solution are fine. It

seems to be independent from the imposed maximum number of iterations,

the desired precision or the desired cartesian position, as i can

observe it depends only on the guess joint coordinates passed.

I notice this behavior testing the component with a 6-DOF custom-made

dynamixel arm.

Have someone of you observed this behavior? How can i obtain an example

of a robust working solution?

Thank you in advance.

Andrea Luzzana

## KDL IK Position Solver

On Mon, 7 May 2012, Andrea Luzzana wrote:

> Hi all,

> i am developing a kinematic component that, once loaded an urdf

> description of a kinematic chain, let the user to obtain direct and

> inverse position and velocity kinematics.

> I observed that the NR solver for the inverse position kinematics is

> strongly affected by the initial guess of joint coordinates passed as

> first parameter to the JntToCart method.

Indeed, which is quite normal for an iterative solver.

> In some cases the algorithm

> finds very incorrect solutions, in other cases the solution are fine. It

> seems to be independent from the imposed maximum number of iterations,

> the desired precision or the desired cartesian position, as i can

> observe it depends only on the guess joint coordinates passed.

The initial value of the iteration is indeed the most important factor. The

other factors you mention have also an influence, but of secondary

importance.

> I notice this behavior testing the component with a 6-DOF custom-made

> dynamixel arm.

>

> Have someone of you observed this behavior? How can i obtain an example

> of a robust working solution?

Define "robust". I fear you are looking for a white elephant... :-)

_If_ your 6DOF arm has one of the traditional "industrial" designs, there

exist closed-form solutions; but they typically have (at least) 8 different

solutions for the inverse position kinematics. These solvers are also in

KDL: ZXXZXZ, etc.

> Thank you in advance.

>

> Andrea Luzzana

Herman

## KDL IK Position Solver

Il 07/05/2012 20:25, Herman Bruyninckx ha scritto:

> On Mon, 7 May 2012, Andrea Luzzana wrote:

>

>> Hi all,

>> i am developing a kinematic component that, once loaded an urdf

>> description of a kinematic chain, let the user to obtain direct and

>> inverse position and velocity kinematics.

>> I observed that the NR solver for the inverse position kinematics is

>> strongly affected by the initial guess of joint coordinates passed as

>> first parameter to the JntToCart method.

>

> Indeed, which is quite normal for an iterative solver.

>

>> In some cases the algorithm

>> finds very incorrect solutions, in other cases the solution are fine. It

>> seems to be independent from the imposed maximum number of iterations,

>> the desired precision or the desired cartesian position, as i can

>> observe it depends only on the guess joint coordinates passed.

>

> The initial value of the iteration is indeed the most important

> factor. The

> other factors you mention have also an influence, but of secondary

> importance.

>

>> I notice this behavior testing the component with a 6-DOF custom-made

>> dynamixel arm.

>>

>> Have someone of you observed this behavior? How can i obtain an example

>> of a robust working solution?

>

> Define "robust". I fear you are looking for a white elephant... :-)

Well... let say "not so dependent from the initial condition". I know

that "robust" and "iterative" are not friends...

>

> _If_ your 6DOF arm has one of the traditional "industrial" designs, there

> exist closed-form solutions; but they typically have (at least) 8

> different

> solutions for the inverse position kinematics. These solvers are also in

> KDL: ZXXZXZ, etc.

>

The 6-DOF robot is just an example in order to test the component with

some physical robot. In particular, the IK of that robot (3-2-1

configuration) can be, of course, resolved in a closed form but i'm not

looking for a solution for that particular structure. In any case i'll

try other solvers from KDL.

I was thinking about a workaround:

I could compute the inverse kinematics obtaining a set of joint

positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

apply the direct kinematics obtaining a cartesian position. If this

solution is "close enough" to the original cartesian position i've found

the solution, if not i can use the already obtained joints positions as

initial condition for the next iteration and so on.

Ignoring the performance and time issues, do you think it will converge?

>> Thank you in advance.

>>

>> Andrea Luzzana

>

> Herman

## KDL IK Position Solver

>

> I was thinking about a workaround:

> I could compute the inverse kinematics obtaining a set of joint

> positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

> apply the direct kinematics obtaining a cartesian position. If this

> solution is "close enough" to the original cartesian position i've found

> the solution, if not i can use the already obtained joints positions as

> initial condition for the next iteration and so on.

> Ignoring the performance and time issues, do you think it will converge?

When the ik solver does not converge to the correct solution, even when you

bump up the number of iterations, then you are probably stuck in a local

minimum. So when you use the joint angles from this local minimum for the

next call to the ik solver, you will remain stuck in this local minimum.

This approach is not any different from increasing the number of

iterations. If you don't care about performance, you could randomly choose

initial conditions, until the ik solver converges to the correct solution.

Wim

## KDL IK Position Solver

On Tue, 8 May 2012, Wim Meeussen wrote:

>>

>> I was thinking about a workaround:

>> I could compute the inverse kinematics obtaining a set of joint

>> positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

>> apply the direct kinematics obtaining a cartesian position. If this

>> solution is "close enough" to the original cartesian position i've found

>> the solution, if not i can use the already obtained joints positions as

>> initial condition for the next iteration and so on.

>> Ignoring the performance and time issues, do you think it will converge?

>

>

> When the ik solver does not converge to the correct solution, even when you

> bump up the number of iterations, then you are probably stuck in a local

> minimum. So when you use the joint angles from this local minimum for the

> next call to the ik solver, you will remain stuck in this local minimum.

> This approach is not any different from increasing the number of

> iterations. If you don't care about performance, you could randomly choose

> initial conditions, until the ik solver converges to the correct solution.

And this is the approach taken in the "RRT planners" for IK, as OpenRave

provides them. A clever combination of both (planners and velocity-based

inverse position kinematics, cf other posts in this thread) would make a

lot of sense :-)

> Wim

Herman

## KDL IK Position Solver

On Tue, 8 May 2012, Andrea Luzzana wrote:

> Il 07/05/2012 20:25, Herman Bruyninckx ha scritto:

>> On Mon, 7 May 2012, Andrea Luzzana wrote:

>>

>>> Hi all,

>>> i am developing a kinematic component that, once loaded an urdf

>>> description of a kinematic chain, let the user to obtain direct and

>>> inverse position and velocity kinematics.

>>> I observed that the NR solver for the inverse position kinematics is

>>> strongly affected by the initial guess of joint coordinates passed as

>>> first parameter to the JntToCart method.

>>

>> Indeed, which is quite normal for an iterative solver.

>>

>>> In some cases the algorithm

>>> finds very incorrect solutions, in other cases the solution are fine. It

>>> seems to be independent from the imposed maximum number of iterations,

>>> the desired precision or the desired cartesian position, as i can

>>> observe it depends only on the guess joint coordinates passed.

>>

>> The initial value of the iteration is indeed the most important

>> factor. The

>> other factors you mention have also an influence, but of secondary

>> importance.

>>

>>> I notice this behavior testing the component with a 6-DOF custom-made

>>> dynamixel arm.

>>>

>>> Have someone of you observed this behavior? How can i obtain an example

>>> of a robust working solution?

>>

>> Define "robust". I fear you are looking for a white elephant... :-)

> Well... let say "not so dependent from the initial condition". I know

> that "robust" and "iterative" are not friends...

>>

>> _If_ your 6DOF arm has one of the traditional "industrial" designs, there

>> exist closed-form solutions; but they typically have (at least) 8

>> different

>> solutions for the inverse position kinematics. These solvers are also in

>> KDL: ZXXZXZ, etc.

>>

> The 6-DOF robot is just an example in order to test the component with

> some physical robot. In particular, the IK of that robot (3-2-1

> configuration) can be, of course, resolved in a closed form but i'm not

> looking for a solution for that particular structure. In any case i'll

> try other solvers from KDL.

>

> I was thinking about a workaround:

> I could compute the inverse kinematics obtaining a set of joint

> positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

> apply the direct kinematics obtaining a cartesian position. If this

> solution is "close enough" to the original cartesian position i've found

> the solution, if not i can use the already obtained joints positions as

> initial condition for the next iteration and so on.

Indeed.

> Ignoring the performance and time issues, do you think it will converge?

You could experience problems around singularities.

>>> Thank you in advance.

>>>

>>> Andrea Luzzana

Herman

## KDL IK Position Solver

On 05/08/2012 04:18 AM, Herman Bruyninckx wrote:

> On Tue, 8 May 2012, Andrea Luzzana wrote:

>

>> Il 07/05/2012 20:25, Herman Bruyninckx ha scritto:

>>> On Mon, 7 May 2012, Andrea Luzzana wrote:

>>>

>>>> Hi all,

>>>> i am developing a kinematic component that, once loaded an urdf

>>>> description of a kinematic chain, let the user to obtain direct and

>>>> inverse position and velocity kinematics.

>>>> I observed that the NR solver for the inverse position kinematics is

>>>> strongly affected by the initial guess of joint coordinates passed as

>>>> first parameter to the JntToCart method.

>>>

>>> Indeed, which is quite normal for an iterative solver.

>>>

>>>> In some cases the algorithm

>>>> finds very incorrect solutions, in other cases the solution are fine. It

>>>> seems to be independent from the imposed maximum number of iterations,

>>>> the desired precision or the desired cartesian position, as i can

>>>> observe it depends only on the guess joint coordinates passed.

>>>

>>> The initial value of the iteration is indeed the most important

>>> factor. The

>>> other factors you mention have also an influence, but of secondary

>>> importance.

>>>

>>>> I notice this behavior testing the component with a 6-DOF custom-made

>>>> dynamixel arm.

>>>>

>>>> Have someone of you observed this behavior? How can i obtain an example

>>>> of a robust working solution?

>>>

>>> Define "robust". I fear you are looking for a white elephant... :-)

>> Well... let say "not so dependent from the initial condition". I know

>> that "robust" and "iterative" are not friends...

>>>

>>> _If_ your 6DOF arm has one of the traditional "industrial" designs, there

>>> exist closed-form solutions; but they typically have (at least) 8

>>> different

>>> solutions for the inverse position kinematics. These solvers are also in

>>> KDL: ZXXZXZ, etc.

>>>

>> The 6-DOF robot is just an example in order to test the component with

>> some physical robot. In particular, the IK of that robot (3-2-1

>> configuration) can be, of course, resolved in a closed form but i'm not

>> looking for a solution for that particular structure. In any case i'll

>> try other solvers from KDL.

>>

>> I was thinking about a workaround:

>> I could compute the inverse kinematics obtaining a set of joint

>> positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

>> apply the direct kinematics obtaining a cartesian position. If this

>> solution is "close enough" to the original cartesian position i've found

>> the solution, if not i can use the already obtained joints positions as

>> initial condition for the next iteration and so on.

Some remarks:

(1) Typically there is more than one solution to the inverse position

kinematics ( even a typical non-redundant industrial robot has 8

different discrete ways to reach a given cartesian position, we call

these configurations ).

So, giving a close initial solution also functions as a way to choose

configurations.

(2) Try to use as much as possible the inverse _velocity_ kinematics.

(3) The robustness of the inverse position kinematics could be slightly

improved by taking smaller steps, i.e. changing:

delta_twist = diff(f,p_in);

into

delta_twist = diff(f,p_in)*percentage;

in the file chainsolverpos_nr.cpp

The algorithm then becomes very similar to planning a trajectory from

the initial estimate pose to the goal pose. And executing this

trajectory using inverse velocity kinematics and integration.

However, there will still be cases were this fails and it will be

slower for all cases.

(4) Do not choose an initial position that is in a singular

configuration. ( i.e. in a robot position where you cannot move

instantanuously in all directions).

>

> Indeed.

>

>> Ignoring the performance and time issues, do you think it will converge?

>

> You could experience problems around singularities.

>

>>>> Thank you in advance.

>>>>

>>>> Andrea Luzzana

>

> Herman

## KDL IK Position Solver

On 05/08/2012 09:25 AM, Erwin Aertbelien wrote:

>

>

> On 05/08/2012 04:18 AM, Herman Bruyninckx wrote:

>> On Tue, 8 May 2012, Andrea Luzzana wrote:

>>

>>> Il 07/05/2012 20:25, Herman Bruyninckx ha scritto:

>>>> On Mon, 7 May 2012, Andrea Luzzana wrote:

>>>>

>>>>> Hi all,

>>>>> i am developing a kinematic component that, once loaded an urdf

>>>>> description of a kinematic chain, let the user to obtain direct and

>>>>> inverse position and velocity kinematics.

>>>>> I observed that the NR solver for the inverse position kinematics is

>>>>> strongly affected by the initial guess of joint coordinates passed as

>>>>> first parameter to the JntToCart method.

>>>>

>>>> Indeed, which is quite normal for an iterative solver.

>>>>

>>>>> In some cases the algorithm

>>>>> finds very incorrect solutions, in other cases the solution are

>>>>> fine. It

>>>>> seems to be independent from the imposed maximum number of

>>>>> iterations,

>>>>> the desired precision or the desired cartesian position, as i can

>>>>> observe it depends only on the guess joint coordinates passed.

>>>>

>>>> The initial value of the iteration is indeed the most important

>>>> factor. The

>>>> other factors you mention have also an influence, but of secondary

>>>> importance.

>>>>

>>>>> I notice this behavior testing the component with a 6-DOF custom-made

>>>>> dynamixel arm.

>>>>>

>>>>> Have someone of you observed this behavior? How can i obtain an

>>>>> example

>>>>> of a robust working solution?

>>>>

>>>> Define "robust". I fear you are looking for a white elephant... :-)

>>> Well... let say "not so dependent from the initial condition". I know

>>> that "robust" and "iterative" are not friends...

>>>>

>>>> _If_ your 6DOF arm has one of the traditional "industrial" designs,

>>>> there

>>>> exist closed-form solutions; but they typically have (at least) 8

>>>> different

>>>> solutions for the inverse position kinematics. These solvers are

>>>> also in

>>>> KDL: ZXXZXZ, etc.

>>>>

>>> The 6-DOF robot is just an example in order to test the component with

>>> some physical robot. In particular, the IK of that robot (3-2-1

>>> configuration) can be, of course, resolved in a closed form but i'm not

>>> looking for a solution for that particular structure. In any case i'll

>>> try other solvers from KDL.

>>>

>>> I was thinking about a workaround:

>>> I could compute the inverse kinematics obtaining a set of joint

>>> positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

>>> apply the direct kinematics obtaining a cartesian position. If this

>>> solution is "close enough" to the original cartesian position i've

>>> found

>>> the solution, if not i can use the already obtained joints positions as

>>> initial condition for the next iteration and so on.

> Some remarks:

> (1) Typically there is more than one solution to the inverse position

> kinematics ( even a typical non-redundant industrial robot has 8

> different discrete ways to reach a given cartesian position, we call

> these configurations ).

> So, giving a close initial solution also functions as a way to choose

> configurations.

>

> (2) Try to use as much as possible the inverse _velocity_ kinematics.

>

> (3) The robustness of the inverse position kinematics could be slightly

> improved by taking smaller steps, i.e. changing:

> delta_twist = diff(f,p_in);

> into

> delta_twist = diff(f,p_in)*percentage;

> in the file chainsolverpos_nr.cpp

>

> The algorithm then becomes very similar to planning a trajectory from

> the initial estimate pose to the goal pose. And executing this

> trajectory using inverse velocity kinematics and integration.

>

> However, there will still be cases were this fails and it will be

> slower for all cases.

>

> (4) Do not choose an initial position that is in a singular

> configuration. ( i.e. in a robot position where you cannot move

> instantanuously in all directions).

>

>

Thank you very much for the suggestions.

>>

>> Indeed.

>>

>>> Ignoring the performance and time issues, do you think it will

>>> converge?

>>

>> You could experience problems around singularities.

>>

>>>>> Thank you in advance.

>>>>>

>>>>> Andrea Luzzana

>>

>> Herman

## KDL IK Position Solver

On Tue, 8 May 2012, Andrea Luzzana wrote:

> On 05/08/2012 09:25 AM, Erwin Aertbelien wrote:

>>

>>

>> On 05/08/2012 04:18 AM, Herman Bruyninckx wrote:

>>> On Tue, 8 May 2012, Andrea Luzzana wrote:

>>>

>>>> Il 07/05/2012 20:25, Herman Bruyninckx ha scritto:

>>>>> On Mon, 7 May 2012, Andrea Luzzana wrote:

>>>>>

>>>>>> Hi all,

>>>>>> i am developing a kinematic component that, once loaded an urdf

>>>>>> description of a kinematic chain, let the user to obtain direct and

>>>>>> inverse position and velocity kinematics.

>>>>>> I observed that the NR solver for the inverse position kinematics is

>>>>>> strongly affected by the initial guess of joint coordinates passed as

>>>>>> first parameter to the JntToCart method.

>>>>>

>>>>> Indeed, which is quite normal for an iterative solver.

>>>>>

>>>>>> In some cases the algorithm

>>>>>> finds very incorrect solutions, in other cases the solution are

>>>>>> fine. It

>>>>>> seems to be independent from the imposed maximum number of

>>>>>> iterations,

>>>>>> the desired precision or the desired cartesian position, as i can

>>>>>> observe it depends only on the guess joint coordinates passed.

>>>>>

>>>>> The initial value of the iteration is indeed the most important

>>>>> factor. The

>>>>> other factors you mention have also an influence, but of secondary

>>>>> importance.

>>>>>

>>>>>> I notice this behavior testing the component with a 6-DOF custom-made

>>>>>> dynamixel arm.

>>>>>>

>>>>>> Have someone of you observed this behavior? How can i obtain an

>>>>>> example

>>>>>> of a robust working solution?

>>>>>

>>>>> Define "robust". I fear you are looking for a white elephant... :-)

>>>> Well... let say "not so dependent from the initial condition". I know

>>>> that "robust" and "iterative" are not friends...

>>>>>

>>>>> _If_ your 6DOF arm has one of the traditional "industrial" designs,

>>>>> there

>>>>> exist closed-form solutions; but they typically have (at least) 8

>>>>> different

>>>>> solutions for the inverse position kinematics. These solvers are

>>>>> also in

>>>>> KDL: ZXXZXZ, etc.

>>>>>

>>>> The 6-DOF robot is just an example in order to test the component with

>>>> some physical robot. In particular, the IK of that robot (3-2-1

>>>> configuration) can be, of course, resolved in a closed form but i'm not

>>>> looking for a solution for that particular structure. In any case i'll

>>>> try other solvers from KDL.

>>>>

>>>> I was thinking about a workaround:

>>>> I could compute the inverse kinematics obtaining a set of joint

>>>> positions (e.g. the 6 joints of my 6-DOF robot). At this point i can

>>>> apply the direct kinematics obtaining a cartesian position. If this

>>>> solution is "close enough" to the original cartesian position i've

>>>> found

>>>> the solution, if not i can use the already obtained joints positions as

>>>> initial condition for the next iteration and so on.

>> Some remarks:

>> (1) Typically there is more than one solution to the inverse position

>> kinematics ( even a typical non-redundant industrial robot has 8

>> different discrete ways to reach a given cartesian position, we call

>> these configurations ).

>> So, giving a close initial solution also functions as a way to choose

>> configurations.

>>

>> (2) Try to use as much as possible the inverse _velocity_ kinematics.

>>

>> (3) The robustness of the inverse position kinematics could be slightly

>> improved by taking smaller steps, i.e. changing:

>> delta_twist = diff(f,p_in);

>> into

>> delta_twist = diff(f,p_in)*percentage;

>> in the file chainsolverpos_nr.cpp

Indeed. This is another way of saying that a _numerical_ IK position solver

_always_ works as a velocity controller that tries to bring the robot form

its current position to the desired one. In my opionion, it is much better

to not hide that fact away behind an API, but make use of it explicitly.

>> The algorithm then becomes very similar to planning a trajectory from

>> the initial estimate pose to the goal pose. And executing this

>> trajectory using inverse velocity kinematics and integration.

>>

>> However, there will still be cases were this fails and it will be

>> slower for all cases.

I think it makes sense to trade in speed for robustness, since that was the

question in the initial post.

>> (4) Do not choose an initial position that is in a singular

>> configuration. ( i.e. in a robot position where you cannot move

>> instantanuously in all directions).

>>

>>

> Thank you very much for the suggestions.

>

>>>

>>> Indeed.

>>>

>>>> Ignoring the performance and time issues, do you think it will

>>>> converge?

>>>

>>> You could experience problems around singularities.

>>>

>>>>>> Thank you in advance.

>>>>>>

>>>>>> Andrea Luzzana

Herman