Fwd: IKsolver



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

<chain>
    <segment>
        <joint>
            <type>TransX</type>

            <scale>1</scale>
            <offset>0</offset>
            <inertia>0</inertia>
            <damping>0</damping>
            <stiffness>0</stiffness>

        </joint>
        <frame>
            <row>
                <cell>1.0</cell>
                <cell>0.0</cell>
                <cell>0.0</cell>

                <cell>0.0</cell>     
            </row>
            <row>
                <cell>0.0</cell>
                <cell>1.0</cell>
                <cell>0.0</cell>

                <cell>0.0</cell>     
            </row>
            <row>
                <cell>0.0</cell>
                <cell>0.0</cell>
                <cell>1.0</cell>

                <cell>0.0</cell>
            </row>
            <row>
                <cell>0.0</cell>
                <cell>0.0</cell>
                <cell>0.0</cell>

                <cell>1.0</cell>     
            </row>
        </frame>
    </segment>
    <segment>
        <joint>
            <type>TransZ</type>

            <scale>1</scale>
            <offset>0</offset>
            <inertia>0</inertia>
            <damping>0</damping>
            <stiffness>0</stiffness>

        </joint>
        <frame>
            <row>
                <cell>1.0</cell>
                <cell>0.0</cell>
                <cell>0.0</cell>

                <cell>0.0</cell>     
            </row>
            <row>
                <cell>0.0</cell>
                <cell>1.0</cell>
                <cell>0.0</cell>

                <cell>0.0</cell>     
            </row>
            <row>
                <cell>0.0</cell>
                <cell>0.0</cell>
                <cell>1.0</cell>

                <cell>0.0</cell>
            </row>
            <row>
                <cell>0.0</cell>
                <cell>3.0</cell>
                <cell>0.0</cell>

                <cell>1.0</cell>     
            </row>
        </frame>
    </segment>
    <segment>
        <joint>
            <type>RotZ</type>

            <scale>1</scale>
            <offset>0</offset>
            <inertia>0</inertia>
            <damping>0</damping>
            <stiffness>0</stiffness>

        </joint>
        <frame>
            <row>
                <cell>1.0</cell>
                <cell>0.0</cell>
                <cell>0.0</cell>

                <cell>0.0</cell>     
            </row>
            <row>
                <cell>0.0</cell>
                <cell>1.0</cell>
                <cell>0.0</cell>

                <cell>0.0</cell>     
            </row>
            <row>
                <cell>0.0</cell>
                <cell>0.0</cell>
                <cell>1.0</cell>

                <cell>0.0</cell>
            </row>
            <row>
                <cell>1.0</cell>
                <cell>0.0</cell>
                <cell>0.0</cell>

                <cell>1.0</cell>     
            </row>
        </frame>
    </segment>
    <forwardKinematics>
        <move>0.1</move>
        <move>0.1</move>

        <move>0.1</move>
    </forwardKinematics>
    <inverseKinematics>
        <frame>
            <row>
                <cell> -0.5048461046</cell>
                <cell>0.863209366649</cell>

                <cell>0.0</cell>
                <cell>0.0</cell>
            </row>
            <row>
                <cell>-0.863209366649</cell>
                <cell>-0.5048461046</cell>

                <cell>0.0</cell>
                <cell>0.0</cell>
            </row>
            <row>
                <cell>0.0</cell>
                <cell>0.0</cell>

                <cell>1.0</cell>
                <cell>0.0</cell>
            </row>
            <row>
                <cell>-0.1048461046</cell>
                <cell>3.86320936665</cell>

                <cell>0.8</cell>
                <cell>1.0</cell>
            </row>
        </frame>
        <iterations>10000</iterations>
        <epsilon>1e-16</epsilon>

    </inverseKinematics>



--
Jovanoski Davor




--
Jovanoski Davor

Ruben Smits's picture

Fwd: IKsolver

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

Ruben Smits's picture

Fwd: IKsolver

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

Fwd: IKsolver

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


Davor
Ruben Smits's picture

Fwd: IKsolver

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

Fwd: IKsolver



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.000000,1.000000],[0.000000,0.000000,0.000000]]

TransZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.000000,1.000000],[0.000000,3.000000,0.000000]]
RotZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.000000,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.000000,1.000000],[0.100000,0.000000,0.000000]]
TransZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.000000,1.000000],[0.000000,3.000000,0.100000]]

RotZ,[[0.540302,-0.841471,0.000000;0.841471,0.540302,0.000000;0.000000,0.000000,1.000000],[0.540302,0.841471,0.000000]]

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


IK GAINED JOINTS:
TransX,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.000000,1.000000],[-0.356434,0.000000,0.000000]]
TransZ,[[1.000000,0.000000,0.000000;0.000000,1.000000,0.000000;0.000000,0.000000,1.000000],[0.000000,3.000000,0.100000]]

RotZ,[[0.996736,0.080725,0.000000;-0.080725,0.996736,0.000000;0.000000,0.000000,1.000000],[0.996736,-0.080725,0.000000]]

5. So either the FK result Frame is wrong, or the IK solver does something wrong....

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


hope this helps,

Davor

Fwd: IKsolver



On Tue, Sep 2, 2008 at 10:51 AM, Davor Jovanoski <djovanoski [..] ...> wrote:



On Tue, Sep 2, 2008 at 10:15 AM, Ruben Smits <ruben [dot] smits [..] ...> wrote:

On Tuesday 02 September 2008 10:03:39 Davor Jovanoski wrote:

> dammit, the problem was that i was using left handed math,

> so i didnt used frame.M frame.p but took it like 4x4 matirx like

> [rot][0]

> [pos][1]

>

> but instead it was the transpose, dammit,

> [rot][pos]

> [0,0,0,1]

>

> it works superb now, thank you


We are working on a User Manual <http://www.orocos.org/kdl/User_Manual>

The following part will hopefully counter these mistakes:

<http://www.orocos.org/kdl/geometry#toc18>



Thank you for pointing us to this valuable piece of information that we should

put in the User Manual somewhere



Ruben




no thank you for such awesome library, python included...

When finished(far future) i will send you the full app, it will be PyQt PyOgl chain representer from XML files


thank you,
Jovanoski Davor



Is it possible to get more than one solutions with the IK solver??
--
Jovanoski Davor

Ruben Smits's picture

Fwd: IKsolver

On Tuesday 02 September 2008 11:30:01 Davor Jovanoski wrote:
> On Tue, Sep 2, 2008 at 10:51 AM, Davor Jovanoski
<djovanoski [..] ...>wrote:
> > On Tue, Sep 2, 2008 at 10:15 AM, Ruben Smits
> > <ruben [dot] smits [..] ...
> >
> > > wrote:
> >>
> >> On Tuesday 02 September 2008 10:03:39 Davor Jovanoski wrote:
> >> > dammit, the problem was that i was using left handed math,
> >> > so i didnt used frame.M frame.p but took it like 4x4 matirx like
> >> > [rot][0]
> >> > [pos][1]
> >> >
> >> > but instead it was the transpose, dammit,
> >> > [rot][pos]
> >> > [0,0,0,1]
> >> >
> >> > it works superb now, thank you
> >>
> >> We are working on a User Manual
> >> The following part will hopefully counter these mistakes:
> >>
> >>
> >> Thank you for pointing us to this valuable piece of information that we
> >> should
> >> put in the User Manual somewhere
> >>
> >> Ruben
> >
> > no thank you for such awesome library, python included...
> >
> > When finished(far future) i will send you the full app, it will be PyQt
> > PyOgl chain representer from XML files
> >
> > thank you,
> > Jovanoski Davor
>
> Is it possible to get more than one solutions with the IK solver??
The iksolverpos does not search for all solutions but you can get other
solutions by giving other starting values for the joint values, q_init:

iksolverpos.CartToJnt(q_init,frame_in,q_out)

Ruben