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