Calculation of inverse kinematics

Hello,

I have a newbie question. I am trying to calculate inverse kinematics with KDL, but I have problems with joint values at the end. Either I get 6 q values of inverse kinematics or I get a "-3". Then I increased the number of my iterations. I think I make a implementation mistake and can´t find it anymore. Would you please have a look and make comments about my code and say where I make mistake(s).

  1. include "stdafx.h"
  2. include "kdl/chain.hpp"
  3. include "kdl/chainfksolver.hpp"
  4. include "kdl/chainfksolverpos_recursive.hpp"
  5. include "kdl/frames_io.hpp"
  6. include "kdl/chainiksolvervel_pinv.hpp"
  7. include "kdl/ChainIkSolverPos_NR.hpp"
  8. include "kdl/ChainIkSolverPos_NR_JL.hpp"
  9. include "stdio.h"
  10. include "iostream"
  11. include "kdl/chainiksolvervel_wdls.hpp"

/////////////////////////////////////////////////////////////////////////////////

KDL::Chain chain;

                                                                                                             //a        alfa          d      theta
    chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH( 410.0, -PI/2,  865.0,   0.0)));
    chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(1000.0,   0.0,    0.0, -PI/2)));
    chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(  45.0, -PI/2,    0.0,  0.0)));
    chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(   0.0,  PI/2, 1000.0,  0.0)));
    chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(   0.0, -PI/2,    0.0,  0.0)));
    chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(   0.0,   0.0,  210.0,  0.0)));

    cout << "GETNROFJNT: " << chain.getNrOfJoints()<< endl <<  endl;
//Joint Limits//
 JntArray jointArrayMin = JntArray(6);
 JntArray jointArrayMax = JntArray(6);

 jointArrayMin(0) = -3.22885911618951; //-185
 jointArrayMin(1) = -2.26892802759263; //-130
 //jointArrayMin(1) = -0.698131700797731; //-40
 jointArrayMin(2) = -2.09439510239319; //-120
 jointArrayMin(3) = -6.10865238198015; //-350
 jointArrayMin(4) = -2.09439510239319; //-120
 jointArrayMin(5) = -6.10865238198015; //-350

 jointArrayMax(0) =  3.22885911618951;   //185
 jointArrayMax(1) =  0.0523598775598298;//3
 jointArrayMax(2) =  2.5830872929516;     //148
 jointArrayMax(3) =  6.10865238198015;   //350
 jointArrayMax(4) =  2.09439510239319;   //120
 jointArrayMax(5) =  6.10865238198015;   //350

 //Def. init. param.
 JntArray q(chain.getNrOfJoints());
 //JntArray q_init(chain.getNrOfJoints());

 
 //FK
 ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );

 //Initial q values///
 JntArray theta = JntArray(6);
 theta(0) = 0.0;
 theta(1) = -0.174532925199433;
 theta(2) = 0.174532925199433;
 theta(3) = 0.0;
 theta(4) = 0.174532925199433;
 theta(5) = 0.0;

 Frame cartPos;
 bool kinStat;
 kinStat = fkSolver.JntToCart(jointpositions, cartPos);
 if(kinStat >= 0) {
        cout << cartPos << endl;
        printf("%s \n","FK calculation succeeded!\n");
    }
 else
    {
        printf("%s \n", "[Error] FK could not be calculated!\n");
    }

 //end-effector
 Frame F_dest;
 //Position 1 (Initial position) -Testpoints-
 //F_dest.p = Vector(1479.2, 0, 1900); ok
 //F_dest.p = Vector(1610, 0, 1900); ok
 F_dest.p = Vector(1479.2, 0, 1850); 
 //F_dest.p = Vector(1479.2, 0, 1900); ok
 F_dest.M= Rotation::RPY(0.0, 0.0, 0.0);

 //Position 2 (Testpoint)
 //F_dest.p= Vector(0.41, 0.42, 1.3);
 //F_dest.M= Rotation::RPY(-0.221255389275321, 0.0222354946704077, 2.7216140756824);

 //IK
 ChainIkSolverVel_wdls ikSolverVel = ChainIkSolverVel_wdls( chain, 0.000001, 150 );
 ChainIkSolverPos_NR_JL ikSolverPos = ChainIkSolverPos_NR_JL( chain, jointArrayMin, jointArrayMax, fkSolver, ikSolverVel, 1000, 10 );

 int ret = ikSolverPos.CartToJnt(jointpositions, F_dest, theta); 

 if( ikSolverPos.CartToJnt( jointpositions, F_dest, theta ) < 0 ) {
     cerr << " [ERROR] IKcould not be calculated, Return: " << ret << endl;
     cout << endl;
 }
 else {
    for( int i = 0; i < 6; i++ )
        cout << "joint #" << i + 1 << ": " << theta( i ) << endl;
 }
And at the end, I get the joint values for: Jnt 1: 0deg Jnt 2: -8.09deg Jnt 3: 8.68deg Jnt 4: 349.99deg Jnt 5: -0.59deg Jnt 6: 349.99deg

Thanks in advance!

Ruben Smits's picture

Calculation of inverse kinematics

On Thursday 19 May 2011 13:19:27 cml [dot] ricardo [..] ... wrote:
> Hello,
>
> I have a newbie question. I am trying to calculate inverse kinematics with
> KDL, but I have problems with joint values at the end. Either I get 6 q
> values of inverse kinematics or I get a "-3". Then I increased the number
> of my iterations. I think I make a implementation mistake and can´t find it
> anymore. Would you please have a look and make comments about my code and
> say where I make mistake(s).

First of all, the inverse position kinematic solvers in KDL are very
experimental and far from usable in a real setting. There are various reasons
why the might not find a solution, the most common ones are:

* starting/going through singular positions of your kinematic structure
* getting stuck in local minima, especially when using the solver which uses
joint limits
* unreachable desired poses (KDL uses 6D geometry, position + orientation,
usually people forget about the orientation part of their desired poses.)

> #include "stdafx.h"
> #include "kdl/chain.hpp"
> #include "kdl/chainfksolver.hpp"
> #include "kdl/chainfksolverpos_recursive.hpp"
> #include "kdl/frames_io.hpp"
> #include "kdl/chainiksolvervel_pinv.hpp"
> #include "kdl/ChainIkSolverPos_NR.hpp"
> #include "kdl/ChainIkSolverPos_NR_JL.hpp"
> #include "stdio.h"
> #include "iostream"
> #include "kdl/chainiksolvervel_wdls.hpp"
>
> ////////////////////////////////////////////////////////////////////////////
> /////
>
> KDL::Chain chain;
>
//a alfa d theta
> chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH( 410.0, -PI/2,
> 865.0, 0.0))); chain.addSegment(Segment(Joint(Joint::RotZ),
> Frame::DH(1000.0, 0.0, 0.0, -PI/2)));
> chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH( 45.0, -PI/2,
> 0.0, 0.0))); chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(
> 0.0, PI/2, 1000.0, 0.0))); chain.addSegment(Segment(Joint(Joint::RotZ),
> Frame::DH( 0.0, -PI/2, 0.0, 0.0)));
> chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH( 0.0, 0.0,
> 210.0, 0.0)));

KDL uses SI units, being meters and radians. So this is a very big kinematic
structure ;), but it should not harm the rest of the calculations

> cout << "GETNROFJNT: " << chain.getNrOfJoints()<< endl << endl;
>
> //Joint Limits//
> JntArray jointArrayMin = JntArray(6);
> JntArray jointArrayMax = JntArray(6);
>
> jointArrayMin(0) = -3.22885911618951; //-185
> jointArrayMin(1) = -2.26892802759263; //-130
> //jointArrayMin(1) = -0.698131700797731; //-40
> jointArrayMin(2) = -2.09439510239319; //-120
> jointArrayMin(3) = -6.10865238198015; //-350
> jointArrayMin(4) = -2.09439510239319; //-120
> jointArrayMin(5) = -6.10865238198015; //-350
>
> jointArrayMax(0) = 3.22885911618951; //185
> jointArrayMax(1) = 0.0523598775598298;//3
> jointArrayMax(2) = 2.5830872929516; //148
> jointArrayMax(3) = 6.10865238198015; //350
> jointArrayMax(4) = 2.09439510239319; //120
> jointArrayMax(5) = 6.10865238198015; //350
>
>
> //Def. init. param.
> JntArray q(chain.getNrOfJoints());
> //JntArray q_init(chain.getNrOfJoints());
>
>
> //FK
> ChainFkSolverPos_recursive fkSolver = ChainFkSolverPos_recursive( chain );
>
>
> //Initial q values///
> JntArray theta = JntArray(6);
> theta(0) = 0.0;
> theta(1) = -0.174532925199433;
> theta(2) = 0.174532925199433;
> theta(3) = 0.0;
> theta(4) = 0.174532925199433;
> theta(5) = 0.0;
>
>
> Frame cartPos;
> bool kinStat;
> kinStat = fkSolver.JntToCart(jointpositions, cartPos);
> if(kinStat >= 0) {
> cout << cartPos << endl;
> printf("%s \n","FK calculation succeeded!\n");
> }
> else
> {
> printf("%s \n", "[Error] FK could not be calculated!\n");
> }
>
>
> //end-effector
> Frame F_dest;
> //Position 1 (Initial position) -Testpoints-
> //F_dest.p = Vector(1479.2, 0, 1900); ok
> //F_dest.p = Vector(1610, 0, 1900); ok
> F_dest.p = Vector(1479.2, 0, 1850);
> //F_dest.p = Vector(1479.2, 0, 1900); ok
> F_dest.M= Rotation::RPY(0.0, 0.0, 0.0);
>
> //Position 2 (Testpoint)
> //F_dest.p= Vector(0.41, 0.42, 1.3);
> //F_dest.M= Rotation::RPY(-0.221255389275321, 0.0222354946704077,
> 2.7216140756824);
>
>
> //IK
> ChainIkSolverVel_wdls ikSolverVel = ChainIkSolverVel_wdls( chain, 0.000001,
> 150 ); ChainIkSolverPos_NR_JL ikSolverPos = ChainIkSolverPos_NR_JL( chain,
> jointArrayMin, jointArrayMax, fkSolver, ikSolverVel, 1000, 10 );
>
> int ret = ikSolverPos.CartToJnt(jointpositions, F_dest, theta);

You give an uninitialized joint array as initial positions (jointpositions),
this probably means you are starting in a singular position of your robot. Try
initialising it with values different from zero.

> if( ikSolverPos.CartToJnt( jointpositions, F_dest, theta ) < 0 ) {
> cerr << " [ERROR] IKcould not be calculated, Return: " << ret << endl;
> cout << endl;
> }
> else {
> for( int i = 0; i < 6; i++ )
> cout << "joint #" << i + 1 << ": " << theta( i ) << endl;
> }
>
>
>
> And at the end, I get the joint values for:
> Jnt 1: 0deg
> Jnt 2: -8.09deg
> Jnt 3: 8.68deg
> Jnt 4: 349.99deg
> Jnt 5: -0.59deg
> Jnt 6: 349.99deg
>

Can you check what the pose is with these angles? Is it far from the desired
pose? Joint 4 and 6 are close to there limit, this probably means that the
desired position is not reachable or you ended up in a local minimum of the
solver. Try with different initial values.

>
> Thanks in advance!

-- Ruben