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