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;
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?

Thanks, darki

‹ Orocos Real-Time Tookit v1.10.0 released Is RTT::Command 'fire-and-forget' ? ›
»

KDL & inverse kinematic

Submitted by bruyninc on Sun, 2009-09-20 10:50.

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

Submitted by darki on Tue, 2009-09-22 16:42.

bruyninc wrote:
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.

OK. Now, I have tried to define the orientation explicitly. The position can be achieved in two ways: [pi/2, -pi/2] and [0, pi/2]. So the orientation of the end frame is either pi or 0 in relation to the Z-axis. 
Frame F_dest = Frame(Rotation::RPY(M_PI/2, 0, 0), Vector(1, 1, 0.0));
Returns -3. 
Frame F_dest = Frame(Rotation::RPY(0, 0, 0), Vector(1, 1, 0.0));
Returns the same incorrect values: [pi/2, pi/2]

Quote:
- 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...)
I understand that the pseudo-inverse solver gives only an approximate solution. But the solution [pi/2, pi/2] is very far from [pi/2, -pi/2] and [0, pi/2].

So the question remains, what am I doing wrong?

Thank,

darki

»