Introduction¶

This paper describes how to derive the kinematic model for a 6-axis machine tool in an XYZABC configuration with Tool side A,B rotation of the spindle and work side C (table) rotation . In this example the B axis is a primary (rotating around the Y) and the A axis is the secondary (45° nutating) rotary axis. The primary being independent of the secondary axis. The model presented here will be named 'XYZABC-TRSRN'.

The method used here will be a step by step approach. Starting with a working kinematic model for a single rotary axis all the required elements will be added to build a complete kinematic model for the machine.

The final model includes tool-length compensation, compensation for mechanical offsets between the two rotational axis A and B and compensation for setups where the machine reference point is not located in the rotation-point of the C rotary table.

In this document only basic mathematical functions are used so the kinematic models derived can be used directly in the 'userkins.comp' template file provided with the LinuxCNC installation. All calculations can be done whithout the use of any computer algebra systems, however the use of computer assistance like 'sage' will make the process of matrix multiplication much less error prone.

Note that there are other and potentially more computationally efficient ways to build custom kinematics using built in libraries like 'posemath'. Posemath provides many functions for efficient matrix manipulation and also offers functions for the use of quaternions. However the use of such a library would require a more indepth understanding of the mathematical theory that is beyond the scope of this presentation. Furthermore importing a library like 'posemath' into the 'userkins.comp' template would require substantially more programming skills than using the method applied in this paper.

A custom kinematic model in LinuxCNC is used to calculate cartesian coordinates from given machine joint positions (forward kinematics) and also to calculate the required machine joint positions to reach a given coordinate position (inverse kinematics). In the following description we will use vectors as mathematical representations of the two positions:

\begin{matrix} (1) & Q = (\begin{array}{rrrr} Q x \\ Q y \\ Q z \\ 1 \end{array}) C a r t e s i a n p o s i t i o n P = (\begin{array}{rrrr} P x \\ P y \\ P z \\ 1 \end{array}) J o i n t p o s i t i o n \end{matrix}

$\begin{equation} Q ~=~ \left(\begin{array}{rrrr} Qx \\ Qy \\ Qz \\ 1 \end{array}\right) ~ Cartesian~position ~~~~~~~~~~~~ P ~=~ \left(\begin{array}{rrrr} Px \\ Py \\ Pz \\ 1 \end{array}\right) ~ Joint~position \end{equation}$

Note that the fourth row is added to be able to multiply the vectors with a 4x4 transformation matrix.

TCP Kinematic model¶

For the tool to follow a point on the work piece we need a model that calculates where a given position on the work piece moves to when the rotary joints are rotated. In our example configuration the work piece will be mounted on the C rotary table and it is therefore here where we start to build our forward kinematic model.
Note that in matrix multiplication the order is important that is $A\cdot B$ is generally not equal to $B \cdot A$ .

Table Rotary C¶

Forward transformation¶

We start with the basic rotation around the C axis. In this case our forward transformation matrix $^QA_P$ is equal to a rotation around the z-axis:

\begin{matrix} (2) & ^{Q} A_{P} = R_{c} \end{matrix}

$\begin{equation} ^QA_P=~ R_c \end{equation}$

with $~~Sc = sin(\theta_c),~~~ Cc = cos(\theta_c)$ and $\theta_c$ being the angle of rotation of joint C

To derive the coordinate position $Q(Qx,Qy,Qz)$ we now need to multiply the joint position vector $P(Px,Py,Pz)$ with our forward transformation matrix $^QA_P$ .
Note that the input values $P$ to our model need to be on the right hand side of the matrix multiplication.

Q =^{Q} A_{P} \cdot P

$Q=~^QA_P \cdot P$

This shows the section of the TCP forward kinematics calculation in the file 'xyzab_tdr_kins.comp'. Note that P(px,py,pz) is equal to the joint position (j[0],j[1],j[2],) while $Q(Qx,Qy,Qz)$ is output to the coordinate position (pos->tran.x, pos->tran.y, pos->tran.z). The values 'cc', 'sc' are calculated and stored in the respective variables earlier in the component file.

  case 1: // ========================= TCP kinematics FORWARD
        px          = j[0];
        py          = j[1];
        pz          = j[2];

        pos->tran.x = cc*px - sc*py;
        pos->tran.y = cc*py + sc*px;
        pos->tran.z = pz;

        pos->a      = j[3];
        pos->b      = j[4];
        pos->c      = j[5];

        break;

Inverse transformation¶

To calculate the joint position $P$ from given coordinate positions $Q$ we need to 'unrotate' joint C. Mathematically this means we need to transpose the rotation part in our transformation matrix.

\begin{matrix} (3) & ^{P} A_{Q} = R_{c}^{T} \end{matrix}

$\begin{equation} ^PA_Q=~ R_c^T \end{equation}$

To derive the joint position $P$ we then multiply the coordinate position vector $Q$ from the right:

\begin{matrix} (4) & P =^{P} A_{Q} \cdot Q \end{matrix}

$\begin{equation} P=~^PA_Q \cdot Q \end{equation}$

This shows the section of the TCP inverse kinematics calculation in the file 'xyzab_tdr_kins.comp'. Note that $Q(qx,qy,qz)$ is equal to the coordinate position (pos->tran.x, pos->tran.y, pos->tran.z) while $P(Px,Py,Pz)$ is output to the joint position (j[0],j[1],j[2]). The values 'cc', 'sc' are calculated and stored in the respective variables earlier in the component file.

    case 1: // ========================= TCP kinematics INVERSE 
        qx   = pos->tran.x;
        qy   = pos->tran.y;
        qz   = pos->tran.z;

        j[0] =   cc*qx + sc*qy;

        j[1] =   cc*qy - sc*qx;

        j[2] =   qz;

        j[3] = pos->a;
        j[4] = pos->b;
        j[5] = pos->c;

        break;

Testing in LinuxCNC shows a working TCP kinematic for rotations around C

Rotary B and C¶

Forward transformation¶

Because the linear (x,y,z)-axis motion or tool-translation $P(Px, Py, Pz)$ happens between the table rotation C and the spindle rotation B we need to insert that tool-translation as a tranformation matrix in between the rotation C and the rotation B.

To add the B rotation we expand $~^QA_P$ by multiplying first the tool-translation $T_p$ from the right and then the rotation matrix $R_b$ also from the right:

\begin{matrix} (5) & ^{Q} A_{P} = R_{c} \cdot T_{p} \cdot R_{b} \end{matrix}

$\begin{equation} ^QA_P=~ R_c \cdot T_p \cdot R_b \end{equation}$

Note how the transformation matrix $~^QA_P$ is constructed from left to right. The first operation is on the left and the last operation is on the right as we work our way from the work side to the spindle side of the kinematic chain.

with $~~Sb = sin(\theta_b),~~~ Cb = cos(\theta_b)$ and $\theta_b$ being the angle of rotation of joint B

Note that the tool-position (ie the vector in the 4th column of our transformation matrix $^QA_P$ ) does not contain the B-rotation in any form. Only the tool-orientation (ie columns 1,2 and 3) is influenced by the addition of the rotation B. This may seem wrong but we have to remember that after the B-rotation ( $Rb$ ) we have not yet built any offset (tool-length or pivot-length) into our model (ie there is no translation matrix on the right side of $Rb$ ). Hence for now a rotation around B simply means that a tool of lenght zero (ie a point) rotates with joint B and does not change it's position. So in the current form it is really not very interesting to test the effect of the added rotation around B so we will move on along the kinematic chain and add the geometric offsets ( $D_x,D_z$ ) , the rotation A ( $R_a$ ) and the pivot-lengths ( $Ly,Lz$ ) to arrive at the spindle nose.

A closer look at the spindle rotary assembly¶

To get a better understanding of these offsets in the spindle rotary assembly of our example machine we need to have a look at how the rotary axes A and B are arranged in the spindle body. This shows our spindle assembly in its home position ( $\theta_a =0$ , $\theta_b=0$ ). To get a clearer idea of the situation a tool (purple cylinder) is shown protruding from the spindle nose.

Pivot-point and pivot-length of the spindle head¶

Because the rotation axes A,B and offsets in question are all hidden inside the spindle body we simplify the model to the relevant kinematic components. The while cross with the arrow pointing up shows the intersection of the rotary axes A and B (yellow sphere). This point will be refered to as the 'pivot-point' of the spindle rotary assembly. The rotary axis B (yellow) extends horizontally from the pivot-point in the machine-y direction. The slanted rotary axis A (yellow) extends from the pivot-point to the spindle nose where the tool (purple) attaches. The 'y-pivot-length' is the distance in the y-direction between the pivot-point and the spindle nose and shown in green. The 'z-pivot-length' is the distance in the z-direction between the pivot-point and the spindle nose and shown in blue. Note that in this image there is no geometric offset between the two rotary axes and they intersect at the pivot-point.
We define the values for the offsets in the example image as y-pivot = 200 and z-pivot = 200. In our kinematic model this represents the situation where, starting from the spindle nose, we need to travel 200 in the positive z-direction and 200 in the positive y-direction to reach the pivot-point.
Note that the direction of travel when defining these offsets is arbitrary so in our case the offset situation in the image could also be defined as -200 in y and -200 in z. However once the definition is made we must keep it throughout the entire process of building the kinematic model.

To be able to add these (y,z)-pivot values to our kinematic model we need to create a transformation matrix that has the effect of a translation. Note that the direction chosen for these offsets is important here as our translation is basically a vector and we have chosen that this 'pivot-length' vector points from the spindle nose to the pivot-point. Since we are building the forward kinematic model we are travelling against this vector and thus its components ( $L_y,L_z$ ) need to be entered in the negative.

Offsets in the spindle rotary assembly¶

The rotational axes of a rotary assembly like the A/B spindle type discussed here will always have an intended or unintented offset from one rotational axis to the other. In our case this is an offset in the x direction (red indicator) and in the z direction (light blue indicator).
We define the values for the offsets in the example image as x-offset = -20 and z-offset = 40. In our kinematic model this represents the situation where, starting from the rotary A axis, we need to travel 40 in the positive z-direction and 20 in the negative x-direction to reach the pivot-point.

Note that the direction of travel when defining these offsets is arbitrary so in our case the offset situation in the image could also be defined as +20 in x and -40 in z. However once the definition is made we must keep it throughout the entire process of building the kinematic model.

Also note that it might be tempting to simplify things by just adding the z-offset (light blue) to the z-pivot (dark blue). However if we consider the situation with a rotation in A then it's clear that the two values need to be handled sparately in our kinematic model.

To be able to add these (x,z)-offset values to our kinematic model we need to create a transformation matrix that has the effect of a translation. Note that the direction chosen for these offsets is important here as our translation is basically a vector and we have chosen that this 'offset' vector points from the A rotation axis to the pivot-point. Since we are building the forward kinematic model we are travelling against this vector and thus its components ( $D_x,D_z$ ) need to be entered in the negative.

Rotary A¶

How to derive a rotation matrix for a nutating joint¶

The machine in our example has a 'nutating' rotary joint A. Nutating meaning here that the rotational axis of A is not parallel to one of the axes (x, y or z) of our machine coordinate system and hence we cannot use any of the basic rotation matrices to model it in our kinematic. The basic rotation matrices are, as we will see, only three particular solutions of a general formula to describe rotations around any given vector $V(v_x, v_y, v_z$ ) in space. So to construct the rotation matrix $R_a$ for the slanted joint A of our machine we can use the ‘Rodrigues' rotation formula. (Note that there is also an 'Euler-Rodrigues' formula that can be used for the same purpose but uses different parameters.)

\begin{matrix} (6) & R_{a} = I + s i n θ \cdot V + (2 s i n ² \frac{θ}{2}) \cdot V ² \end{matrix}

$\begin{equation} R_a ~=~ I ~+~ sin\theta \cdot V + (2sin²{\theta \over2}) \cdot V² \label{eq:rod_rot} \end{equation}$

With: $\begin{equation} I ~=~ \left(\begin{array}{rrrr} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right) ~~ V ~=~ \left(\begin{array}{rrrr} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{array}\right) \label{eq:mat_I_V} \end{equation}$

For the matrix $V$ we need a unit vector that represents the rotational axis of the 45° joint:

\vec{v} = (v_{x}, v_{y}, v_{z})

$\vec{v}=(v_x,v_y,v_z)$

with the components:

\begin{matrix} (8) & v_{x} = 0 v_{y} = \frac{1}{\sqrt{2}} v_{z} = \frac{1}{\sqrt{2}} \end{matrix}

$\begin{equation} v_x=0 ~~~~ v_y = {1\over{\sqrt2}} ~~~~ v_z = {1\over{\sqrt2}} \label{eq:vector_v} \end{equation}$

Using $\eqref{eq:rod_rot}$ and $\eqref{eq:mat_I_V}$ , $R_A$ can thus be written as:

\begin{matrix} (9) & R_{a} = (\begin{array}{rrrr} c o s θ + v ²_{x} (1 - c o s θ) & v_{x} v_{y} (1 - c o s θ) - v_{z} s i n θ & v_{x} v_{z} (1 - c o s θ) + v_{y} s i n θ \\ v_{x} v_{y} (1 - c o s θ) + v_{z} s i n θ & c o s θ + v ²_{y} (1 - c o s θ) & v_{y} v_{z} (1 - c o s θ) - v_{x} s i n θ \\ v_{x} v_{z} (1 - c o s θ) - v_{y} s i n θ & v_{y} v_{z} (1 - c o s θ) + v_{x} s i n θ & c o s θ + v ²_{z} (1 - c o s θ) \end{array}) \end{matrix}

$\begin{equation} R_a ~=~ \left(\begin{array}{rrrr} cos\theta+v²_x(1-cos\theta) & v_x v_y(1-cos\theta)-v_z sin\theta & v_x v_z(1-cos\theta)+v_y sin\theta \\ v_x v_y(1-cos\theta)+v_z sin\theta & cos\theta+v²_y(1-cos\theta) & v_y v_z(1-cos\theta)-v_x sin\theta \\ v_x v_z(1-cos\theta)-v_y sin\theta & v_y v_z(1-cos\theta)+v_x sin\theta & cos\theta+v²_z(1-cos\theta) \end{array}\right) \end{equation}$

using $\eqref{eq:vector_v}$ and the substitution $sin\theta = Sa ~~~ cos\theta = Ca$

$R_a$ becomes

\begin{matrix} (10) & R_{a} = (\begin{array}{rrrr} C a & - \frac{1}{\sqrt{2}} S a & \frac{1}{\sqrt{2}} S a \\ \frac{1}{\sqrt{2}} S a & \frac{1}{2} (1 + C a) & \frac{1}{2} (1 - C a) \\ - \frac{1}{\sqrt{2}} S a & \frac{1}{2} (1 - C a) & \frac{1}{2} (1 + C a) \end{array}) \end{matrix}

$\begin{equation} R_a ~=~ \left(\begin{array}{rrrr} Ca & -{1\over{\sqrt2}}Sa & {1\over{\sqrt2}}Sa \\ {1\over{\sqrt2}}Sa & {1\over2}(1+Ca) & {1\over2}(1-Ca) \\ -{1\over{\sqrt2}}Sa & {1\over2}(1-Ca) & {1\over2}(1+Ca) \end{array}\right) \end{equation}$

and by expanding it to a 4x4 matrix we have now the required transformation matrix

\begin{matrix} (11) & R_{a} = (\begin{array}{rrrr} C a & - \frac{1}{\sqrt{2}} S a & \frac{1}{\sqrt{2}} S a & 0 \\ \frac{1}{\sqrt{2}} S a & \frac{1}{2} (1 + C a) & \frac{1}{2} (1 - C a) & 0 \\ - \frac{1}{\sqrt{2}} S a & \frac{1}{2} (1 - C a) & \frac{1}{2} (1 + C a) & 0 \\ 0 & 0 & 0 & 1 \end{array}) \end{matrix}

$\begin{equation} R_a ~=~ \left(\begin{array}{rrrr} Ca & -{1\over{\sqrt2}}Sa & {1\over{\sqrt2}}Sa & 0 \\ {1\over{\sqrt2}}Sa & {1\over2}(1+Ca) & {1\over2}(1-Ca) & 0 \\ -{1\over{\sqrt2}}Sa & {1\over2}(1-Ca) & {1\over2}(1+Ca) & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \end{equation}$

To make the matrix multiplications more manageable we substitute

\begin{matrix} (12) & \frac{1}{\sqrt{2}} S a = t \frac{1}{2} (1 + C a) = u \frac{1}{2} (1 - C a) = v \end{matrix}

$\begin{equation} {1\over{\sqrt2}}Sa ~=~ t ~~~~~~~~ {1\over2}(1+Ca)~=~ u ~~~~~~~~ {1\over2}(1-Ca)~=~ v \end{equation}$

and get the transformation matrix in it's final form

\begin{matrix} (13) & R_{a} = (\begin{array}{rrrr} C a & - t & t & 0 \\ t & u & v & 0 \\ - t & v & u & 0 \\ 0 & 0 & 0 & 1 \end{array}) \end{matrix}

$\begin{equation} R_a ~=~ \left(\begin{array}{rrrr} Ca & -t & t & 0 \\ t & u & v & 0 \\ -t & v & u & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \end{equation}$

Tool length offset (TLO)¶

As we could see above in our inspection of the spindle rotary assembly the tool length offset ( $Dt$ ) has the same direction as the z-component of the pivot-offset ( $Lz$ ) that is, a positive $Dt$ value points in the negative machine-z direction. So as a building block in our forward kinematic transformation the value of $Dt$ needs to be in the negative:

Rotary C, B and A with offsets¶

Now that we have all the building blocks we can put everything together:

^{Q} A_{P} = R_{c} \cdot T_{p} \cdot R_{b} \cdot T_{d} \cdot R_{a} \cdot T_{l} \cdot T_{t}

$^QA_P = R_c \cdot T_p \cdot R_b \cdot T_d \cdot R_a \cdot T_l \cdot T_t$

^{Q} A_{P} = (\begin{array}{rrrr} C a C b C c - C c S b t - S c t & - C b C c t + C c S b v - S c u & C b C c t + C c S b u - S c v & - C b C c D x - C c D z S b - (C b C c t + C c S b u - S c v) (D t + L z) + (C b C c t - C c S b v + S c u) L y + C c P x - P y S c \\ C a C b S c - S b S c t + C c t & - C b S c t + S b S c v + C c u & C b S c t + S b S c u + C c v & - C b D x S c - D z S b S c - (C b S c t + S b S c u + C c v) (D t + L z) + (C b S c t - S b S c v - C c u) L y + C c P y + P x S c \\ - C a S b - C b t & S b t + C b v & - S b t + C b u & (S b t - C b u) (D t + L z) - C b D z - (S b t + C b v) L y + D x S b + P z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Ca} \mathit{Cb} \mathit{Cc} - \mathit{Cc} \mathit{Sb} t - \mathit{Sc} t & -\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} v - \mathit{Sc} u & \mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v & -\mathit{Cb} \mathit{Cc} \mathit{Dx} - \mathit{Cc} \mathit{Dz} \mathit{Sb} - {\left(\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Cc} t - \mathit{Cc} \mathit{Sb} v + \mathit{Sc} u\right)} \mathit{Ly} + \mathit{Cc} \mathit{Px} - \mathit{Py} \mathit{Sc} \\ \mathit{Ca} \mathit{Cb} \mathit{Sc} - \mathit{Sb} \mathit{Sc} t + \mathit{Cc} t & -\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} v + \mathit{Cc} u & \mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v & -\mathit{Cb} \mathit{Dx} \mathit{Sc} - \mathit{Dz} \mathit{Sb} \mathit{Sc} - {\left(\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Sc} t - \mathit{Sb} \mathit{Sc} v - \mathit{Cc} u\right)} \mathit{Ly} + \mathit{Cc} \mathit{Py} + \mathit{Px} \mathit{Sc} \\ -\mathit{Ca} \mathit{Sb} - \mathit{Cb} t & \mathit{Sb} t + \mathit{Cb} v & -\mathit{Sb} t + \mathit{Cb} u & {\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - \mathit{Cb} \mathit{Dz} - {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} + \mathit{Dx} \mathit{Sb} + \mathit{Pz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

So with this as our core forward transformation matrix we can now check the output of this model for the home position $P(Px,Py,Pz)=(0,0,0)$ and $\theta_a = \theta_b = \theta_c =0$ $(Ca=Cb=Cc=1, ~~Sa=Sb=Sc=0)$

and taking our substitution in $R_a$ into account:

\begin{matrix} (14) & \frac{1}{\sqrt{2}} S a = t \frac{1}{2} (1 + C a) = u \frac{1}{2} (1 - C a) = v \end{matrix}

$\begin{equation} {1\over{\sqrt2}}Sa ~=~ t ~~~~~~~~ {1\over2}(1+Ca)~=~ u ~~~~~~~~ {1\over2}(1-Ca)~=~ v \end{equation}$

which for $\theta_a = 0$ reduces to:

\begin{matrix} (15) & t = 0 u = 1 v = 0 \end{matrix}

$\begin{equation} t~=~ 0 ~~~~~~~~ u=~ 1 ~~~~~~~~ v~=~ 0 \end{equation}$

we get the coordinates: $Qx= -Dx$ $Qy= -Ly$ $Qz= -(Dt + Lz) -Dz$

\begin{matrix} (16) & Q = (\begin{array}{r} Q x \\ Q y \\ Q z \\ 1 \end{array}) = (\begin{array}{r} - D x \\ - L y \\ - D t - L z - D z \\ 1 \end{array}) \end{matrix}

$\begin{equation} Q ~=~ \left(\begin{array}{r} Qx \\ Qy \\ Qz \\ 1 \end{array}\right) ~=~ \left(\begin{array}{r} -Dx \\ -Ly \\ -Dt-Lz-Dz \\ 1 \end{array}\right) \end{equation}$

To get the expected coordinate position $Q(Qx,Qy,Qz)=(0,0,0)$ we need to compensate for these offsets by subtracting them from the result of our core transformation matrix. Note subtracting a vector is the same as adding its inverse vector We define the transformation:

Note that this is a translation of the result of our forward transformation matrix $^QA_P$ it needs to be multiplied from the left:

^{Q} A_{P} = T_{c} \cdot R_{c} \cdot T_{p} \cdot R_{b} \cdot T_{d} \cdot R_{a} \cdot T_{l} \cdot T_{t}

$^QA_P = T_c \cdot R_c \cdot T_p \cdot R_b \cdot T_d \cdot R_a \cdot T_l \cdot T_t$

^{Q} A_{P} = (\begin{array}{rrrr} C a C b C c - C c S b t - S c t & - C b C c t + C c S b v - S c u & C b C c t + C c S b u - S c v & - C b C c D x - C c D z S b - (C b C c t + C c S b u - S c v) (D t + L z) + (C b C c t - C c S b v + S c u) L y + C c P x - P y S c + D x \\ C a C b S c - S b S c t + C c t & - C b S c t + S b S c v + C c u & C b S c t + S b S c u + C c v & - C b D x S c - D z S b S c - (C b S c t + S b S c u + C c v) (D t + L z) + (C b S c t - S b S c v - C c u) L y + C c P y + P x S c + L y \\ - C a S b - C b t & S b t + C b v & - S b t + C b u & (S b t - C b u) (D t + L z) - C b D z - (S b t + C b v) L y + D x S b + D t + D z + L z + P z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Ca} \mathit{Cb} \mathit{Cc} - \mathit{Cc} \mathit{Sb} t - \mathit{Sc} t & -\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} v - \mathit{Sc} u & \mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v & -\mathit{Cb} \mathit{Cc} \mathit{Dx} - \mathit{Cc} \mathit{Dz} \mathit{Sb} - {\left(\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Cc} t - \mathit{Cc} \mathit{Sb} v + \mathit{Sc} u\right)} \mathit{Ly} + \mathit{Cc} \mathit{Px} - \mathit{Py} \mathit{Sc} + \mathit{Dx} \\ \mathit{Ca} \mathit{Cb} \mathit{Sc} - \mathit{Sb} \mathit{Sc} t + \mathit{Cc} t & -\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} v + \mathit{Cc} u & \mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v & -\mathit{Cb} \mathit{Dx} \mathit{Sc} - \mathit{Dz} \mathit{Sb} \mathit{Sc} - {\left(\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Sc} t - \mathit{Sb} \mathit{Sc} v - \mathit{Cc} u\right)} \mathit{Ly} + \mathit{Cc} \mathit{Py} + \mathit{Px} \mathit{Sc} + \mathit{Ly} \\ -\mathit{Ca} \mathit{Sb} - \mathit{Cb} t & \mathit{Sb} t + \mathit{Cb} v & -\mathit{Sb} t + \mathit{Cb} u & {\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - \mathit{Cb} \mathit{Dz} - {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} + \mathit{Dx} \mathit{Sb} + \mathit{Dt} + \mathit{Dz} + \mathit{Lz} + \mathit{Pz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Note that in the kinematic component $Dt$ is added to the z-pivot value Lz in the line
Lz = Lz + Dt;

    case 1: // ========================= TCP kinematics FORWARD 
        Lz          = Lz + Dt;
        // onLy used to be consistent with math documentation
        Px          = j[0];
        Py          = j[1];
        Pz          = j[2];

        pos->tran.x = - Cb*Cc*Dx - Cc*Dz*Sb 
                      + (Cb*Cc*t - Cc*Sb*v + Sc*u)*Ly 
                      - (Cb*Cc*t + Cc*Sb*u - Sc*v)*Lz 
                      + Cc*Px - Py*Sc
                      + Dx;

        pos->tran.y = - Cb*Dx*Sc - Dz*Sb*Sc 
                      + (Cb*Sc*t - Sb*Sc*v - Cc*u)*Ly 
                      - (Cb*Sc*t + Sb*Sc*u + Cc*v)*Lz 
                      + Cc*Py + Px*Sc
                      + Ly;

        pos->tran.z = - Cb*Dz 
                      - (Sb*t + Cb*v)*Ly 
                      + (Sb*t - Cb*u)*Lz 
                      + Dx*Sb 
                      + Pz
                      + Lz
                      + Dz 
                      ; 

        pos->a      = j[3];
        pos->b      = j[4];
        pos->c      = j[5];

        break;

Position offset of the rotary table to the machine reference point¶

Up to this point we have assumed that the machine reference point coincides with the rotational axis of our rotary C table. However, it is somewhat unlikely that a machines home position is exactly in the rotational axis of the rotary table and this will need to be taken into account in the kinematic model.
If we assume that we have a setup with no TLO ( $Dt=0$ ), no geometric offset ( $Dx=Dz=0$ ), no pivot-length ( $Ly=Dz=0$ ) and no offset of the rotation-axis of the rotary C to the machine reference point.
A tool positioned in rotational axis of C would have a joint-position of $P(0,0,P_z)$ and that would give the expected resulting coordinate position of $Q(0,0,P_z)$ .
If we now assume that the rotation-axis of our rotary C table is offset from the machine reference point by $(ra_x,ra_y)$ then our joint-position would be equal to $P(ra_x,ra_y,P_z)$ which would give us a result of $Q(ra_x, ra_y, P_z)$ .
So we need to subtract the offset $(ra_x,ra_y)$ from the joint-position $P=(P_x-ra_x, P_y-ra_y, P_z)$ before we input it into our transformation matrix. In other words we need to translate the joint-position vector in the opposite direction along the offset vector with the components $(ra_x,ra_y,0)$ .

Note that this is a translation $T_r$ of the input $T_p$ of our forward transformation matrix $^QA_P$ so it needs to be multiplied to $T_p$ from the right :

Thus we get:

^{Q} A_{P} = T_{c} \cdot R_{c} \cdot T_{p} \cdot T_{r} \cdot R_{b} \cdot T_{d} \cdot R_{a} \cdot T_{l} \cdot T_{t}

$^QA_P = T_c \cdot R_c \cdot T_p \cdot T_r \cdot R_b \cdot T_d \cdot R_a \cdot T_l \cdot T_t$

However some more consideration is needed for now a joint position of $P(ra_x,ra_y,P_z)$ will result in a coordinate position of $Q(0,0,P_z)$ which is of course not the value we can hand back to LinuxCNC because if the rotation axis is offset from machine home position then the coordinate position would need to be $Q(rp_x,rp_y,P_z)$ .
So to be consistent we need to add the offset values back to the results of our calculations which we do again by multiplying a vector translation to the left of our forward kinematic:

Thus we get our final forward transformation matrix:

^{Q} A_{P} = T_{- r} \cdot T_{c} \cdot R_{c} \cdot T_{p} \cdot T_{r} \cdot R_{b} \cdot T_{d} \cdot R_{a} \cdot T_{l} \cdot T_{t}

$^QA_P = T_{-r} \cdot T_c \cdot R_c \cdot T_p \cdot T_r \cdot R_b \cdot T_d \cdot R_a \cdot T_l \cdot T_t$

^{Q} A_{P} = (\begin{array}{rrrr} C a C b C c - C c S b t - S c t & - C b C c t + C c S b v - S c u & C b C c t + C c S b u - S c v & - C b C c D x - C c D z S b - C c (D r a x - P x) - (C b C c t + C c S b u - S c v) (D t + L z) + (C b C c t - C c S b v + S c u) L y + (D r a y - P y) S c + D r a x + D x \\ C a C b S c - S b S c t + C c t & - C b S c t + S b S c v + C c u & C b S c t + S b S c u + C c v & - C b D x S c - D z S b S c - C c (D r a y - P y) - (C b S c t + S b S c u + C c v) (D t + L z) + (C b S c t - S b S c v - C c u) L y - (D r a x - P x) S c + D r a y + L y \\ - C a S b - C b t & S b t + C b v & - S b t + C b u & (S b t - C b u) (D t + L z) - C b D z - (S b t + C b v) L y + D x S b + D t + D z + L z + P z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Ca} \mathit{Cb} \mathit{Cc} - \mathit{Cc} \mathit{Sb} t - \mathit{Sc} t & -\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} v - \mathit{Sc} u & \mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v & -\mathit{Cb} \mathit{Cc} \mathit{Dx} - \mathit{Cc} \mathit{Dz} \mathit{Sb} - \mathit{Cc} {\left(\mathit{Drax} - \mathit{Px}\right)} - {\left(\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Cc} t - \mathit{Cc} \mathit{Sb} v + \mathit{Sc} u\right)} \mathit{Ly} + {\left(\mathit{Dray} - \mathit{Py}\right)} \mathit{Sc} + \mathit{Drax} + \mathit{Dx} \\ \mathit{Ca} \mathit{Cb} \mathit{Sc} - \mathit{Sb} \mathit{Sc} t + \mathit{Cc} t & -\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} v + \mathit{Cc} u & \mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v & -\mathit{Cb} \mathit{Dx} \mathit{Sc} - \mathit{Dz} \mathit{Sb} \mathit{Sc} - \mathit{Cc} {\left(\mathit{Dray} - \mathit{Py}\right)} - {\left(\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Sc} t - \mathit{Sb} \mathit{Sc} v - \mathit{Cc} u\right)} \mathit{Ly} - {\left(\mathit{Drax} - \mathit{Px}\right)} \mathit{Sc} + \mathit{Dray} + \mathit{Ly} \\ -\mathit{Ca} \mathit{Sb} - \mathit{Cb} t & \mathit{Sb} t + \mathit{Cb} v & -\mathit{Sb} t + \mathit{Cb} u & {\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - \mathit{Cb} \mathit{Dz} - {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} + \mathit{Dx} \mathit{Sb} + \mathit{Dt} + \mathit{Dz} + \mathit{Lz} + \mathit{Pz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Q x = - C b C c D x - C c D z S b - C c (D r a x - P x) - (C b C c t + C c S b u - S c v) (D t + L z) + (C b C c t - C c S b v + S c u) L y + (D r a y - P y) S c + D r a x + D x

$\displaystyle \mathit{Qx} ~=~ -\mathit{Cb} \mathit{Cc} \mathit{Dx} - \mathit{Cc} \mathit{Dz} \mathit{Sb} - \mathit{Cc} {\left(\mathit{Drax} - \mathit{Px}\right)} - {\left(\mathit{Cb} \mathit{Cc} t + \mathit{Cc} \mathit{Sb} u - \mathit{Sc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Cc} t - \mathit{Cc} \mathit{Sb} v + \mathit{Sc} u\right)} \mathit{Ly} + {\left(\mathit{Dray} - \mathit{Py}\right)} \mathit{Sc} + \mathit{Drax} + \mathit{Dx}$

Q y = - C b D x S c - D z S b S c - C c (D r a y - P y) - (C b S c t + S b S c u + C c v) (D t + L z) + (C b S c t - S b S c v - C c u) L y - (D r a x - P x) S c + D r a y + L y

$\displaystyle \mathit{Qy} ~=~ -\mathit{Cb} \mathit{Dx} \mathit{Sc} - \mathit{Dz} \mathit{Sb} \mathit{Sc} - \mathit{Cc} {\left(\mathit{Dray} - \mathit{Py}\right)} - {\left(\mathit{Cb} \mathit{Sc} t + \mathit{Sb} \mathit{Sc} u + \mathit{Cc} v\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Cb} \mathit{Sc} t - \mathit{Sb} \mathit{Sc} v - \mathit{Cc} u\right)} \mathit{Ly} - {\left(\mathit{Drax} - \mathit{Px}\right)} \mathit{Sc} + \mathit{Dray} + \mathit{Ly}$

Q z = (S b t - C b u) (D t + L z) - C b D z - (S b t + C b v) L y + D x S b + D t + D z + L z + P z

$\displaystyle \mathit{Qz} ~=~ {\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - \mathit{Cb} \mathit{Dz} - {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} + \mathit{Dx} \mathit{Sb} + \mathit{Dt} + \mathit{Dz} + \mathit{Lz} + \mathit{Pz}$

    case 1: // ========================= TCP kinematics FORWARD 
        // onLy used to be consistent with math in documentation
        Px          = j[0];
        Py          = j[1];
        Pz          = j[2];

        pos->tran.x = - Cb*Cc*Dx 
                      - Cc*Dz*Sb 
                      - Cc*(Drax - Px) 
                      - (Cb*Cc*t + Cc*Sb*u - Sc*v)*(Dt + Lz) 
                      + (Cb*Cc*t - Cc*Sb*v + Sc*u)*Ly 
                      + (Dray - Py)*Sc 
                      + Drax 
                      + Dx;

        pos->tran.y = - Cb*Dx*Sc 
                      - Dz*Sb*Sc 
                      - Cc*(Dray - Py) 
                      - (Cb*Sc*t + Sb*Sc*u + Cc*v)*(Dt + Lz) 
                      + (Cb*Sc*t - Sb*Sc*v - Cc*u)*Ly 
                      - (Drax - Px)*Sc 
                      + Dray 
                      + Ly
                      ;

        pos->tran.z =  (Sb*t - Cb*u)*(Dt + Lz)
                      - Cb*Dz 
                      - (Sb*t + Cb*v)*Ly 
                      + Dx*Sb 
                      + Dt 
                      + Dz 
                      + Lz 
                      + Pz; 

        pos->a      = j[3];
        pos->b      = j[4];
        pos->c      = j[5];

        break;

Inverse transformation¶

To calculate the joint position $P$ from a given coordinate position $Q$ we need to follow the kinematic chain in the opposite direction from the spindle to work piece. We can invert our forward kinematic transformation and build it backwards, using the inverted rotations and the inverted translations.
Note that the input matrix $T_p$ with its translation { $T_p \cdot T_r$ } for the offset of the rotation axis and the output translation $[T_{-r} \cdot T_c]$ need to be handled with some consideration:

^{Q} A_{P} = [T_{- r} \cdot T_{c}] \cdot R_{c} \cdot {T_{p} \cdot T_{r}} \cdot (R_{b} \cdot T_{d} \cdot R_{a} \cdot T_{l} \cdot T_{t})

$^QA_P = [T_{-r} \cdot T_c] \cdot R_c \cdot \{T_p \cdot T_r\} \cdot (R_b \cdot T_d \cdot R_a \cdot T_l \cdot T_t)$

^{Q} A_{P} = [o f f s e t_{f o}] \cdot R_{c} \cdot {i n p u t + o f f s e t_{f i}} \cdot (R_{b} \cdot T_{d} \cdot R_{a} \cdot T_{l} \cdot T_{t})

$^QA_P = [offset_{fo}] \cdot R_c \cdot \{input+offset_{fi}\} \cdot (R_b \cdot T_d \cdot R_a \cdot T_l \cdot T_t)$

In the inverse transformation the input is $T_q$ and it's 'offset' is the inverse of the 'output offset' of the forward transformation. Basically what we added to the result of the forward transformation needs to be removed from the input to the inverse transformation and vice versa. On the right side we additionally need to rotate the inverted spindle offsets by the rotation of the rotary C:

^{Q} A_{P} = [- o f f s e t_{f i}] \cdot R_{c}^{T} \cdot {i n p u t - o f f s e t_{f o}} \cdot (R_{c} \cdot R_{b} \cdot T_{- d} \cdot R_{a} \cdot T_{- l} \cdot T_{- t})

$^QA_P = [-offset_{fi}] \cdot R^T_c \cdot \{input-offset_{fo}\} \cdot (R_c \cdot R_b \cdot T_{-d} \cdot R_a \cdot T_{-l} \cdot T_{-t})$

^{P} A_{Q} = {T_{- r}} \cdot R_{c}^{T} \cdot T_{q} \cdot [T_{r} \cdot T_{- c}] \cdot (R_{c} \cdot R_{b} \cdot T_{- d} \cdot R_{a} \cdot T_{- l} \cdot T_{- t})

$^PA_Q = \{ T_{-r}\} \cdot R^T_c \cdot T_q \cdot [T_{r} \cdot T_{-c}] \cdot(R_c \cdot R_b \cdot T_{-d} \cdot R_a \cdot T_{-l} \cdot T_{-t})$

Note that the brackets in the above expressions are only used to highlight the different parts of the kinematic model and are not mathematically required.

This being the inverse transformation our input matrix has changed to $T_q$ :

(\begin{array}{rrrr} 1 & 0 & 0 & Q x \\ 0 & 1 & 0 & Q y \\ 0 & 0 & 1 & Q z \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} C b & 0 & - S b & 0 \\ 0 & 1 & 0 & 0 \\ S b & 0 & C b & 0 \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} C a & t & - t & 0 \\ - t & u & v & 0 \\ t & v & u & 0 \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & L y \\ 0 & 0 & 1 & L z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle \left(\begin{array}{rrrr} 1 & 0 & 0 & \mathit{Qx} \\ 0 & 1 & 0 & \mathit{Qy} \\ 0 & 0 & 1 & \mathit{Qz} \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} \mathit{Cb} & 0 & -\mathit{Sb} & 0 \\ 0 & 1 & 0 & 0 \\ \mathit{Sb} & 0 & \mathit{Cb} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} \mathit{Ca} & t & -t & 0 \\ -t & u & v & 0 \\ t & v & u & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & \mathit{Ly} \\ 0 & 0 & 1 & \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

^{P} A_{Q} = (\begin{array}{rrrr} ({C c}^{2} + {S c}^{2}) C a C b - ({C c}^{2} + {S c}^{2}) S b t & - ({C c}^{2} + {S c}^{2}) C b t + ({C c}^{2} + {S c}^{2}) S b v & ({C c}^{2} + {S c}^{2}) C b t + ({C c}^{2} + {S c}^{2}) S b u & ({C c}^{2} + {S c}^{2}) C b D x + ({C c}^{2} + {S c}^{2}) D z S b - C c (D r a x + D x - Q x) + (({C c}^{2} + {S c}^{2}) C b t + ({C c}^{2} + {S c}^{2}) S b u) (D t + L z) - (({C c}^{2} + {S c}^{2}) C b t - ({C c}^{2} + {S c}^{2}) S b v) L y - (D r a y + L y - Q y) S c + D r a x \\ ({C c}^{2} + {S c}^{2}) t & ({C c}^{2} + {S c}^{2}) u & ({C c}^{2} + {S c}^{2}) v & ({C c}^{2} + {S c}^{2}) L y u + ({C c}^{2} + {S c}^{2}) (D t + L z) v - C c (D r a y + L y - Q y) + (D r a x + D x - Q x) S c + D r a y \\ - C a S b - C b t & S b t + C b v & - S b t + C b u & - (S b t - C b u) (D t + L z) + C b D z + (S b t + C b v) L y - D x S b - D t - D z - L z + Q z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle ^PA_Q=~ \left(\begin{array}{rrrr} {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Ca} \mathit{Cb} - {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} t & -{\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} t + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} v & {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} t + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} u & {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} \mathit{Dx} + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Dz} \mathit{Sb} - \mathit{Cc} {\left(\mathit{Drax} + \mathit{Dx} - \mathit{Qx}\right)} + {\left({\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} t + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - {\left({\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} t - {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} v\right)} \mathit{Ly} - {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} \mathit{Sc} + \mathit{Drax} \\ {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} t & {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} u & {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} v & {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Ly} u + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} v - \mathit{Cc} {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} + {\left(\mathit{Drax} + \mathit{Dx} - \mathit{Qx}\right)} \mathit{Sc} + \mathit{Dray} \\ -\mathit{Ca} \mathit{Sb} - \mathit{Cb} t & \mathit{Sb} t + \mathit{Cb} v & -\mathit{Sb} t + \mathit{Cb} u & -{\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Cb} \mathit{Dz} + {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} - \mathit{Dx} \mathit{Sb} - \mathit{Dt} - \mathit{Dz} - \mathit{Lz} + \mathit{Qz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

P x = ({C c}^{2} + {S c}^{2}) C b D x + ({C c}^{2} + {S c}^{2}) D z S b - C c (D r a x + D x - Q x) + (({C c}^{2} + {S c}^{2}) C b t + ({C c}^{2} + {S c}^{2}) S b u) (D t + L z) - (({C c}^{2} + {S c}^{2}) C b t - ({C c}^{2} + {S c}^{2}) S b v) L y - (D r a y + L y - Q y) S c + D r a x

$\displaystyle \mathit{Px} ~=~ {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} \mathit{Dx} + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Dz} \mathit{Sb} - \mathit{Cc} {\left(\mathit{Drax} + \mathit{Dx} - \mathit{Qx}\right)} + {\left({\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} t + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - {\left({\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Cb} t - {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Sb} v\right)} \mathit{Ly} - {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} \mathit{Sc} + \mathit{Drax}$

P y = ({C c}^{2} + {S c}^{2}) L y u + ({C c}^{2} + {S c}^{2}) (D t + L z) v - C c (D r a y + L y - Q y) + (D r a x + D x - Q x) S c + D r a y

$\displaystyle \mathit{Py} ~=~ {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} \mathit{Ly} u + {\left(\mathit{Cc}^{2} + \mathit{Sc}^{2}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} v - \mathit{Cc} {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} + {\left(\mathit{Drax} + \mathit{Dx} - \mathit{Qx}\right)} \mathit{Sc} + \mathit{Dray}$

P z = - (S b t - C b u) (D t + L z) + C b D z + (S b t + C b v) L y - D x S b - D t - D z - L z + Q z

$\displaystyle \mathit{Pz} ~=~ -{\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Cb} \mathit{Dz} + {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} - \mathit{Dx} \mathit{Sb} - \mathit{Dt} - \mathit{Dz} - \mathit{Lz} + \mathit{Qz}$

Note that $cos²\theta+sin²\theta=1$

    case 1: // ========================= TCP kinematics INVERSE 
        Qx = pos->tran.x;
        Qy = pos->tran.y; 
        Qz = pos->tran.z;   

        j[0] =   Cb*Dx 
               + Dz*Sb 
               - Cc*(Drax + Dx - Qx) 
               + (Cb*t + Sb*u)*(Dt + Lz) 
               - (Cb*t - Sb*v)*Ly 
               - (Dray + Ly - Qy)*Sc 
               + Drax;

        j[1] =   Ly*u 
               + (Dt + Lz)*v 
               - Cc*(Dray + Ly - Qy) 
               + (Drax + Dx - Qx)*Sc 
               + Dray;

        j[2] = - (Sb*t - Cb*u)*(Dt + Lz) 
               + Cb*Dz + (Sb*t + Cb*v)*Ly 
               - Dx*Sb 
               - Dt 
               - Dz 
               - Lz 
               + Qz;

        j[3] = pos->a;
        j[4] = pos->b;
        j[5] = pos->c;

        break;

Also note that with this kinematic model the offset of the rotational axis of the rotary C is the vector $(Dra_x,Dra_y,0)$ going from the pivot-point of the spindle assembly to the rotatio-axis of C with the machine in the home position. In other words if the C-rotary-axis has the absolute machine coordinates $(Rot_x,Rot_y,0)$ then $Dra_x = Rot_x - Dx$ $Dra_y = Rot_y - Ly$

In the kinematic comp this is handled in the variable declaration:

// geometric offsets of the universal spindle head as defined in 
double         Ly = *haldata->y_pivot;
double         Lz = *haldata->z_pivot;
double         Dx = *haldata->x_offset;
double         Dz = *haldata->z_offset;
double       Drax = *haldata->x_rot_axis - Dx;
double       Dray = *haldata->y_rot_axis - Ly;

This concludes the TCP kinematic

Tool Kinematic model¶

Many, if not most, applications for 5-axis milling do not require TCP kinematics where all five axes are moving simultaneously but only need the work piece to be oriented at certain angles to the tool in between 'conventional' three axis (x,y,z) milling operations. In these operations the tool is not reoriented while cutting the material. This is what is called '3+2' mode.
In 3+2 mode the machine operator can use the familiar built in cycles the machine controller offers for 3d (x,y,z) use and does not necessarily require CAM/CAD software to machine a part. The ability to move the tool in a plane perpendicular to its rotational axis also allows the use of probes for job setup.
Machines with work side rotation (eg the C rotary in our example, or the 'table-rotary-tilting' examples included in LinuxCNC simulation configs) can orient the work piece to the tool by simply rotating the work to the required orientation as the tool always remains oriented along the machine z-axis. A drilling operation on such a machine will thus still only require the machine z-axis to be moved no matter how the part is oriented. In this case no special kinematic is required.
Machines with tool side rotation like the one presented here with A and B spindle rotations however retain the directions of the unrotated machine coordinate system when moving in IDENTITY and TCP kinematics while the tool orientation changes in respect to the machine coordinate system. A drilling operation on such a machine will require the machine to move in a complex manner that may include all three (x,y,z) axes depending on how tool is oriented.
Commercial 5-axis machine controllers offer built in functionality that allow the operator to define work plane orientation using Gcode commands like G68.2 or similar; automatically adjusting the kinematic model to the type of kinematic of the machine. To implement such a feature for our example machine using LinuxCNC we need to create another kind of kinematic model which we will call the TOOL kinematic.

Forward transformation¶

For the TOOL kinematic start at the tool tip and work towards the work piece omitting the C axis rotation. So in our example case our forward transformation matrix $^QA_P$ is :

^{Q} A_{P} = R_{t c}^{T} \cdot T_{l} \cdot R_{a}^{T} \cdot T_{d} \cdot R_{b}^{T} \cdot (T_{p} \cdot T_{- d} \cdot T_{- l})

$^QA_P = R^T_{tc} \cdot T_l \cdot R^T_a \cdot T_d \cdot R^T_b\ \cdot ( T_p \cdot T_{-d} \cdot T_{-l})$

Where $R_{tc}$ is a virtual rotation around the rotational axis of the tool. This is required because we need a way to define the orientation of the tool-x (and -y). Without this the tool-coordinate system will be 'fixed' to the spindle head orientation which we do certainly want for the tool-z orientation but not for the x,y-directions. This will be important when developing the 'Tilted Work Plane' features later.

Tool-length offset is applied automatically in LinuxCNC by subtracting the tool length value stored in the tool table from the z-axis coordinate position while the joint position remains unchanged. This built in compensation also work in our custom tool kinematic because the tool coordinate system is always aligned with the rotational axis of the tool.

Note that the brackets in the above expressions are only used to highlight the different parts of the kinematic model and are not mathematically required.

(\begin{array}{rrrr} C t c & S t c & 0 & 0 \\ - S t c & C t c & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & - L y \\ 0 & 0 & 1 & - L z \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} C a & t & - t & 0 \\ - t & u & v & 0 \\ t & v & u & 0 \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} 1 & 0 & 0 & - D x \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & - D z \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} C b & 0 & - S b & 0 \\ 0 & 1 & 0 & 0 \\ S b & 0 & C b & 0 \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} 1 & 0 & 0 & P x \\ 0 & 1 & 0 & P y \\ 0 & 0 & 1 & P z \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} 1 & 0 & 0 & D x \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & D z \\ 0 & 0 & 0 & 1 \end{array}) \cdot (\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & L y \\ 0 & 0 & 1 & L z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle \left(\begin{array}{rrrr} \mathit{Ctc} & \mathit{Stc} & 0 & 0 \\ -\mathit{Stc} & \mathit{Ctc} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & -\mathit{Ly} \\ 0 & 0 & 1 & -\mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} \mathit{Ca} & t & -t & 0 \\ -t & u & v & 0 \\ t & v & u & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & -\mathit{Dx} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & -\mathit{Dz} \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} \mathit{Cb} & 0 & -\mathit{Sb} & 0 \\ 0 & 1 & 0 & 0 \\ \mathit{Sb} & 0 & \mathit{Cb} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & \mathit{Px} \\ 0 & 1 & 0 & \mathit{Py} \\ 0 & 0 & 1 & \mathit{Pz} \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & \mathit{Dx} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \mathit{Dz} \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & \mathit{Ly} \\ 0 & 0 & 1 & \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

^{Q} A_{P} = (\begin{array}{rrrr} (C a C t c - S t c t) C b - (C t c t - S t c v) S b & C t c t + S t c u & - (C t c t - S t c v) C b - (C a C t c - S t c t) S b & ((C a C t c - S t c t) C b - (C t c t - S t c v) S b) (D x + P x) - (C a C t c - S t c t) D x - ((C t c t - S t c v) C b + (C a C t c - S t c t) S b) (D z + L z + P z) + (C t c t - S t c v) D z + (C t c t + S t c u) (L y + P y) - L y S t c \\ - (C a S t c + C t c t) C b + (S t c t + C t c v) S b & - S t c t + C t c u & (S t c t + C t c v) C b + (C a S t c + C t c t) S b & - ((C a S t c + C t c t) C b - (S t c t + C t c v) S b) (D x + P x) + (C a S t c + C t c t) D x + ((S t c t + C t c v) C b + (C a S t c + C t c t) S b) (D z + L z + P z) - (S t c t + C t c v) D z - (S t c t - C t c u) (L y + P y) - C t c L y \\ C b t + S b u & v & - S b t + C b u & (C b t + S b u) (D x + P x) - (S b t - C b u) (D z + L z + P z) - D x t - D z u + (L y + P y) v - L z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} {\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Cb} - {\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Sb} & \mathit{Ctc} t + \mathit{Stc} u & -{\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Cb} - {\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Sb} & {\left({\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Cb} - {\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Sb}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} - {\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Dx} - {\left({\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Cb} + {\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Sb}\right)} {\left(\mathit{Dz} + \mathit{Lz} + \mathit{Pz}\right)} + {\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Dz} + {\left(\mathit{Ctc} t + \mathit{Stc} u\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ly} \mathit{Stc} \\ -{\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Cb} + {\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Sb} & -\mathit{Stc} t + \mathit{Ctc} u & {\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Cb} + {\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Sb} & -{\left({\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Cb} - {\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Sb}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} + {\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Dx} + {\left({\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Cb} + {\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Sb}\right)} {\left(\mathit{Dz} + \mathit{Lz} + \mathit{Pz}\right)} - {\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Dz} - {\left(\mathit{Stc} t - \mathit{Ctc} u\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ctc} \mathit{Ly} \\ \mathit{Cb} t + \mathit{Sb} u & v & -\mathit{Sb} t + \mathit{Cb} u & {\left(\mathit{Cb} t + \mathit{Sb} u\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} - {\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dz} + \mathit{Lz} + \mathit{Pz}\right)} - \mathit{Dx} t - \mathit{Dz} u + {\left(\mathit{Ly} + \mathit{Py}\right)} v - \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Q x = ((C a C t c - S t c t) C b - (C t c t - S t c v) S b) (D x + P x) - (C a C t c - S t c t) D x - ((C t c t - S t c v) C b + (C a C t c - S t c t) S b) (D z + L z + P z) + (C t c t - S t c v) D z + (C t c t + S t c u) (L y + P y) - L y S t c

$\displaystyle \mathit{Qx} ~=~ {\left({\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Cb} - {\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Sb}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} - {\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Dx} - {\left({\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Cb} + {\left(\mathit{Ca} \mathit{Ctc} - \mathit{Stc} t\right)} \mathit{Sb}\right)} {\left(\mathit{Dz} + \mathit{Lz} + \mathit{Pz}\right)} + {\left(\mathit{Ctc} t - \mathit{Stc} v\right)} \mathit{Dz} + {\left(\mathit{Ctc} t + \mathit{Stc} u\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ly} \mathit{Stc}$

Q y = - ((C a S t c + C t c t) C b - (S t c t + C t c v) S b) (D x + P x) + (C a S t c + C t c t) D x + ((S t c t + C t c v) C b + (C a S t c + C t c t) S b) (D z + L z + P z) - (S t c t + C t c v) D z - (S t c t - C t c u) (L y + P y) - C t c L y

$\displaystyle \mathit{Qy} ~=~ -{\left({\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Cb} - {\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Sb}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} + {\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Dx} + {\left({\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Cb} + {\left(\mathit{Ca} \mathit{Stc} + \mathit{Ctc} t\right)} \mathit{Sb}\right)} {\left(\mathit{Dz} + \mathit{Lz} + \mathit{Pz}\right)} - {\left(\mathit{Stc} t + \mathit{Ctc} v\right)} \mathit{Dz} - {\left(\mathit{Stc} t - \mathit{Ctc} u\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ctc} \mathit{Ly}$

Q z = (C b t + S b u) (D x + P x) - (S b t - C b u) (D z + L z + P z) - D x t - D z u + (L y + P y) v - L z

$\displaystyle \mathit{Qz} ~=~ {\left(\mathit{Cb} t + \mathit{Sb} u\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} - {\left(\mathit{Sb} t - \mathit{Cb} u\right)} {\left(\mathit{Dz} + \mathit{Lz} + \mathit{Pz}\right)} - \mathit{Dx} t - \mathit{Dz} u + {\left(\mathit{Ly} + \mathit{Py}\right)} v - \mathit{Lz}$

In preparation for the 'Tilted work plane' (TWP) feature we introduce a custom offset Twp-(x,y,z). This will be the offset from the current work offset (eg G54) to the origin of the TWP as defined by the (X,Y,Z) words in the Gcode (eg G68.2).
Example:
G68.2 X10 Y20 Z30 ... (the other words are not relevant here) will define a TWP with an offset of (10,20,30) to the active work origin (0,0,0).

This custom offset can be built into our forward kinematic as a translation of the result of the forward transformation:
$Q~=~ T_{-twp} \cdot~ ^QA_P \cdot P$ Which means simply adding a subtraction to the respective formula in our kinematic component:

 case 3: // ========================= TOOL kinematics FORWARD 
        // onLy used to be consistent with math in documentation
        Px          = j[0];
        Py          = j[1];
        Pz          = j[2];

        pos->tran.x =    ((Ca*Ctc - Stc*t)*Cb  
                           - (Ctc*t - Stc*v)*Sb)*(Dx + Px) 
                       -  (Ca*Ctc - Stc*t)*Dx 
                       - ((Ctc*t - Stc*v)*Cb 
                           + (Ca*Ctc - Stc*t)*Sb)*(Dz + Lz + Pz) 
                       +  (Ctc*t - Stc*v)*Dz 
                       +  (Ctc*t + Stc*u)*(Ly + Py) 
                       -  Ly*Stc
                       -  Twp_x;


        pos->tran.y =  - ((Ca*Stc + Ctc*t)*Cb - (Stc*t 
                           + Ctc*v)*Sb)*(Dx + Px) 
                       +  (Ca*Stc + Ctc*t)*Dx 
                       + ((Stc*t + Ctc*v)*Cb + (Ca*Stc 
                           + Ctc*t)*Sb)*(Dz + Lz + Pz) 
                       -  (Stc*t + Ctc*v)*Dz 
                       -  (Stc*t - Ctc*u)*(Ly + Py) 
                       -  Ctc*Ly
                       -  Twp_y;


        pos->tran.z =    (Cb*t + Sb*u)*(Dx + Px) 
                       - (Sb*t - Cb*u)*(Dz + Lz + Pz)
                       -  Dx*t 
                       -  Dz*u
                       + (Ly + Py)*v
                       -  Lz
                       -  Twp_z;

        pos->a      = j[3];
        pos->b      = j[4];
        pos->c      = j[5];

        break;

Inverse transformation¶

For the inverse kinematic we need to move from the machine coordinates to the tool coordinates starting with the inverted offset translations $(T_d \cdot T_l )$ for the offsets we used on the input of the forward kinematic $T_{-d} \cdot T_{-l}$ :

^{Q} A_{P} = R_{t c}^{T} \cdot T_{l} \cdot R_{a}^{T} \cdot T_{d} \cdot R_{b}^{T} \cdot (T_{p} \cdot T_{- d} \cdot T_{- l})

$^QA_P = R^T_{tc} \cdot T_l \cdot R^T_a \cdot T_d \cdot R^T_b\ \cdot ( T_p \cdot T_{-d} \cdot T_{-l})$

^{P} A_{Q} = (T_{d} \cdot T_{l}) \cdot R_{b} \cdot T_{- d} \cdot R_{a} \cdot T_{- l} \cdot R_{t c} \cdot T_{q}

$^PA_Q = (T_d \cdot T_l ) \cdot R_b \cdot T_{-d} \cdot R_a \cdot T_{-l} \cdot R_{tc}\cdot T_q$

Note that the brackets in the above expressions are only used to highlight the different parts of the kinematic model and are not mathematically required.

^{P} A_{Q} = (\begin{array}{rrrr} (C a C b - S b t) C t c - (C b t - S b v) S t c & - (C b t - S b v) C t c - (C a C b - S b t) S t c & C b t + S b u & C b D x - (C b t - S b v) L y + (C b t + S b u) L z + ((C a C b - S b t) C t c - (C b t - S b v) S t c) Q x - ((C b t - S b v) C t c + (C a C b - S b t) S t c) Q y + (C b t + S b u) Q z + D z S b - D x \\ C t c t + S t c u & - S t c t + C t c u & v & (C t c t + S t c u) Q x - (S t c t - C t c u) Q y + L y u + L z v + Q z v - L y \\ - (C a S b + C b t) C t c + (S b t + C b v) S t c & (S b t + C b v) C t c + (C a S b + C b t) S t c & - S b t + C b u & C b D z + (S b t + C b v) L y - (S b t - C b u) L z - ((C a S b + C b t) C t c - (S b t + C b v) S t c) Q x + ((S b t + C b v) C t c + (C a S b + C b t) S t c) Q y - (S b t - C b u) Q z - D x S b - D z - L z \\ 0 & 0 & 0 & 1 \end{array})

$\displaystyle ^PA_Q =~ \left(\begin{array}{rrrr} {\left(\mathit{Ca} \mathit{Cb} - \mathit{Sb} t\right)} \mathit{Ctc} - {\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Stc} & -{\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Ctc} - {\left(\mathit{Ca} \mathit{Cb} - \mathit{Sb} t\right)} \mathit{Stc} & \mathit{Cb} t + \mathit{Sb} u & \mathit{Cb} \mathit{Dx} - {\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Ly} + {\left(\mathit{Cb} t + \mathit{Sb} u\right)} \mathit{Lz} + {\left({\left(\mathit{Ca} \mathit{Cb} - \mathit{Sb} t\right)} \mathit{Ctc} - {\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Stc}\right)} \mathit{Qx} - {\left({\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Ctc} + {\left(\mathit{Ca} \mathit{Cb} - \mathit{Sb} t\right)} \mathit{Stc}\right)} \mathit{Qy} + {\left(\mathit{Cb} t + \mathit{Sb} u\right)} \mathit{Qz} + \mathit{Dz} \mathit{Sb} - \mathit{Dx} \\ \mathit{Ctc} t + \mathit{Stc} u & -\mathit{Stc} t + \mathit{Ctc} u & v & {\left(\mathit{Ctc} t + \mathit{Stc} u\right)} \mathit{Qx} - {\left(\mathit{Stc} t - \mathit{Ctc} u\right)} \mathit{Qy} + \mathit{Ly} u + \mathit{Lz} v + \mathit{Qz} v - \mathit{Ly} \\ -{\left(\mathit{Ca} \mathit{Sb} + \mathit{Cb} t\right)} \mathit{Ctc} + {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Stc} & {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ctc} + {\left(\mathit{Ca} \mathit{Sb} + \mathit{Cb} t\right)} \mathit{Stc} & -\mathit{Sb} t + \mathit{Cb} u & \mathit{Cb} \mathit{Dz} + {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} - {\left(\mathit{Sb} t - \mathit{Cb} u\right)} \mathit{Lz} - {\left({\left(\mathit{Ca} \mathit{Sb} + \mathit{Cb} t\right)} \mathit{Ctc} - {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Stc}\right)} \mathit{Qx} + {\left({\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ctc} + {\left(\mathit{Ca} \mathit{Sb} + \mathit{Cb} t\right)} \mathit{Stc}\right)} \mathit{Qy} - {\left(\mathit{Sb} t - \mathit{Cb} u\right)} \mathit{Qz} - \mathit{Dx} \mathit{Sb} - \mathit{Dz} - \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

P x = C b D x - (C b t - S b v) L y + (C b t + S b u) L z + ((C a C b - S b t) C t c - (C b t - S b v) S t c) Q x - ((C b t - S b v) C t c + (C a C b - S b t) S t c) Q y + (C b t + S b u) Q z + D z S b - D x

$\displaystyle \mathit{Px} ~=~ \mathit{Cb} \mathit{Dx} - {\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Ly} + {\left(\mathit{Cb} t + \mathit{Sb} u\right)} \mathit{Lz} + {\left({\left(\mathit{Ca} \mathit{Cb} - \mathit{Sb} t\right)} \mathit{Ctc} - {\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Stc}\right)} \mathit{Qx} - {\left({\left(\mathit{Cb} t - \mathit{Sb} v\right)} \mathit{Ctc} + {\left(\mathit{Ca} \mathit{Cb} - \mathit{Sb} t\right)} \mathit{Stc}\right)} \mathit{Qy} + {\left(\mathit{Cb} t + \mathit{Sb} u\right)} \mathit{Qz} + \mathit{Dz} \mathit{Sb} - \mathit{Dx}$

P y = (C t c t + S t c u) Q x - (S t c t - C t c u) Q y + L y u + L z v + Q z v - L y

$\displaystyle \mathit{Py} ~=~ {\left(\mathit{Ctc} t + \mathit{Stc} u\right)} \mathit{Qx} - {\left(\mathit{Stc} t - \mathit{Ctc} u\right)} \mathit{Qy} + \mathit{Ly} u + \mathit{Lz} v + \mathit{Qz} v - \mathit{Ly}$

P z = C b D z + (S b t + C b v) L y - (S b t - C b u) L z - ((C a S b + C b t) C t c - (S b t + C b v) S t c) Q x + ((S b t + C b v) C t c + (C a S b + C b t) S t c) Q y - (S b t - C b u) Q z - D x S b - D z - L z

$\displaystyle \mathit{Pz} ~=~ \mathit{Cb} \mathit{Dz} + {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ly} - {\left(\mathit{Sb} t - \mathit{Cb} u\right)} \mathit{Lz} - {\left({\left(\mathit{Ca} \mathit{Sb} + \mathit{Cb} t\right)} \mathit{Ctc} - {\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Stc}\right)} \mathit{Qx} + {\left({\left(\mathit{Sb} t + \mathit{Cb} v\right)} \mathit{Ctc} + {\left(\mathit{Ca} \mathit{Sb} + \mathit{Cb} t\right)} \mathit{Stc}\right)} \mathit{Qy} - {\left(\mathit{Sb} t - \mathit{Cb} u\right)} \mathit{Qz} - \mathit{Dx} \mathit{Sb} - \mathit{Dz} - \mathit{Lz}$

The custom TWP-offset we built into our forward kinematic as a translation of the result needs to be reflected in our inverse kinematic by an inverse translation of the input:
$P~=~ ^PA_Q \cdot Q \cdot T_{twp}~$ In the kinematic component this is done in lines:

Qx = pos->tran.x + Twp_x;
Qy = pos->tran.y + Twp_y;
Qz = pos->tran.z + Twp_z;

case 3: // ========================= TOOL  kinematics INVERSE
        Qx = pos->tran.x + Twp_x;
        Qy = pos->tran.y + Twp_y;
        Qz = pos->tran.z + Twp_z;

        j[0] =   Cb*Dx 
               - (Cb*t - Sb*v)*Ly 
               + (Cb*t + Sb*u)*Lz 
               + ((Ca*Cb - Sb*t)*Ctc - (Cb*t - Sb*v)*Stc)*Qx 
               - ((Cb*t - Sb*v)*Ctc + (Ca*Cb - Sb*t)*Stc)*Qy 
               + (Cb*t + Sb*u)*Qz + Dz*Sb - Dx
               ;

        j[1] =   (Ctc*t + Stc*u)*Qx 
               - (Stc*t - Ctc*u)*Qy 
               + Ly*u 
               + Lz*v 
               + Qz*v 
               - Ly
               ;


        j[2] =    Cb*Dz 
               + (Sb*t + Cb*v)*Ly 
               - (Sb*t - Cb*u)*Lz 
               - ((Ca*Sb + Cb*t)*Ctc - (Sb*t + Cb*v)*Stc)*Qx 
               + ((Sb*t + Cb*v)*Ctc + (Ca*Sb + Cb*t)*Stc)*Qy 
               - (Sb*t - Cb*u)*Qz 
               - Dx*Sb 
               - Dz 
               - Lz
                ;

        j[3] = pos->a;
        j[4] = pos->b;
        j[5] = pos->c;

        break;

This completes the TOOL kinematic

Caveats of TOOL kinematics (TOOL kinematics is NOT TCP!)¶

Any machine operator using TOOL kinematics should be well aware of its behavior before issuing motion commands!
It must be stressed that, while TOOL kinematics are active, any movement of a rotary joint will cause the machine to rotate the TCS around the machine reference point. Because the magnitude of the resulting movement increases with the distance between the tool position and the machine reference point the effect may seem unpredictable to the inexperienced operator. Note here that it is possible to set the rotation point in TOOL kinematics in much the same way as we offset the rotation-point from machine zero in TCP kinematics. This however would further complicate work offset transformation and there seems to be no immediate benefit to it.

'Tilted Work Plane' (TWP)¶

Commercial 5-axis machine controllers offer built in functionality that allow the operator to define work plane orientation using Gcode commands like G68.2 or similar. With the new custom TOOL kinematic we can now explore a possible implementation of TWP in LinuxCNC.
For the purpose of this example we choose the already mentioned 'G68.2' and the associated Gcodes as used by Fanuc:

G68.2 - Set custom coordinate system using five different modes
G68.3 - Set custom coordinate system using current tool orientation
G68.4 - Same as 68.2 but as an incremental reorientation of an existing TWP
G53.1 - Orient the tool to the TWP using non-TCP joint rotation
G53.6 - Orient the tool to the TWP using TCP joint rotation
G69 - Cancel the TWP setting

The TWP feature always requires a TWP-definition (G68.x) before an orientation command (G53.x) can be issued.
G68.x commands do not cause any machine movement while G53 command will cause immediate machine movement. At the end of a TWP operation a G69 command is used to return to machine coordinates.

In this presentation we use a 'Pure Python Remap' to make the above Gcode commands available to the machine operator. As a consequence our depth of integration is quite limited in the sense that the LinuxCNC Gcode interpreter will be totally ignorant of our 'TWP'-mode.

Implementation outline¶

This should give a rough overview of how TWP functionality is to be achieved with a python remap:

The idea of TWP is to give the machine operator a mechanism to define a virtual work plane that is rotated (and optionally offset) in respect to the machine coordinate system. As mentioned above this is fairly trivial in a machine with work side rotation as its tool spindle is fixed to be aligned with the machine z-axis. In such a machine the tool coordinate system (TCS) remains in the orientation of the machine coordinate system (MCS)
In a machine with tool side rotation however the TCS rotates in respect to the MCS as the rotary joints move. The orientation of the TCS for a given joint rotation can be calculated using the rotational part of the transformation matrix as derived above. Furthermore we can also solve for the inverse case of finding the joint positions that will orient the tool to a given orientation. Note here that this is very much dependent on the specific kinematic of the machine at hand and will require careful analysis of the kinematic model to find any ambiguities in the solution as some specific tool orientations might be achieved in multiple joint positions.

So to define the TWP means to define a target TCS in respect to the MCS and for this the use of a transformation matrix is well suited as it contains the rotation as well as the (optional) translation.

Step 1: Definition of the TWP¶

The TWP is defined by a rotation (I,J,K) and optional offset (X,Y,Z) of the TCS in respect to the MCS. The optional offset is calulated from the current work offset position at the time when the G68.2 or G68.3 command is issued.

The operator can choose from different methods to define the rotation of the custom work plane:

G68.2 ... P0 ... - 'True Euler'-angles (this is the default if the P word is omitted)
G68.2 ... P1 ... - 'Pitch-Roll-Yaw'-angles
G68.2 ... P2 ... - 3 points in the plane (3 points define two vectors)
G68.2 ... P3 ... - 2 vectors (tool-x and tool-z)
G68.2 ... P4 ... - projected angles (A around machine-x, B around machine-y)

For the above methods we create the transformation matrix either by using euler-rotations of the 4x4 identity matrix or by the vector information passed by the command using the cross product to calculate the y-vector. For the projected angles we use basich trigonometry to calculate the vector components. These calculations are independent from the specific machine kinematics.

G68.3 ...... - define TWP perpendicular to the current tool orientation

Here we need to use the specific machine kinematic model to calculate the transformation matrix of the current TCS.

G68.4 ...... - same as G68.2 but relative to the current TWP

Here we need to multiply the current TWP transformation matrix with the one built from the command words.

G69 - cancel TWP

Here we reset the TWP transformation matrix to the 4x4 identity matrix (ie no rotation, no translation).

Note that, contrary to some other controllers, moves commanded between G68.x and G53.x will be in the MCS.

Step 2: Orientation of the tool to the TWP¶

Once the TWP has been defined the machine spindle can be oriented by moving the rotary joints to the appropriate positions.

The operator can choose whether the rotation is done in IDENTITY kinematics (only the rotary joints move) or in TCP kinematics where all joint may move to keep the tool center point in position:

G53.1 - orient the tool to the TWP without using TCP
G53.6 - orient the tool to the TWP with using TCP

Handling work offsets when switching to TWP¶

The TWP is defined relative to the work offset (eg G54) active when issuing a G62.x command. With the execution of the following G53.x command and the reorientation of the spindle the kinematic is switched to the TOOL kinematics and the coordinate position in the DRO will then reflect the coordinate position in the rotated TCS. If the work offset values were to be used without adjusting for the kinematic switch then the physical position of the work offset would be rotated out of place and the reference to the work coordinate system (WCS) would be lost. So in order to find the reference on the work piece in the TCS we need to use the TWP transformation matrix to transform the work offset values set in WCS to the new TCS.
Note that the original work offset will have to be restored when the TWP is canceled with a G69 command.