[KDL] newbie problems with IK solver

Hi everybody,

I am new to KDL and robotics in general, so this might be kind of a newbie question. Although this is a lot of text the problem itself might be a very easy to solve and the solution might be very obvious (to you), so I'd ask you read through this, thanks.

I have some problems calculating IK for my segment chain. For testing purposes I am using a small Lynxmotion robot (see picture below to get an idea of the segment configuration). I tried to build my segment chain accordingly (since I'm computer graphcis programmer the orientation of the bases might differ from the one you are used to, I don't know...), here's the test code:


using namespace KDL;int main( int argc, char **argv ) {
    Chain chain;
    chain.addSegment( Segment( Joint( Joint::None ), Frame( Vector( 0.0, 6.7, 0.0 ) ) ) );
    chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.8, 0.0 ) ) ) );
    chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.5, 0.0 ) ) ) );
    chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 3.4, 0.0 ) ) ) );
    chain.addSegment( 
        Segment( 
            Joint( Joint::RotY ), 
            Frame( 
                Rotation( 
                    Vector( 0.0, 0.0, -1.0 ),
                    Vector( 1.0, 0.0, 0.0 ),
                    Vector( 0.0, -1.0, 0.0 ) ),
                Vector( 0.0, 6.1, 0.0 ) 
            ) 
        ) 
    );

    ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );
 
    int jointCount = chain.getNrOfJoints();
    JntArray jointPositions = JntArray( jointCount );

    ChainIkSolverVel_pinv ikSolverVel = ChainIkSolverVel_pinv( chain, 0.0, 150 );
    ChainIkSolverPos_NR ikSolverPos = ChainIkSolverPos_NR( chain, fkSolver, ikSolverVel, 100, 0.000001 );

    JntArray resultJoints = JntArray( jointCount );

    Frame destFrame( 
        Rotation::Rotation( 
            Vector( 0.0, 0.0, -1.0 ),
            Vector( 0.0, 1.0, 0.0 ),
            Vector( 1.0, 0.0, 0.0 ) ),
        Vector( -30, 10.0, 0.0 ) );

    if( ikSolverPos.CartToJnt( jointPositions, destFrame, resultJoints ) < 0 )
        std::cerr << "[ERROR] solving IK failed" << std::endl;

    for( int i = 0; i < jointCount; i++ )
        std::cout << "joint #" << i << ": " << resultJoints( i ) << std::endl;
}

As you can see I rotate each Joint around the z-axis. The frame-to-tip transformations are just translations along the y-axes (up), the last one being an exception since it represents the rotation of the gripper along its forward axes. This seems to work, also if I move a virtual model of my robot accordingly, the tool is at the expected position. here is the output:


joint #0: 0.576486 joint #1: 1.69103 joint #2: -0.696722 joint #3: 0

Unfortunately, if I change the joint type of the first segment from Joint::None to Joint::RotY (since it actually represents the rotation of the whole arm around the base --- the y-axis in my coordinate system), the IK solver is not successful.


[ERROR] solving IK failed joint #0: 1.#QNAN joint #1: 1.#QNAN joint #2: 1.#QNAN joint #3: 1.#QNAN joint #4: 1.#QNAN

This somewhat confuses me. How can the IK calculation be not successful if I *add* a degree of freedom? Note that I try to calculate the IK for a position right in front of the robot so there is not even a need for a base-rotation at all, so it should not make any difference at all? I noticed that instead of returning weird numbers, what I observed that normally is the case if the frame is not reachable, in this case I get NANs, so this might be a hint to the problem, but since I am not familiar with the mathematics of IK (and maybe of relevant preconditions) I am not able to figure out what the actual problem is.

Thanks for reading and for your help.

--ithrak.

http://www.lynxmotion.com/images/jpg/al5c.jpg

[KDL] newbie problems with

Thanks to both of you!

@Ruben: Seems like this was the problem.

@bruyninc: I'm not sure what's the difference between the wdls and the pinv velocity solvers. I would like to use the cheapest solver in terms of computation that is available since I have much other calculations going on and accuracy is not a critical factor to me. I'm wondering since you wrote this:

>>but, in the second case, the extra joint
>>gives the robot the ability to move
>>_outside_ of the plane. even with the
>>smallest numerical non-zero value of Joint
>>0, the end-effector is then not reachable
>>anymore. -> singularity!

-> Wouldn't this be a very common setup anyway? I think most of the time I try to calculate IK, I have three dimensions and do NOT only move along a plane. Is the pinv solver only useful for 2D-IK?

[KDL] newbie problems with

Hi again and sorry for the double-post but since I am in a hurry and really would need some help I want to get sure you see this post.

I am experiencing some serious problems using the ChainIkSolverPos_NR_JL solver. In fact I'm not getting the required info out of the documentation and cannot find any examples either. Do you have some hints?

In particular the problem is I don't know how to specify the joint limits. My guess was:

    JntArray jointArrayMin = JntArray( jointCount );
    JntArray jointArrayMax = JntArray( jointCount );

    jointArrayMin( 0 ) = -ahumari::PI() / 2.0;
    jointArrayMin( 1 ) = 0.0;
    jointArrayMin( 2 ) = 0.0;
    jointArrayMin( 3 ) = -ahumari::PI() / 2.0;
    jointArrayMin( 4 ) = -ahumari::PI() / 2.0;

    jointArrayMax( 0 ) = ahumari::PI() / 2.0;
    jointArrayMax( 1 ) = ahumari::PI() / 2.0;
    jointArrayMax( 2 ) = ahumari::PI();
    jointArrayMax( 3 ) = 0.15;
    jointArrayMax( 4 ) = ahumari::PI() / 2.0;

    ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );
    KDL::JntArray jointPositions = JntArray( jointCount );

    ChainIkSolverVel_wdls ikSolverVel = ChainIkSolverVel_wdls( chain, 0.000001, 150 );

    ChainIkSolverPos_NR_JL ikSolverPos = ChainIkSolverPos_NR_JL( chain, jointArrayMin, jointArrayMax, fkSolver, ikSolverVel, 100, 0.000001 );
but unfortunately this doesn't work, the CartToJnt call fails. If I use a ChainIkSolverPos_NR (no joint limits) instead, I get these results:

    joint #0: 0
    joint #1: -18.2731
    joint #2: 39.3901
    joint #3: -19.5463
    joint #4: 8.74416e-027
In the range of -PI to +PI this would be:

    joint #0: 0
    joint #1: 0.576486
    joint #2: 1.69103
    joint #3: -0.696722
    joint #4: 8.74416e-027
Note that the angles are all within the ranges I specified in my JntArrays. But unfortunately if I use the ChainIkSolverPos_NR_JL no solution is found. I'm sure this has to do with the fact that the resulting Angles are not in the range of -PI to +PI or even 0 to 2*PI, so how do I have to specify joint limits?

Thanks for your help!

Ruben Smits's picture

[KDL] newbie problems

On Fri, Jan 29, 2010 at 11:37 AM, peter [dot] soetens [..] ...
<peter [dot] soetens [..] ...> wrote:
> Hi again and sorry for the double-post but since I am in a hurry and really would need some help I want to get sure you see this post.
>
> I am experiencing some serious problems using the ChainIkSolverPos_NR_JL solver. In fact I'm not getting the required info out of the documentation and cannot find any examples either. Do you have some hints?
>
> In particular the problem is I don't know how to specify the joint limits. My guess was:
>
>        JntArray jointArrayMin = JntArray( jointCount );
>        JntArray jointArrayMax = JntArray( jointCount );
>
>        jointArrayMin( 0 ) = -ahumari::PI() / 2.0;
>        jointArrayMin( 1 ) = 0.0;
>        jointArrayMin( 2 ) = 0.0;
>        jointArrayMin( 3 ) = -ahumari::PI() / 2.0;
>        jointArrayMin( 4 ) = -ahumari::PI() / 2.0;
>
>        jointArrayMax( 0 ) = ahumari::PI() / 2.0;
>        jointArrayMax( 1 ) = ahumari::PI() / 2.0;
>        jointArrayMax( 2 ) = ahumari::PI();
>        jointArrayMax( 3 ) = 0.15;
>        jointArrayMax( 4 ) = ahumari::PI() / 2.0;
>
>        ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );
>        KDL::JntArray jointPositions = JntArray( jointCount );
>
>        ChainIkSolverVel_wdls ikSolverVel = ChainIkSolverVel_wdls( chain, 0.000001, 150 );
>
>        ChainIkSolverPos_NR_JL ikSolverPos = ChainIkSolverPos_NR_JL( chain, jointArrayMin, jointArrayMax, fkSolver, ikSolverVel, 100, 0.000001 );
>
> but unfortunately this doesn't work, the CartToJnt call fails. If I use a ChainIkSolverPos_NR (no joint limits) instead, I get these results:

You forgot to mention how you invoke CartToJnt, could it be that your
initial guess is a singular position for the chain? If not it might be
that you are trapped in a local minimum which this solver cannot deal
with. Or you need to allow more iterations for the position solver.

Ruben
>        joint #0: 0
>        joint #1: -18.2731
>        joint #2: 39.3901
>        joint #3: -19.5463
>        joint #4: 8.74416e-027
>
> In the range of -PI to +PI this would be:
>
>        joint #0: 0
>        joint #1: 0.576486
>        joint #2: 1.69103
>        joint #3: -0.696722
>        joint #4: 8.74416e-027
>
> Note that the angles are all within the ranges I specified in my JntArrays. But unfortunately if I use the ChainIkSolverPos_NR_JL no solution is found. I'm sure this has to do with the fact that the resulting Angles are not in the range of -PI to +PI or even 0 to 2*PI, so how do I have to specify joint limits?
>
> Thanks for your help!
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>

[KDL] newbie problems

Yeah, I think this was the problem.

Thanks, Ruben!

[KDL] newbie problems

Yeah, I think the problem was that I started in a singularity.

Thanks, Ruben!

[EDIT] If anyone reads this, who has the same troubles: I also remember the problem of choosing a bad start configuration. I had to put the manipulator into a pose similar to the one to be reached. First I just rotated all joints for some degrees to exit the singularity -- all of them into the same direction, so the arm was almost vertically but twisted slightly. Using this start configuration the solver could not find a solution. When I later chose a start configuration in a more zig-zag manner, that seems to be more "common" to a gripper, it suddenly worked.

[KDL] newbie problems with

Thanks to both of you!

@Ruben: Seems like this was the problem.

@bruyninc: I'm not sure what's the difference between the wdls and the pinv velocity solvers. I would like to use the cheapest solver in terms of computation that is available since I have much other calculations going on and accuracy is not a critical factor to me. I'm wondering since you wrote this:

>>but, in the second case, the extra joint >>gives the robot the ability to move >>_outside_ of the plane. even with the >>smallest numerical non-zero value of Joint >>0, the end-effector is then not reachable >>anymore. -> singularity!

-> Wouldn't this be a very common setup anyway? I think most of the time I try to calculate IK, I have three dimensions and do NOT only move along a plane. Is the pinv solver only useful for 2D-IK?

[KDL] newbie problems

(dublicate post deleted)

[KDL] newbie problems with IK solver

On Fri, 15 Jan 2010, orocos [..] ... wrote:

> I am new to KDL and robotics in general, so this might be kind of a newbie question. Although this is a lot of text the problem itself might be a very easy to solve and the solution might be very obvious (to you), so I'd ask you read through this, thanks.
>
> I have some problems calculating IK for my segment chain. For testing purposes I am using a small Lynxmotion robot (see picture below to get an idea of the segment configuration). I tried to build my segment chain accordingly (since I'm computer graphcis programmer the orientation of the bases might differ from the one you are used to, I don't know...), here's the test code:
>
> -------------------------------------------------------------------------------------
> using namespace KDL;
>
> int main( int argc, char **argv )
> {
> Chain chain;
> chain.addSegment( Segment( Joint( Joint::None ), Frame( Vector( 0.0, 6.7, 0.0 ) ) ) );
> chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.8, 0.0 ) ) ) );
> chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.5, 0.0 ) ) ) );
> chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 3.4, 0.0 ) ) ) );
> chain.addSegment(
> Segment(
> Joint( Joint::RotY ),
> Frame(
> Rotation(
> Vector( 0.0, 0.0, -1.0 ),
> Vector( 1.0, 0.0, 0.0 ),
> Vector( 0.0, -1.0, 0.0 ) ),
> Vector( 0.0, 6.1, 0.0 )
> )
> )
> );
>
> ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );
>
> int jointCount = chain.getNrOfJoints();
> JntArray jointPositions = JntArray( jointCount );
>
> ChainIkSolverVel_pinv ikSolverVel = ChainIkSolverVel_pinv( chain, 0.0, 150 );
> ChainIkSolverPos_NR ikSolverPos = ChainIkSolverPos_NR( chain, fkSolver, ikSolverVel, 100, 0.000001 );
>
> JntArray resultJoints = JntArray( jointCount );
>
> Frame destFrame(
> Rotation::Rotation(
> Vector( 0.0, 0.0, -1.0 ),
> Vector( 0.0, 1.0, 0.0 ),
> Vector( 1.0, 0.0, 0.0 ) ),
> Vector( -30, 10.0, 0.0 ) );
>
> if( ikSolverPos.CartToJnt( jointPositions, destFrame, resultJoints ) < 0 )
> std::cerr << "[ERROR] solving IK failed" << std::endl;
>
> for( int i = 0; i < jointCount; i++ )
> std::cout << "joint #" << i << ": " << resultJoints( i ) << std::endl;
> }
> -------------------------------------------------------------------------------------
>
> As you can see I rotate each Joint around the z-axis. The frame-to-tip transformations are just translations along the y-axes (up), the last one being an exception since it represents the rotation of the gripper along its forward axes. This seems to work, also if I move a virtual model of my robot accordingly, the tool is at the expected position. here is the output:
>
> -------------------------------------------------------------------------------------
> joint #0: 0.576486
> joint #1: 1.69103
> joint #2: -0.696722
> joint #3: 0
> -------------------------------------------------------------------------------------
>
>
> Unfortunately, if I change the joint type of the first segment from Joint::None to Joint::RotY (since it actually represents the rotation of the whole arm around the base --- the y-axis in my coordinate system), the IK solver is not successful.
>
> -------------------------------------------------------------------------------------
> [ERROR] solving IK failed
> joint #0: 1.#QNAN
> joint #1: 1.#QNAN
> joint #2: 1.#QNAN
> joint #3: 1.#QNAN
> joint #4: 1.#QNAN
> -------------------------------------------------------------------------------------
>
> This somewhat confuses me. How can the IK calculation be not successful if I *add* a degree of freedom? Note that I try to calculate the IK for a position right in front of the robot so there is not even a need for a base-rotation at all, so it should not make any difference at all? I noticed that instead of returning weird numbers, what I observed that normally is the case if the frame is not reachable, in this case I get NANs, so this might be a hint to the problem, but since I am not familiar with the mathematics of IK (and maybe of relevant preconditions) I am not able to figure out what the actual problem is.
>
> Thanks for reading and for your help.
>
> --ithrak.
>
> http://www.lynxmotion.com/images/jpg/al5c.jpg

_Maybe_ this is the cause of the problems:
- in the first case (without the extra joint in the root segment) your
destination frame lies in the XY plane
- and all your joint just move the end-effector in that plane
- hence, you are basically solving a planar problem.
- but, in the second case, the extra joint gives the robot the ability to
move _outside_ of the plane
- even with the smallest numerical non-zero value of Joint 0, the
end-effector is then not reachable anymore.
-> singularity!
- you are not using the singularity-robust IK of
<http://people.mech.kuleuven.be/~orocos/pub/devel/kdl/latest/api/html/classKDL_1_1ChainIkSolverVel__wdls.html>
- hence, small numerical errors could "kill" your IK computations when
using the (simpler) "ChainIkSolverVel_pinv" solver.

"Kill" could mean that you keep on doing iterations because you don't reach
the planar case within the specified epsilon

Caveat: I might be wrong about what I say above, since I am not sure
whether I completely understand your case.

Herman

Ruben Smits's picture

[KDL] newbie problems with IK solver

On Fri, Jan 15, 2010 at 2:14 PM, orocos [..] ...
<orocos [..] ...> wrote:
> Hi everybody,
>
> I am new to KDL and robotics in general, so this might be kind of a newbie question. Although this is a lot of text the problem itself might be a very easy to solve and the solution might be very obvious (to you), so I'd ask you read through this, thanks.
>
> I have some problems calculating IK for my segment chain. For testing purposes I am using a small Lynxmotion robot (see picture below to get an idea of the segment configuration). I tried to build my segment chain accordingly (since I'm computer graphcis programmer the orientation of the bases might differ from the one you are used to, I don't know...), here's the test code:
>
> -------------------------------------------------------------------------------------
> using namespace KDL;
>
> int main( int argc, char **argv )
> {
>        Chain chain;
>        chain.addSegment( Segment( Joint( Joint::None ), Frame( Vector( 0.0, 6.7, 0.0 ) ) ) );
>        chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.8, 0.0 ) ) ) );
>        chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.5, 0.0 ) ) ) );
>        chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 3.4, 0.0 ) ) ) );
>        chain.addSegment(
>                Segment(
>                        Joint( Joint::RotY ),
>                        Frame(
>                                Rotation(
>                                        Vector( 0.0, 0.0, -1.0 ),
>                                        Vector( 1.0, 0.0, 0.0 ),
>                                        Vector( 0.0, -1.0, 0.0 ) ),
>                                Vector( 0.0, 6.1, 0.0 )
>                        )
>                )
>        );
>
>        ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );
>
>        int jointCount = chain.getNrOfJoints();
>        JntArray jointPositions = JntArray( jointCount );
>
>        ChainIkSolverVel_pinv ikSolverVel = ChainIkSolverVel_pinv( chain, 0.0, 150 );
>        ChainIkSolverPos_NR ikSolverPos = ChainIkSolverPos_NR( chain, fkSolver, ikSolverVel, 100, 0.000001 );
>
>        JntArray resultJoints = JntArray( jointCount );
>
>        Frame destFrame(
>                Rotation::Rotation(
>                        Vector( 0.0, 0.0, -1.0 ),
>                        Vector( 0.0, 1.0, 0.0 ),
>                        Vector( 1.0, 0.0, 0.0 ) ),
>                Vector( -30, 10.0, 0.0 ) );
>
>        if( ikSolverPos.CartToJnt( jointPositions, destFrame, resultJoints ) < 0 )
>                std::cerr << "[ERROR] solving IK failed" << std::endl;
>
>        for( int i = 0; i < jointCount; i++ )
>                std::cout << "joint #" << i << ": " << resultJoints( i ) << std::endl;
> }
> -------------------------------------------------------------------------------------
>
> As you can see I rotate each Joint around the z-axis. The frame-to-tip transformations are just translations along the y-axes (up), the last one being an exception since it represents the rotation of the gripper along its forward axes. This seems to work, also if I move a virtual model of my robot accordingly, the tool is at the expected position. here is the output:
>
> -------------------------------------------------------------------------------------
> joint #0: 0.576486
> joint #1: 1.69103
> joint #2: -0.696722
> joint #3: 0
> -------------------------------------------------------------------------------------
>
>
> Unfortunately, if I change the joint type of the first segment from Joint::None to Joint::RotY (since it actually represents the rotation of the whole arm around the base --- the y-axis in my coordinate system), the IK solver is not successful.
>
> -------------------------------------------------------------------------------------
> [ERROR] solving IK failed
> joint #0: 1.#QNAN
> joint #1: 1.#QNAN
> joint #2: 1.#QNAN
> joint #3: 1.#QNAN
> joint #4: 1.#QNAN
> -------------------------------------------------------------------------------------
>
> This somewhat confuses me. How can the IK calculation be not successful if I *add* a degree of freedom? Note that I try to calculate the IK for a position right in front of the robot so there is not even a need for a base-rotation at all, so it should not make any difference at all? I noticed that instead of returning weird numbers, what I observed that normally is the case if the frame is not reachable, in this case I get NANs, so this might be a hint to the problem, but since I am not familiar with the mathematics of IK (and maybe of relevant preconditions) I am not able to figure out what the actual problem is.
>

You probably start in a singular position of the robot: Both Y-axes
are aligned, the iksolvervel-pinv is not able to deal with this kind
of situations.

You can try to start in a non-singular position (make jointpositions
not equal to zero for one of the Z-joints)

or

you can try to use the ChainIkSolverVel_wdls
<http://people.mech.kuleuven.be/~orocos/pub/devel/kdl/latest/api/html/classKDL_1_1ChainIkSolverVel__wdls.html>
instead, choosing the right value for lamda and eps should do the
trick.

Also don't forget that the ChainIkSolverPos_NR always tries to solve
the full 6D position-orientation problem.

Ruben

> Thanks for reading and for your help.
>
> --ithrak.
>
> http://www.lynxmotion.com/images/jpg/al5c.jpg
> --
> Orocos-Users mailing list
> Orocos-Users [..] ...
> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users
>

[KDL] newbie problems with IK solver

Hi everybody,

I am new to KDL and robotics in general, so this might be kind of a newbie question. Although this is a lot of text the problem itself might be a very easy to solve and the solution might be very obvious (to you), so I'd ask you read through this, thanks.

I have some problems calculating IK for my segment chain. For testing purposes I am using a small Lynxmotion robot (see picture below to get an idea of the segment configuration). I tried to build my segment chain accordingly (since I'm computer graphcis programmer the orientation of the bases might differ from the one you are used to, I don't know...), here's the test code:

-------------------------------------------------------------------------------------
using namespace KDL;

int main( int argc, char **argv )
{
Chain chain;
chain.addSegment( Segment( Joint( Joint::None ), Frame( Vector( 0.0, 6.7, 0.0 ) ) ) );
chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.8, 0.0 ) ) ) );
chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 15.5, 0.0 ) ) ) );
chain.addSegment( Segment( Joint( Joint::RotZ ), Frame( Vector( 0.0, 3.4, 0.0 ) ) ) );
chain.addSegment(
Segment(
Joint( Joint::RotY ),
Frame(
Rotation(
Vector( 0.0, 0.0, -1.0 ),
Vector( 1.0, 0.0, 0.0 ),
Vector( 0.0, -1.0, 0.0 ) ),
Vector( 0.0, 6.1, 0.0 )
)
)
);

ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );

int jointCount = chain.getNrOfJoints();
JntArray jointPositions = JntArray( jointCount );

ChainIkSolverVel_pinv ikSolverVel = ChainIkSolverVel_pinv( chain, 0.0, 150 );
ChainIkSolverPos_NR ikSolverPos = ChainIkSolverPos_NR( chain, fkSolver, ikSolverVel, 100, 0.000001 );

JntArray resultJoints = JntArray( jointCount );

Frame destFrame(
Rotation::Rotation(
Vector( 0.0, 0.0, -1.0 ),
Vector( 0.0, 1.0, 0.0 ),
Vector( 1.0, 0.0, 0.0 ) ),
Vector( -30, 10.0, 0.0 ) );

if( ikSolverPos.CartToJnt( jointPositions, destFrame, resultJoints ) < 0 )
std::cerr << "[ERROR] solving IK failed" << std::endl;

for( int i = 0; i < jointCount; i++ )
std::cout << "joint #" << i << ": " << resultJoints( i ) << std::endl;
}
-------------------------------------------------------------------------------------

As you can see I rotate each Joint around the z-axis. The frame-to-tip transformations are just translations along the y-axes (up), the last one being an exception since it represents the rotation of the gripper along its forward axes. This seems to work, also if I move a virtual model of my robot accordingly, the tool is at the expected position. here is the output:

-------------------------------------------------------------------------------------
joint #0: 0.576486
joint #1: 1.69103
joint #2: -0.696722
joint #3: 0
-------------------------------------------------------------------------------------

Unfortunately, if I change the joint type of the first segment from Joint::None to Joint::RotY (since it actually represents the rotation of the whole arm around the base --- the y-axis in my coordinate system), the IK solver is not successful.

-------------------------------------------------------------------------------------
[ERROR] solving IK failed
joint #0: 1.#QNAN
joint #1: 1.#QNAN
joint #2: 1.#QNAN
joint #3: 1.#QNAN
joint #4: 1.#QNAN
-------------------------------------------------------------------------------------

This somewhat confuses me. How can the IK calculation be not successful if I *add* a degree of freedom? Note that I try to calculate the IK for a position right in front of the robot so there is not even a need for a base-rotation at all, so it should not make any difference at all? I noticed that instead of returning weird numbers, what I observed that normally is the case if the frame is not reachable, in this case I get NANs, so this might be a hint to the problem, but since I am not familiar with the mathematics of IK (and maybe of relevant preconditions) I am not able to figure out what the actual problem is.

Thanks for reading and for your help.

--ithrak.

http://www.lynxmotion.com/images/jpg/al5c.jpg