Hi all,

I would like to implement inverse kinematics using KDL, I have used the code of the example provided but I have not the desired results. I am implementing forward kinematics and after that I set the desired position to get the angles but It does not work. Here is the code:

1. include <iostream>
2. include <stdlib.h>
3. include <kdl/chain.hpp>
4. include <kdl/chainiksolver.hpp>
5. include <kdl/chainfksolver.hpp>
6. include <kdl/chainfksolverpos_recursive.hpp>
7. include <kdl/chainiksolvervel_pinv.hpp>
8. include <kdl/chainiksolverpos_nr.hpp>
9. include <kdl/frames_io.hpp>

using namespace KDL; using namespace std;

int main(int argc, char** argv) {

    KDL::Chain chain;
    chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ), KDL::Frame(Vector(1.0, 0.0, 0.0))));

    ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);

    KDL::JntArray jointpositions = JntArray(chain.getNrOfJoints());

    cout << "Initial Pose: " << (chain.getSegment(0)).pose(jointpositions(0)) << endl;
    cout << endl << "Direct Kinematics" << endl << endl;
    cout << "Set the Joint angles" << endl;
    double position = 0.0;
    for( int i = 0; i < chain.getNrOfJoints(); i++ )
    {
        cout << "Q(" << i << "): ";
        cin >> position;
        cout << position * M_PI / 180 << endl;
        jointpositions( i ) = position * M_PI / 180;
    }

    KDL::Frame cartpos;

    bool succes = fksolver.JntToCart(jointpositions, cartpos);

    cout << "Status: " << succes << endl;
    cout << "Frame: " << cartpos << endl;

    getchar();
    cout << endl << "Inverse Kinematics" << endl << endl;

    ChainIkSolverVel_pinv iksolver1v(chain);
    ChainIkSolverPos_NR iksolver1(chain, fksolver, iksolver1v, 100, 1e-6);

    JntArray q(chain.getNrOfJoints());
    JntArray q_init(chain.getNrOfJoints());

    cout << "Set desired position: ";
    double px, py, pz;
    cin >> px >> py >> pz;
    Frame F_dest = Frame(Vector(px, py, pz));
    int ret = iksolver1.CartToJnt(q_init, F_dest, q);

    cout << "Status: " << ret << endl;
    cout << "Frame: " << F_dest << endl;
    cout << "Angles: ";
    for( int i = 0; i < chain.getNrOfJoints(); i++ )
        cout << endl << "Q(" << i << "): " << q(i) * 180 / M_PI;

    cout << endl;

    return 0;

The result is that I do not have the correct angles of rotation and in the ret variable I always get -3, so there is something wrong.

Could someone help me with this?

Thanks