KDL - Unexpected Forward Kinematics result

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

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

spencerjackson wrote:
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; 87 [ -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 11:11:10AM -0000, miguel [dot] prada [dot] sarasola [..] ... wrote:
>

spencerjackson wrote:
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

>
> 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:
> >

spencerjackson wrote:
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

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

Ruben Smits's picture

(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:

>

peter wrote:
On Wed, Jan 9, 2013 at 9:43 AM, wrote:
>
> > 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