# KDL & inverse kinematic

Submitted by darki on Fri, 2009-09-18 13:04 |

Hi, I'm new here and I have a problem with KDL and the inverse kinematic. For the first tests I used the examples from the KDL documentation. Here is the source code.

// Simple robot arm with two segments. KDL::Chain chain; chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(1.0, 0.0, 0.0)))); chain.addSegment(Segment(Joint(Joint::RotZ), Frame(Vector(1.0, 0.0,0.0)))); // Create solver based on kinematic chain ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain); // Create joint array KDL::JntArray jointpositions = JntArray(chain.getNrOfJoints()); // The joint positions jointpositions(0) = 0; jointpositions(1) = M_PI/2; // Create the frame that will contain the results KDL::Frame cartpos; // Calculate forward position kinematics bool kinematics_status = fksolver.JntToCart(jointpositions, cartpos); Vector tmp(0.0, 0.0, 0.0); Vector tr = cartpos * tmp; std::cout << tr << std::endl;

ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver ChainIkSolverPos_NR iksolver1(chain, fksolver, iksolver1v, 100, 1e-6); //Creation of jntarrays: JntArray q(chain.getNrOfJoints()); JntArray q_init(chain.getNrOfJoints()); //Set destination frame Frame F_dest = Frame(Vector(1.0, 1.0, 0.0)); int ret = iksolver1.CartToJnt(q_init, F_dest, q);

So and now comes the question. What am I doing wrong?

Thanks, darki

»

- Login to post comments

## KDL & inverse kinematic

On Fri, 18 Sep 2009, aschatz [..] ... wrote:

> Hi, I'm new here and I have a problem with KDL and the inverse kinematic. For the first tests I used the examples from the KDL documentation. Here is the source code.

>

` > // Simple robot arm with two segments. > KDL::Chain chain; > chain.addSegment(Segment(Joint(Joint::RotZ), > Frame(Vector(1.0, 0.0, 0.0)))); > chain.addSegment(Segment(Joint(Joint::RotZ), > Frame(Vector(1.0, 0.0,0.0)))); > > // Create solver based on kinematic chain > ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain); > > // Create joint array > KDL::JntArray jointpositions = JntArray(chain.getNrOfJoints()); > > // The joint positions > jointpositions(0) = 0; > jointpositions(1) = M_PI/2; > > // Create the frame that will contain the results > KDL::Frame cartpos; > > // Calculate forward position kinematics > bool kinematics_status = fksolver.JntToCart(jointpositions, cartpos); > > Vector tmp(0.0, 0.0, 0.0); > Vector tr = cartpos * tmp; > std::cout << tr << std::endl; >`

> The result is the vector [1, 1, 0]. Now I try to calculate the joint positions over the inverse kinematic.

>

` > ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver > ChainIkSolverPos_NR iksolver1(chain, fksolver, iksolver1v, 100, 1e-6); > > //Creation of jntarrays: > JntArray q(chain.getNrOfJoints()); > JntArray q_init(chain.getNrOfJoints()); > > //Set destination frame > Frame F_dest = Frame(Vector(1.0, 1.0, 0.0)); > int ret = iksolver1.CartToJnt(q_init, F_dest, q); >`

> The results are angles 1.5708 and 1.5708 => pi/2 and pi/2. If I test now the angles (pi/2, pi/2) with the forward kinematic, I get the position [-1, 1, 0].

>

> So and now comes the question. What am I doing wrong?

I haven't run the code myself, but probably you aren't doing anything

wrong. I think you are not aware of the fact that you solve two different

problems:

- when assigning the joint angles 0 and pi/2, you not only move the end

_position_ to [1 1 0] but you are also changing the end frame

_orientation_.

- with "F_dest = Frame(Vector(1.0, 1.0, 0.0))" you ask for a final position

of [1 1 0] but _no_ final orientation change with respect to the start

orientation.

- the pseudo-inverse solver gives you an approximated solution (an exact

one does not exist for the given chain and joint angles) that weighs

rotational and positional errors equally... (I do find the two pi/2

angles weird for your chain...)

Hope this helps...

Best regards,

Herman

## Re: KDL & inverse kinematic

bruynincwrote:- when assigning the joint angles 0 and pi/2, you not only move the end _position_ to [1 1 0] but you are also changing the end frame _orientation_.

- with "F_dest = Frame(Vector(1.0, 1.0, 0.0))" you ask for a final position of [1 1 0] but _no_ final orientation change with respect to the start orientation.

So the question remains, what am I doing wrong?

Thank,

darki