kinematics answer

["Till Franitza" <xfa-at-isw.uni-stuttgart.de>]

Hi Robin!
It is as trivial as you supposed.
This is the code of the trivial kinematics. Do not bother about a,b and c.
These are the (for example rotational) axes you do not need.


int kinematicsForward(const double * joints,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
  pos->tran.x = joints[0];
  pos->tran.y = joints[1];
  pos->tran.z = joints[2];
  pos->a = joints[3];
  pos->b = joints[4];
  pos->c = joints[5];

  return 0;
}

int kinematicsInverse(const EmcPose * pos,
                      double * joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
  joints[0] = pos->tran.x;
  joints[1] = pos->tran.y;
  joints[2] = pos->tran.z;
  joints[3] = pos->a;
  joints[4] = pos->b;
  joints[5] = pos->c;

  return 0;
}

pos is the cartesian position that you refer to in g-code and that you see
in the display of the NC normally. In EMC you can switch between worls
coordinates and joint coordinates. the inverse kinematics and the forward
kins have to fit together. They have to be the inverse function of one
another. So, for example you find out that when your y-axis moves 100mm, the
x-Position of the TCP moves 1 mm down. To compensate this you figure out you
have to make the y-Axis go 1% of the x motion up:

joint 0 = pos x+ pos y * 0.01
joint 1 = pos y
joint 2 = pos z

that means:
pos=forw_trafo*joint
inv_trafo=inv(forw_trafo)
joint=inv_trafo*pos

in our case:
(this is matlab output)
» forw_trafo

forw_trafo =

    1.0000   -0.0100         0
         0    1.0000         0
         0         0    1.0000

inv_trafo =

    1.0000    0.0100         0
         0    1.0000         0
         0         0    1.0000


pos =

   100
   100
   100

» joint=inv_trafo*pos

joint =

   101
   100
   100

» -------------------------
» joint=[100 100 100]'

joint =

   100
   100
   100

» pos=forw_trafo*joint

pos =

    99
   100
   100

»
so the forward kinematics:

pos x = joint 0 - joint 1 *0.01
pos y = joint 1
pos z = joint 2

now, if you want to put the relationship between pos x and joint 1 in a
table or in another function.
tell me jour fumction an i will write trafos if jou want.
Till