On Tuesday 02 September 2008 08:45:58 Davor Jovanoski wrote:
> On Mon, Sep 1, 2008 at 5:33 PM, Ruben Smits
<ruben [dot] smits [..] ...>wrote:
> > On Monday 01 September 2008 17:02:57 Davor Jovanoski wrote:
> > > Hi,
> > >
> > > As I sad, i use the XML for that purpose and that purpose only,
> > > because,
> >
> > i
> >
> > > have entered a couple of FK, that i can think of, and then the result
> >
> > Frame
> >
> > > of the FKs, i used as inverse kinematics input, So it is definitely
> > > reachable, or I have some stupid calculation error, but that is
> > > unlikely
> >
> > Let's look at the desired frame:
> >
> > Frame(Rotation(-0.504846,0.863209,0.000000,
> > -0.863209,-0.504846,0.000000,
> > 0.000000,0.000000,1.000000),
> > Vector(-0.104846,3.863209,0.800000));
> >
> > The rotation is around the Z-axis with an angle of -2.1 radians. The
> > Translation about Z is 0.8.
> >
> > These two values fix two joint values, only the translation around z can
> > be used to solve X and Y position.
> >
> > This translation can only change the X position of the frame, so the Y
> > position can never be solved.
> >
> > So i really think the desired frame is unreachable.
> >
> > Some trial and error gives me the following frame for the jointvalues
> > (0.4,0.8,-2.1)
> >
> > f_sol1 = [[ -0.504846, 0.863209, 0;
> > -0.863209, -0.504846, 0;
> > 0, 0, 1]
> > [ -0.104846, 2.13679, 0.8]]
> >
> > which has the exact same rotation matrix, x-value and z-value as your
> > desired
> > frame, only the y value is different.
> >
> > Unless you can show me the joint values that resulted in your desired
> > frame i
> > will keep thinking that it is unreachable.
> >
> > Ruben
> >
> > well, what i do is this,
>
> 1. i have this chain (see start.png)
>
> JOINTS:
> TransX,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.0
>00000,1.000000],[0.000000,0.000000,0.000000]]
> TransZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.0
>00000,1.000000],[0.000000,3.000000,0.000000]]
> RotZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.000
>000,1.000000],[1.000000,0.000000,0.000000]]
>
>
> 2. i have an FK mover for (0.1,0.1,1.0) (see fk.png and fk2.png )
> 3. frame gained as result of the FK from 2.
> FK:
> [[0.540302,-0.841471,0.000000;0.841471,0.540302,0.000000;0.000000,0.000000,
>1.000000],[0.640302,3.841471,0.100000]] Move: [0.10000000000000001,
> 0.10000000000000001, 1.0]
>
> FK RESULT JOINTS:
> TransX,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.0
>00000,1.000000],[0.100000,0.000000,0.000000]]
> TransZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.0
>00000,1.000000],[0.000000,3.000000,0.100000]]
> RotZ,[[0.540302,-0.841471,0.000000;0.841471,0.540302,0.000000;0.000000,0.00
>0000,1.000000],[0.540302,0.841471,0.000000]]

How did you get this information, because these are Frame values not joint
values. Are these the intermediate frames?

> 4. The same frame used as param for the inverse kinematics, that gives
> (see ik.png ), which is obvious not the right solution, and in frames are
> Desired:
> [[0.540302,0.841471,0.000000;-0.841471,0.540302,0.000000;0.000000,0.000000,
>1.000000],[0.640302,3.841471,0.100000]] FK:
> [[0.996736,0.080725,0.000000;-0.080725,0.996736,0.000000;0.000000,0.000000,
>1.000000],[0.640302,2.919275,0.100000]]
[...]
> 5. So either the FK result Frame is wrong, or the IK solver does something
> wrong....

I think you are using KDL in the wrong way, It looks like in 3. you changed
the chain. But i'm not sure.

This is some python code that does the exact same thing as you are trying to
do :

#!/usr/bin/python

from PyKDL import *
#Create the chain
chain=Chain();
chain.addSegment(Segment(Joint(Joint.TransX)));
chain.addSegment(Segment(Joint(Joint.TransZ),Frame(Vector(0.0,3.0,0.0))));
chain.addSegment(Segment(Joint(Joint.RotZ),Frame(Vector(1.0,0.0,0.0))));
//Create the solvers
fksolverpos = ChainFkSolverPos_recursive(chain);
iksolvervel = ChainIkSolverVel_pinv(chain);
iksolverpos = ChainIkSolverPos_NR(chain,fksolverpos,iksolvervel);
//Set starting joint values
q=JntArray(3);
q[0]=0.1;
q[1]=0.1;
q[2]=1.0;
print "starting joint values: ",q
//Calculate forward position kinematics
f1=Frame();
fksolverpos.JntToCart(q,f1);

print "resulting starting frame: ",f1
//Reset joint values to zero
SetToZero(q)
//Calculate inverse position kinematics
iksolverpos.CartToJnt(q,f1,q);

print "resulting joint values: ",q

The output of this program is:

starting joint values: [0.100000,0.100000,1.000000]
resulting starting frame: [[ 0.540302, -0.841471, 0;
0.841471, 0.540302, 0;
0, 0, 1]
[ 0.640302, 3.84147, 0.1]]
resulting joint values: [0.100000,0.100000,1.000000]

As you can see the resulting joint values are the same as the starting ones,
which means the inverse position kinematics work perfectly.

> but the position cannot be unreachable, beacause i alredy take the same FK
> result frame as my param.. so it is one time reached...

As my code example shows, KDL is able to give you the right solution.

Ruben

On Monday 01 September 2008 16:48:08 Davor Jovanoski wrote:
> ---------- Forwarded message ----------
> From: Davor Jovanoski <djovanoski [..] ...>
> Date: Mon, Sep 1, 2008 at 4:47 PM
> Subject: Re: [Orocos-users] IKsolver
> To: Ruben Smits <ruben [dot] smits [..] ...>
>
> On Mon, Sep 1, 2008 at 4:32 PM, Ruben Smits
<ruben [dot] smits [..] ...>wrote:
> > On Monday 01 September 2008 15:59:48 Davor Jovanoski wrote:
> > > Hi Herman,
> > > 3DOFs ???
> > >
> > > But Yes i am sure that the chain can reach the frame, as stated earlier
> > > I use the result frame from previous FK, so basicly i am making this,
> > > IK(FK(a,b,c)) and the result should be (a,b,c)... but it is not always.
> > > PS. My Joints are TransX TransZ RotZ
> >
> > Can you give some more information:
> >
> > How does your chain look like: Segments, Joints
> > What are the joint values you gave to the FK?
> >
> > Are you sure you're not in a kinematic singularity?
> >
> > Ruben
> >
> > > Davor
> >
> > So here is all then,
>
> I use an XML file for the input&output, and it looks like this, Chain as
> root,
> and each segment has a joint and a frame, the the joint is represented from
> all of the params, and the frame is as shown, each row has 4 cell(s), so
> the 4x4 matrix...
> In this example i use 3 Joints, TransX(0,0,0) TransZ(0,3,0) RotZ(1,0,0),
> desired frame is the frame of tag
>
>
>
>
> TransX
> 1
> 0
> 0
> 0
> 0
>
>
>
> 1.0
> 0.0
> 0.0
> 0.0
>
>
> 0.0
> 1.0
> 0.0
> 0.0
>
>
> 0.0
> 0.0
> 1.0
> 0.0
>
>
> 0.0
> 0.0
> 0.0
> 1.0
>
>
>
>
>
> TransZ
> 1
> 0
> 0
> 0
> 0
>
>
>
> 1.0
> 0.0
> 0.0
> 0.0
>
>
> 0.0
> 1.0
> 0.0
> 0.0
>
>
> 0.0
> 0.0
> 1.0
> 0.0
>
>
> 0.0
> 3.0
> 0.0
> 1.0
>
>
>
>
>
> RotZ
> 1
> 0
> 0
> 0
> 0
>
>
>
> 1.0
> 0.0
> 0.0
> 0.0
>
>
> 0.0
> 1.0
> 0.0
> 0.0
>
>
> 0.0
> 0.0
> 1.0
> 0.0
>
>
> 1.0
> 0.0
> 0.0
> 1.0
>
>
>
>
> 0.1
> 0.1
> 0.1
>
>
>
>
> -0.5048461046
> 0.863209366649
> 0.0
> 0.0
>
>
> -0.863209366649
> -0.5048461046
> 0.0
> 0.0
>
>
> 0.0
> 0.0
> 1.0
> 0.0
>
>
> -0.1048461046
> 3.86320936665
> 0.8
> 1.0
>
>
> 10000
> 1e-16
>
>

Ok, thanks, i builded the same chain in c++ and in python, both are giving me
the same results as you. But are you sure that the desired frame is reachable
for the chain, and by reachable i mean, the exact position AND rotation?

Don't forget KDLs inverse kinematics handles 3D translation and 3D rotation.

Ruben

>
> --
> Jovanoski Davor