# KDL - Unexpected Forward Kinematics result

Submitted by spencerjackson on Thu, 2012-03-22 02:34 |

Hi! I'm doing some experimentation with KDL, and I'm not getting the

results that I'm expecting. My final arm is going to have 4DOF, but as a

prototype, I knocked something up with only 3 joints, looking like this:

KDL::Chain chain;

chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

chain.addSegment(Segment(Joint(Joint::None),

Frame(Vector(0.1,0.0,0.0))));

chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

chain.addSegment(Segment(Joint(Joint::None),

Frame(Vector(0.1,0.0,0.0))));

chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

After having some issues with the IK, I realized I could test the

forward kinematics. I decided to move the end effector to the tip of the

first segment, canceling out the lengths of the last two joints. To do

this, I rotated the last segment by 180 degrees. That came out like

this:

[sajack@khezef test2]$ ./kdltest

Enter the position of joint 0: 0

Enter the position of joint 1: 0

Enter the position of joint 2: 3.14159265

[[ 1, 0, 0;

0, -1, 8.74228e-08;

0,-8.74228e-08, -1]

[ 0.2, 8.74228e-09, 0.25]]

Why is the y component nonzero?

Thanks!

Spencer Jackson

- Login to post comments

## KDL - Unexpected Forward Kinematics result

On Thu, 22 Mar 2012, Spencer Jackson wrote:

> Hi! I'm doing some experimentation with KDL, and I'm not getting the

> results that I'm expecting. My final arm is going to have 4DOF, but as a

> prototype, I knocked something up with only 3 joints, looking like this:

>

> KDL::Chain chain;

> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> chain.addSegment(Segment(Joint(Joint::None),

> Frame(Vector(0.1,0.0,0.0))));

> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> chain.addSegment(Segment(Joint(Joint::None),

> Frame(Vector(0.1,0.0,0.0))));

> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

>

> After having some issues with the IK, I realized I could test the

> forward kinematics. I decided to move the end effector to the tip of the

> first segment, canceling out the lengths of the last two joints. To do

> this, I rotated the last segment by 180 degrees. That came out like

> this:

>

> [sajack@khezef test2]$ ./kdltest

> Enter the position of joint 0: 0

> Enter the position of joint 1: 0

> Enter the position of joint 2: 3.14159265

> [[ 1, 0, 0;

> 0, -1, 8.74228e-08;

> 0,-8.74228e-08, -1]

> [ 0.2, 8.74228e-09, 0.25]]

>

> Why is the y component nonzero?

Are you referring to the numbers like "8.74228e-09"? That's probably

because your value of Pi is not 100% exact...

> Thanks!

>

> Spencer Jackson

Herman

## KDL - Unexpected Forward Kinematics result

On Thu, Mar 22, 2012 at 05:59:53AM +0100, Herman Bruyninckx wrote:

> On Thu, 22 Mar 2012, Spencer Jackson wrote:

>

> >Hi! I'm doing some experimentation with KDL, and I'm not getting the

> >results that I'm expecting. My final arm is going to have 4DOF, but as a

> >prototype, I knocked something up with only 3 joints, looking like this:

> >

> > KDL::Chain chain;

> > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > chain.addSegment(Segment(Joint(Joint::None),

> >Frame(Vector(0.1,0.0,0.0))));

> > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > chain.addSegment(Segment(Joint(Joint::None),

> >Frame(Vector(0.1,0.0,0.0))));

> > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> >

> >After having some issues with the IK, I realized I could test the

> >forward kinematics. I decided to move the end effector to the tip of the

> >first segment, canceling out the lengths of the last two joints. To do

> >this, I rotated the last segment by 180 degrees. That came out like

> >this:

> >

> >[sajack@khezef test2]$ ./kdltest

> >Enter the position of joint 0: 0

> >Enter the position of joint 1: 0

> >Enter the position of joint 2: 3.14159265

> >[[ 1, 0, 0;

> > 0, -1, 8.74228e-08;

> > 0,-8.74228e-08, -1]

> >[ 0.2, 8.74228e-09, 0.25]]

> >

> >Why is the y component nonzero?

>

> Are you referring to the numbers like "8.74228e-09"? That's probably

> because your value of Pi is not 100% exact...

>

> >Thanks!

> >

> >Spencer Jackson

>

> Herman

Hm. That's unfortunate. In which case, it's likely something wrong with

my inverse kinematics stuff. I'm likely misunderstanding something

trivial. Because my final arm will only have 4DOF, I was trying to

discard the extra rotational dimensions. I tried creating an arm with a

single segment Z axis joint, and realized I couldn't simply use

the Identity rotational matrix. That arm looks like this:

KDL::Chain chain;

chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(1, 0.0,

0.0))));

FK works just about as well as the previous example:

[sajack@khezef test2]$ ./kdltest

Enter the position of joint 0: 3.14159265

[[ -1, 8.74228e-08, 0;

-8.74228e-08, -1, 0;

0, 0, 1]

[ -1,-8.74228e-08, 0.25]]

However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using

the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've

set the taskspace matrix like so:

Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);

matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;

iksolver1v.setWeightTS( matrix_Mx )

Additionally, I'm using the Weighted Error IK position solver, from

here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html

ChainIkSolverPos_NR_WE

iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations,

stop at accuracy 1e-6

const double weights[6] = {1, 1, 1, 0, 0, 0};

iksolver1.setWeights(weights);

The Frame I'm navigating to is:

Frame F_dest = Frame(Vector(-1, 0, .25));

I feel as if this is a great deal more simple than the arm I'm going to

be working with later, and would like to know what it is that I'm doing

wrong with it. I'm not really sure what I'm missing... Thanks!

Spencer Jackson

## KDL - Unexpected Forward Kinematics result

Hi Spencer,

In order to have the inverse kinematic compute in function of the

position only, i modified one of the existing solver (i.e. copy paste of

chainiksolverpos_nr.cpp/.hpp).

Mainly, the condition to see whether the IK converges is checked only on

positions, as shown below.

Cheers, Gianni.

------------------------

int ChainIkSolverPos_NR_Pos::CartToJnt(const JntArray& q_init, const

Frame& p_in, JntArray& q_out)

{

// this solver computes the inverse kinematic for the position

only.

q_out = q_init;

unsigned int i;

for(i=0;i<maxiter;i++){

fksolver.JntToCart(q_out,f);

delta_vel = f.p-p_in.p;

//Before was:delta_twist = diff(f,p_in);

iksolver.CartToJnt(q_out,Twist(delta_vel,Vector::Zero()),delta_q);

Add(q_out,delta_q,q_out);

if(Equal(delta_vel,Vector::Zero(),eps))

break;

//before was:

//if(Equal(delta_twist,Twist::Zero(),eps))

// break;

}

if(i!=maxiter)

return 0;

else

return -3;

}

------------------------

Il giorno gio, 22/03/2012 alle 18.35 -0400, Spencer Jackson ha scritto:

> On Thu, Mar 22, 2012 at 05:59:53AM +0100, Herman Bruyninckx wrote:

> > On Thu, 22 Mar 2012, Spencer Jackson wrote:

> >

> > >Hi! I'm doing some experimentation with KDL, and I'm not getting the

> > >results that I'm expecting. My final arm is going to have 4DOF, but as a

> > >prototype, I knocked something up with only 3 joints, looking like this:

> > >

> > > KDL::Chain chain;

> > > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > > chain.addSegment(Segment(Joint(Joint::None),

> > >Frame(Vector(0.1,0.0,0.0))));

> > > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > chain.addSegment(Segment(Joint(Joint::None),

> > >Frame(Vector(0.1,0.0,0.0))));

> > > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > >

> > >After having some issues with the IK, I realized I could test the

> > >forward kinematics. I decided to move the end effector to the tip of the

> > >first segment, canceling out the lengths of the last two joints. To do

> > >this, I rotated the last segment by 180 degrees. That came out like

> > >this:

> > >

> > >[sajack@khezef test2]$ ./kdltest

> > >Enter the position of joint 0: 0

> > >Enter the position of joint 1: 0

> > >Enter the position of joint 2: 3.14159265

> > >[[ 1, 0, 0;

> > > 0, -1, 8.74228e-08;

> > > 0,-8.74228e-08, -1]

> > >[ 0.2, 8.74228e-09, 0.25]]

> > >

> > >Why is the y component nonzero?

> >

> > Are you referring to the numbers like "8.74228e-09"? That's probably

> > because your value of Pi is not 100% exact...

> >

> > >Thanks!

> > >

> > >Spencer Jackson

> >

> > Herman

>

> Hm. That's unfortunate. In which case, it's likely something wrong with

> my inverse kinematics stuff. I'm likely misunderstanding something

> trivial. Because my final arm will only have 4DOF, I was trying to

> discard the extra rotational dimensions. I tried creating an arm with a

> single segment Z axis joint, and realized I couldn't simply use

> the Identity rotational matrix. That arm looks like this:

>

> KDL::Chain chain;

> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(1, 0.0,

> 0.0))));

>

> FK works just about as well as the previous example:

> [sajack@khezef test2]$ ./kdltest

> Enter the position of joint 0: 3.14159265

> [[ -1, 8.74228e-08, 0;

> -8.74228e-08, -1, 0;

> 0, 0, 1]

> [ -1,-8.74228e-08, 0.25]]

>

> However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using

> the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've

> set the taskspace matrix like so:

>

> Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);

> matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;

> iksolver1v.setWeightTS( matrix_Mx )

>

> Additionally, I'm using the Weighted Error IK position solver, from

> here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html

>

> ChainIkSolverPos_NR_WE

> iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations,

> stop at accuracy 1e-6

> const double weights[6] = {1, 1, 1, 0, 0, 0};

> iksolver1.setWeights(weights);

>

> The Frame I'm navigating to is:

>

> Frame F_dest = Frame(Vector(-1, 0, .25));

>

> I feel as if this is a great deal more simple than the arm I'm going to

> be working with later, and would like to know what it is that I'm doing

> wrong with it. I'm not really sure what I'm missing... Thanks!

>

>

> Spencer Jackson

## KDL - Unexpected Forward Kinematics result

Hi Gianni, this doesn't seem to work... I started with the

ChainIkSolverPos_NR, made your suggested modifications, and still am

unable to find the IK solution. Using the simple 1 joint arm, I tried

stepping through with GDB. q_out is initially zero, the IK solver sets

delta_q to zero. Through every iteration of the loop, q_out remains

unchanged at zero. I tried this with both the ChainIkSolverVel_pinv and

ChainIkSolverVel_wdls solvers. Advice? Thanks!

Spencer

On Fri, Mar 23, 2012 at 01:40:18PM +0100, gianni borghesan wrote:

> Hi Spencer,

> In order to have the inverse kinematic compute in function of the

> position only, i modified one of the existing solver (i.e. copy paste of

> chainiksolverpos_nr.cpp/.hpp).

> Mainly, the condition to see whether the IK converges is checked only on

> positions, as shown below.

> Cheers, Gianni.

>

> ------------------------

> int ChainIkSolverPos_NR_Pos::CartToJnt(const JntArray& q_init, const

> Frame& p_in, JntArray& q_out)

> {

> // this solver computes the inverse kinematic for the position

> only.

> q_out = q_init;

>

> unsigned int i;

> for(i=0;i<maxiter;i++){

> fksolver.JntToCart(q_out,f);

>

> delta_vel = f.p-p_in.p;

> //Before was:delta_twist = diff(f,p_in);

>

> iksolver.CartToJnt(q_out,Twist(delta_vel,Vector::Zero()),delta_q);

> Add(q_out,delta_q,q_out);

> if(Equal(delta_vel,Vector::Zero(),eps))

> break;

> //before was:

> //if(Equal(delta_twist,Twist::Zero(),eps))

> // break;

> }

> if(i!=maxiter)

> return 0;

> else

> return -3;

> }

> ------------------------

>

>

>

>

> Il giorno gio, 22/03/2012 alle 18.35 -0400, Spencer Jackson ha scritto:

> > On Thu, Mar 22, 2012 at 05:59:53AM +0100, Herman Bruyninckx wrote:

> > > On Thu, 22 Mar 2012, Spencer Jackson wrote:

> > >

> > > >Hi! I'm doing some experimentation with KDL, and I'm not getting the

> > > >results that I'm expecting. My final arm is going to have 4DOF, but as a

> > > >prototype, I knocked something up with only 3 joints, looking like this:

> > > >

> > > > KDL::Chain chain;

> > > > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > > > chain.addSegment(Segment(Joint(Joint::None),

> > > >Frame(Vector(0.1,0.0,0.0))));

> > > > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > > chain.addSegment(Segment(Joint(Joint::None),

> > > >Frame(Vector(0.1,0.0,0.0))));

> > > > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > >

> > > >After having some issues with the IK, I realized I could test the

> > > >forward kinematics. I decided to move the end effector to the tip of the

> > > >first segment, canceling out the lengths of the last two joints. To do

> > > >this, I rotated the last segment by 180 degrees. That came out like

> > > >this:

> > > >

> > > >[sajack@khezef test2]$ ./kdltest

> > > >Enter the position of joint 0: 0

> > > >Enter the position of joint 1: 0

> > > >Enter the position of joint 2: 3.14159265

> > > >[[ 1, 0, 0;

> > > > 0, -1, 8.74228e-08;

> > > > 0,-8.74228e-08, -1]

> > > >[ 0.2, 8.74228e-09, 0.25]]

> > > >

> > > >Why is the y component nonzero?

> > >

> > > Are you referring to the numbers like "8.74228e-09"? That's probably

> > > because your value of Pi is not 100% exact...

> > >

> > > >Thanks!

> > > >

> > > >Spencer Jackson

> > >

> > > Herman

> >

> > Hm. That's unfortunate. In which case, it's likely something wrong with

> > my inverse kinematics stuff. I'm likely misunderstanding something

> > trivial. Because my final arm will only have 4DOF, I was trying to

> > discard the extra rotational dimensions. I tried creating an arm with a

> > single segment Z axis joint, and realized I couldn't simply use

> > the Identity rotational matrix. That arm looks like this:

> >

> > KDL::Chain chain;

> > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(1, 0.0,

> > 0.0))));

> >

> > FK works just about as well as the previous example:

> > [sajack@khezef test2]$ ./kdltest

> > Enter the position of joint 0: 3.14159265

> > [[ -1, 8.74228e-08, 0;

> > -8.74228e-08, -1, 0;

> > 0, 0, 1]

> > [ -1,-8.74228e-08, 0.25]]

> >

> > However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using

> > the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've

> > set the taskspace matrix like so:

> >

> > Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);

> > matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;

> > iksolver1v.setWeightTS( matrix_Mx )

> >

> > Additionally, I'm using the Weighted Error IK position solver, from

> > here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html

> >

> > ChainIkSolverPos_NR_WE

> > iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations,

> > stop at accuracy 1e-6

> > const double weights[6] = {1, 1, 1, 0, 0, 0};

> > iksolver1.setWeights(weights);

> >

> > The Frame I'm navigating to is:

> >

> > Frame F_dest = Frame(Vector(-1, 0, .25));

> >

> > I feel as if this is a great deal more simple than the arm I'm going to

> > be working with later, and would like to know what it is that I'm doing

> > wrong with it. I'm not really sure what I'm missing... Thanks!

> >

> >

> > Spencer Jackson

>

## R: KDL - Unexpected Forward Kinematics result

Hi Spencer,

It cannot work with a single degree of freedom...

You must try with a 3 degree of freedom that is able to actually reach the desired position.

if the delta_q and delta_p is zero, probably, the J matrix is defective along the direction that you want to span.

if you want to inverse kinematic a single joint, then you must check for 1 direction only (the right one!).

if in

iksolver.CartToJnt(q_out,Twist(delta_vel,Vector::Zero()),delta_q);

the twist is zero, with a delta_q different from zero, it means that you have a problem with the jacoban ...

Try with 3dof robot cartesian robot (prismatic joint in x,y,z,), in that way the jacobian will be [eye(3,3); zero(3,3)]. it should work.

________________________________________

Inizio: orocos-users-bounces [..] ... [orocos-users-bounces [..] ...] per conto di Spencer Jackson [spencerandrewjackson [..] ...]

Inviato: venerdì 23 marzo 2012 21.14

Fine: orocos-users [..] ...

Oggetto: Re: [Orocos-users] KDL - Unexpected Forward Kinematics result

Hi Gianni, this doesn't seem to work... I started with the

ChainIkSolverPos_NR, made your suggested modifications, and still am

unable to find the IK solution. Using the simple 1 joint arm, I tried

stepping through with GDB. q_out is initially zero, the IK solver sets

delta_q to zero. Through every iteration of the loop, q_out remains

unchanged at zero. I tried this with both the ChainIkSolverVel_pinv and

ChainIkSolverVel_wdls solvers. Advice? Thanks!

Spencer

On Fri, Mar 23, 2012 at 01:40:18PM +0100, gianni borghesan wrote:

> Hi Spencer,

> In order to have the inverse kinematic compute in function of the

> position only, i modified one of the existing solver (i.e. copy paste of

> chainiksolverpos_nr.cpp/.hpp).

> Mainly, the condition to see whether the IK converges is checked only on

> positions, as shown below.

> Cheers, Gianni.

>

> ------------------------

> int ChainIkSolverPos_NR_Pos::CartToJnt(const JntArray& q_init, const

> Frame& p_in, JntArray& q_out)

> {

> // this solver computes the inverse kinematic for the position

> only.

> q_out = q_init;

>

> unsigned int i;

> for(i=0;i<maxiter;i++){

> fksolver.JntToCart(q_out,f);

>

> delta_vel = f.p-p_in.p;

> //Before was:delta_twist = diff(f,p_in);

>

> iksolver.CartToJnt(q_out,Twist(delta_vel,Vector::Zero()),delta_q);

> Add(q_out,delta_q,q_out);

> if(Equal(delta_vel,Vector::Zero(),eps))

> break;

> //before was:

> //if(Equal(delta_twist,Twist::Zero(),eps))

> // break;

> }

> if(i!=maxiter)

> return 0;

> else

> return -3;

> }

> ------------------------

>

>

>

>

> Il giorno gio, 22/03/2012 alle 18.35 -0400, Spencer Jackson ha scritto:

> > On Thu, Mar 22, 2012 at 05:59:53AM +0100, Herman Bruyninckx wrote:

> > > On Thu, 22 Mar 2012, Spencer Jackson wrote:

> > >

> > > >Hi! I'm doing some experimentation with KDL, and I'm not getting the

> > > >results that I'm expecting. My final arm is going to have 4DOF, but as a

> > > >prototype, I knocked something up with only 3 joints, looking like this:

> > > >

> > > > KDL::Chain chain;

> > > > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > > > chain.addSegment(Segment(Joint(Joint::None),

> > > >Frame(Vector(0.1,0.0,0.0))));

> > > > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > > chain.addSegment(Segment(Joint(Joint::None),

> > > >Frame(Vector(0.1,0.0,0.0))));

> > > > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > >

> > > >After having some issues with the IK, I realized I could test the

> > > >forward kinematics. I decided to move the end effector to the tip of the

> > > >first segment, canceling out the lengths of the last two joints. To do

> > > >this, I rotated the last segment by 180 degrees. That came out like

> > > >this:

> > > >

> > > >[sajack@khezef test2]$ ./kdltest

> > > >Enter the position of joint 0: 0

> > > >Enter the position of joint 1: 0

> > > >Enter the position of joint 2: 3.14159265

> > > >[[ 1, 0, 0;

> > > > 0, -1, 8.74228e-08;

> > > > 0,-8.74228e-08, -1]

> > > >[ 0.2, 8.74228e-09, 0.25]]

> > > >

> > > >Why is the y component nonzero?

> > >

> > > Are you referring to the numbers like "8.74228e-09"? That's probably

> > > because your value of Pi is not 100% exact...

> > >

> > > >Thanks!

> > > >

> > > >Spencer Jackson

> > >

> > > Herman

> >

> > Hm. That's unfortunate. In which case, it's likely something wrong with

> > my inverse kinematics stuff. I'm likely misunderstanding something

> > trivial. Because my final arm will only have 4DOF, I was trying to

> > discard the extra rotational dimensions. I tried creating an arm with a

> > single segment Z axis joint, and realized I couldn't simply use

> > the Identity rotational matrix. That arm looks like this:

> >

> > KDL::Chain chain;

> > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(1, 0.0,

> > 0.0))));

> >

> > FK works just about as well as the previous example:

> > [sajack@khezef test2]$ ./kdltest

> > Enter the position of joint 0: 3.14159265

> > [[ -1, 8.74228e-08, 0;

> > -8.74228e-08, -1, 0;

> > 0, 0, 1]

> > [ -1,-8.74228e-08, 0.25]]

> >

> > However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using

> > the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've

> > set the taskspace matrix like so:

> >

> > Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);

> > matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;

> > iksolver1v.setWeightTS( matrix_Mx )

> >

> > Additionally, I'm using the Weighted Error IK position solver, from

> > here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html

> >

> > ChainIkSolverPos_NR_WE

> > iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations,

> > stop at accuracy 1e-6

> > const double weights[6] = {1, 1, 1, 0, 0, 0};

> > iksolver1.setWeights(weights);

> >

> > The Frame I'm navigating to is:

> >

> > Frame F_dest = Frame(Vector(-1, 0, .25));

> >

> > I feel as if this is a great deal more simple than the arm I'm going to

> > be working with later, and would like to know what it is that I'm doing

> > wrong with it. I'm not really sure what I'm missing... Thanks!

> >

> >

> > Spencer Jackson

>

> --

> Gianni Borghesan, Ph.D.

>

> Dep. of Mechanical Engineering, K.U. Leuven

> Celestijnenlaan 300 - bus 2419, 3001 Heverlee, Belgium

> Room: 01.017

> Tel: +32-16-3-22515

> Fax: +32-16-3-22987

>

>

--

Orocos-Users mailing list

Orocos-Users [..] ...

http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

## Re: KDL - Unexpected Forward Kinematics result

spencerjacksonwrote:Hm. That's unfortunate. In which case, it's likely something wrong with my inverse kinematics stuff. I'm likely misunderstanding something trivial. Because my final arm will only have 4DOF, I was trying to discard the extra rotational dimensions. I tried creating an arm with a single segment Z axis joint, and realized I couldn't simply use the Identity rotational matrix. That arm looks like this:

0.0))));FK works just about as well as the previous example: [sajack@khezef test2]$ ./kdltest Enter the position of joint 0: 3.14159265 -1, 8.74228e-08, 0; 1 [ -1,-8.74228e-08, 0.25

However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've set the taskspace matrix like so:

Additionally, I'm using the Weighted Error IK position solver, from here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6 The Frame I'm navigating to is: I feel as if this is a great deal more simple than the arm I'm going to be working with later, and would like to know what it is that I'm doing wrong with it. I'm not really sure what I'm missing... Thanks!Spencer Jackson

Hi Spencer,

You cannot use the identity matrix to ignore rotations like that. If you put the identity matrix you are in fact requesting that the orientation of the end effector is the same as the base frame.

I don't think there's an easy solution for this when using the KDL IK algorithms, since all of them assume you are setting a full position and orientation target pose.

One thing that you could do is add a three additional segments with no length and with the configuration of a spherical joint (e.g. Z-Y-Z) in the end of your chain, and then ignore the part of the solution that involves those degrees of freedom. This way no matter the orientation you ask on the IK request, it will be obtainable with those three last DOF's, and you will be effectively calculating a position only IK for your 4 DOF chain.

If there's a more elegant solution to this I hope someone steps up and corrects me :)

Best regards, Miguel.

## (no subject)

On Fri, Mar 23, 2012 at 11:11:10AM -0000, miguel [dot] prada [dot] sarasola [..] ... wrote:

>

spencerjacksonwrote:> Bruyninckx wrote:

> > On Thu, 22 Mar 2012, Spencer Jackson wrote:

> >

> > >Hi! I'm doing some experimentation with KDL, and I'm not getting the

> > >results that I'm expecting. My final arm is going to have 4DOF, but as a

> > >prototype, I knocked something up with only 3 joints, looking like this:

> > >

> > > KDL::Chain chain;

> > >

> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > > chain.addSegment(Segment(Joint(Joint::None),

> > >Frame(Vector(0.1,0.0,0.0))));

> > >

> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > chain.addSegment(Segment(Joint(Joint::None),

> > >Frame(Vector(0.1,0.0,0.0))));

> > >

> chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > >

> > >After having some issues with the IK, I realized I could test the

> > >forward kinematics. I decided to move the end effector to the tip of the

> > >first segment, canceling out the lengths of the last two joints. To do

> > >this, I rotated the last segment by 180 degrees. That came out like

> > >this:

> > >

> > >[sajack@khezef test2]$ ./kdltest

> > >Enter the position of joint 0: 0

> > >Enter the position of joint 1: 0

> > >Enter the position of joint 2: 3.14159265

> > >[[ 1, 0, 0;

> > > 0, -1, 8.74228e-08;

> > > 0,-8.74228e-08, -1]

> > >[ 0.2, 8.74228e-09, 0.25]]

> > >

> > >Why is the y component nonzero?

> >

> > Are you referring to the numbers like "8.74228e-09"? That's probably

> > because your value of Pi is not 100% exact...

> >

> > >Thanks!

> > >

> > >Spencer Jackson

> >

> > Herman

>

> Hm. That's unfortunate. In which case, it's likely something wrong with

> my inverse kinematics stuff. I'm likely misunderstanding something

> trivial. Because my final arm will only have 4DOF, I was trying to

> discard the extra rotational dimensions. I tried creating an arm with a

> single segment Z axis joint, and realized I couldn't simply use

> the Identity rotational matrix. That arm looks like this:

>

> KDL::Chain chain;

>

> chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(1, 0.0,

> 0.0))));

>

> FK works just about as well as the previous example:

> [sajack@khezef test2]$ ./kdltest

> Enter the position of joint 0: 3.14159265

> [[ -1, 8.74228e-08, 0;

> -8.74228e-08, -1, 0;

> 0, 0, 1]

> [ -1,-8.74228e-08, 0.25]]

>

> However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using

> the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've

> set the taskspace matrix like so:

>

> Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);

> matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;

> iksolver1v.setWeightTS( matrix_Mx )

>

> Additionally, I'm using the Weighted Error IK position solver, from

> here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html

>

> ChainIkSolverPos_NR_WE

> iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations,

> stop at accuracy 1e-6

> const double weights[6] = {1, 1, 1, 0, 0, 0};

> iksolver1.setWeights(weights);

>

> The Frame I'm navigating to is:

>

> Frame F_dest = Frame(Vector(-1, 0, .25));

>

> I feel as if this is a great deal more simple than the arm I'm going to

> be working with later, and would like to know what it is that I'm doing

> wrong with it. I'm not really sure what I'm missing... Thanks!

>

>

> Spencer Jackson

>

> Hi Spencer,

>

> You cannot use the identity matrix to ignore rotations like that. If you put

> the identity matrix you are in fact requesting that the orientation of the

> end effector is the same as the base frame.

>

> I don't think there's an easy solution for this when using the KDL IK

> algorithms, since all of them assume you are setting a full position and

> orientation target pose.

>

> One thing that you could do is add a three additional segments with no length

> and with the configuration of a spherical joint (e.g. Z-Y-Z) in the end of

> your chain, and then ignore the part of the solution that involves those

> degrees of freedom. This way no matter the orientation you ask on the IK

> request, it will be obtainable with those three last DOF's, and you will be

> effectively calculating a position only IK for your 4 DOF chain.

>

> If there's a more elegant solution to this I hope someone steps up and

> corrects me :)

>

> Best regards,

> Miguel.

## (no subject)

On Fri, Mar 23, 2012 at 04:17:32PM -0400, Spencer Jackson wrote:

> On Fri, Mar 23, 2012 at 11:11:10AM -0000, miguel [dot] prada [dot] sarasola [..] ... wrote:

> >

spencerjacksonwrote:> > Bruyninckx wrote:

> > > On Thu, 22 Mar 2012, Spencer Jackson wrote:

> > >

> > > >Hi! I'm doing some experimentation with KDL, and I'm not getting the

> > > >results that I'm expecting. My final arm is going to have 4DOF, but as a

> > > >prototype, I knocked something up with only 3 joints, looking like this:

> > > >

> > > > KDL::Chain chain;

> > > >

> > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > > > chain.addSegment(Segment(Joint(Joint::None),

> > > >Frame(Vector(0.1,0.0,0.0))));

> > > >

> > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > > chain.addSegment(Segment(Joint(Joint::None),

> > > >Frame(Vector(0.1,0.0,0.0))));

> > > >

> > chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.10))));

> > > >

> > > >After having some issues with the IK, I realized I could test the

> > > >forward kinematics. I decided to move the end effector to the tip of the

> > > >first segment, canceling out the lengths of the last two joints. To do

> > > >this, I rotated the last segment by 180 degrees. That came out like

> > > >this:

> > > >

> > > >[sajack@khezef test2]$ ./kdltest

> > > >Enter the position of joint 0: 0

> > > >Enter the position of joint 1: 0

> > > >Enter the position of joint 2: 3.14159265

> > > >[[ 1, 0, 0;

> > > > 0, -1, 8.74228e-08;

> > > > 0,-8.74228e-08, -1]

> > > >[ 0.2, 8.74228e-09, 0.25]]

> > > >

> > > >Why is the y component nonzero?

> > >

> > > Are you referring to the numbers like "8.74228e-09"? That's probably

> > > because your value of Pi is not 100% exact...

> > >

> > > >Thanks!

> > > >

> > > >Spencer Jackson

> > >

> > > Herman

> >

> > Hm. That's unfortunate. In which case, it's likely something wrong with

> > my inverse kinematics stuff. I'm likely misunderstanding something

> > trivial. Because my final arm will only have 4DOF, I was trying to

> > discard the extra rotational dimensions. I tried creating an arm with a

> > single segment Z axis joint, and realized I couldn't simply use

> > the Identity rotational matrix. That arm looks like this:

> >

> > KDL::Chain chain;

> >

> > chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,0.25))));

> > chain.addSegment(Segment(Joint(Joint::None),Frame(Vector(1, 0.0,

> > 0.0))));

> >

> > FK works just about as well as the previous example:

> > [sajack@khezef test2]$ ./kdltest

> > Enter the position of joint 0: 3.14159265

> > [[ -1, 8.74228e-08, 0;

> > -8.74228e-08, -1, 0;

> > 0, 0, 1]

> > [ -1,-8.74228e-08, 0.25]]

> >

> > However, trying to achieve IK for (-1, 0, 0.25) doesn't work. I'm using

> > the Identity Matrix, but with the ChainIkSolverVel_wdls solver. I've

> > set the taskspace matrix like so:

> >

> > Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);

> > matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;

> > iksolver1v.setWeightTS( matrix_Mx )

> >

> > Additionally, I'm using the Weighted Error IK position solver, from

> > here: http://ros-users.122217.n3.nabble.com/IK-with-KDL-td1867716.html

> >

> > ChainIkSolverPos_NR_WE

> > iksolver1(chain,fksolver,iksolver1v,100,1e-6);//Maximum 100 iterations,

> > stop at accuracy 1e-6

> > const double weights[6] = {1, 1, 1, 0, 0, 0};

> > iksolver1.setWeights(weights);

> >

> > The Frame I'm navigating to is:

> >

> > Frame F_dest = Frame(Vector(-1, 0, .25));

> >

> > I feel as if this is a great deal more simple than the arm I'm going to

> > be working with later, and would like to know what it is that I'm doing

> > wrong with it. I'm not really sure what I'm missing... Thanks!

> >

> >

> > Spencer Jackson

> >

> > Hi Spencer,

> >

> > You cannot use the identity matrix to ignore rotations like that. If you put

> > the identity matrix you are in fact requesting that the orientation of the

> > end effector is the same as the base frame.

> >

> > I don't think there's an easy solution for this when using the KDL IK

> > algorithms, since all of them assume you are setting a full position and

> > orientation target pose.

> >

> > One thing that you could do is add a three additional segments with no length

> > and with the configuration of a spherical joint (e.g. Z-Y-Z) in the end of

> > your chain, and then ignore the part of the solution that involves those

> > degrees of freedom. This way no matter the orientation you ask on the IK

> > request, it will be obtainable with those three last DOF's, and you will be

> > effectively calculating a position only IK for your 4 DOF chain.

> >

> > If there's a more elegant solution to this I hope someone steps up and

> > corrects me :)

> >

> > Best regards,

> > Miguel.

> > --

> > Orocos-Users mailing list

> > Orocos-Users [..] ...

> > http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

>

> Hi Miguel,

>

> That worked with my complex arm model! Is there any way to use this to

> specify only the last degree of freedom in the orientation? Thanks!

>

> Spencer

## (no subject)

On Fri, Dec 21, 2012 at 3:05 PM, <t [dot] t [dot] g [dot] clephas [..] ...> wrote:

> > Debian packages for Orocos Toolchain 2.6 can be fetched using apt-get

> > from the ros debian package repository: apt-get install

> > ros-fuerte-orocos-toolchain ros-fuerte-rtt-ros-integration

> > ros-fuerte-rtt-ros-comm ros-fuerte-rtt-common-msgs

> >

> > You'll still have to build rtt_geometry and soem from source though

> > but it will take most of the compilation time away.

> rtt_geometry is set to "intentionally missing" on

> http://www.ros.org/debbuild/fuerte.html

> Why is that?

No good reason, I issued a prerelease build, if it succeeds I'll

release rtt_geometry 0.3.0 into Fuerte

>

> Tim

> >

> > Ruben

> > Ruben Smits, Phd

> > Chief Technology Officer

> > Intermodalics BVBA

> > +32479511786

> > www.intermodalics.eu

> >

> >

> > On Thu, Dec 20, 2012 at 9:12 PM, Uwe Fechner wrote:

> >> Hello,

> >>

> >> the installation instructions on:

> >> http://www.ros.org/wiki/orocos_toolchain_ros

> >> section "Manual installation" do not work for Orocos 2.6.

> >>

> >> I updated my own installation instructions:

> >> http://www.kieltech.de/uweswiki/ROS

> >>

> >> but this is ridiculously complicated.

> >>

> >> Is there a way to simplify the installation?

> >>

> >> All we want is Orocos + Ethercat, and installing it takes

> >> hundreds of megabytes and hours of compilation.

> >>

> >> Regards:

> >>

> >> Uwe Fechner

> >> --

> >> Orocos-Users mailing list

> >> Orocos-Users [..] ...

> >> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

> --

> Orocos-Users mailing list

> Orocos-Users [..] ...

> http://lists.mech.kuleuven.be/mailman/listinfo/orocos-users

## (no subject)

On Thu, Jan 10, 2013 at 2:04 PM, <dgerb [..] ...> wrote:

>

peterwrote:>

> > I use Orocos toolchain 2.6 under ubuntu 10.04 with RTAI 3.9. The system

> > hangs

> > when an activity is created and only if the module "rtai_sem" is loaded.

> >

>

> The rtai_sem module must be loaded. It has been many years since I tested

> Orocos against RTAI/LXRT. Solving this will require typical debugging

> skills such as analysing the last lines of 'dmesg', turning on the debug

> features of the RTAI kernel modules, checking if the API of RTAI 3.5 -> 3.9

> didn't change semantics (ie we check for a return value which was different

> in 3.5 or 3.9)

>

> Another thing you should do in RTT is :

> cd build

> cmake .. -DENABLE_TESTS=ON

> make check

>

> and report on which unit tests work and which don't.

>

> Good luck,

> Peter

>

> this is the output of make check :

> [..][100%] Built target typekit_test

> UpdateCTestConfiguration from

>

> :/home/laboratorio/orocos/orocos-toolchain/rtt/build/tests/DartConfiguration.tcl

> Parse Config

>

> file:/home/laboratorio/orocos/orocos-toolchain/rtt/build/tests/DartConfiguration.tcl

> UpdateCTestConfiguration from

> :/home/laboratorio/orocos/orocos-toolchain/rtt/build/DartConfiguration.tcl

> Parse Config

>

> file:/home/laboratorio/orocos/orocos-toolchain/rtt/build/DartConfiguration.tcl

> Test project /home/laboratorio/orocos/orocos-toolchain/rtt/build

> Constructing a list of tests

> Done constructing a list of tests

> Checking test dependency graph...

> test 1

> Start 1: main-test

>

> 1: Test command:

> /home/laboratorio/orocos/orocos-toolchain/rtt/build/tests/main-test

> 1: Test timeout computed to be: 1500

> 1/38 Test #1: main-test ........................***Exception: SegFault

> 0.07 sec

> test 2

> Start 2: list-test

>

> 2: Test command:

> /home/laboratorio/orocos/orocos-toolchain/rtt/build/tests/list-test

> 2: Test timeout computed to be: 1500

> 2: Running 2 test cases...

> 2:

> 2: *** No errors detected

> 2/38 Test #2: list-test ........................***Exception: SegFault

> 0.08 sec

> test 3

> Start 3: core-test

>

> 3: Test command:

> /home/laboratorio/orocos/orocos-toolchain/rtt/build/tests/core-test

> 3: Test timeout computed to be: 1500

> (the system hangs)

Ok. We first need to solve why main-test did SegFault. I see in the trace

that the kernel got a NULL pointer in the rtai_lxrt module, so that

triggered this case.

You can try to build rtt with this extra option:

cd rtt/build

cmake .. -DOROSEM_OS_LXRT_CHECK=ON

make

cd tests

gdb ./main-test

run

... crash ...

bt

In case this does not work, try to run without gdb too.

Peter