# Inverse Kinematics problem

 Submitted by uriel on Thu, 2011-04-28 15:33

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