Titlepic

Introduction¶

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

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 B and C and compensation for setups where the machine reference point is not located in the rotation-point of the A 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.

In [1]:
from IPython.display import display, Image
# only used to display equations out of code blocks
from IPython.display import Math, display

# joint position vector P 
var('Px','Py','Pz')
P_=matrix([[Px],
           [Py],
           [Pz],
           [1]])

# cartesian position vector Q
var('Qx','Qy','Qz')
Q_=matrix([[Qx],
           [Qy],
           [Qz],
           [1]])

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{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 A 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 A¶

Forward transformation¶

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

\begin{equation} ^QA_P=~ R_a \end{equation}
In [63]:
#define rotation matrix for joint B (ie rotation around the y-axis)
var('Ca','Sa')
Ra=matrix([[ 1,  0,   0,  0],
           [ 0, Ca, -Sa,  0],
           [ 0, Sa,  Ca, 0],
           [ 0,  0,   0, 1]])

display(Math(rf'R_a =~'+latex(Ra)))
$\displaystyle R_a =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & \mathit{Ca} & -\mathit{Sa} & 0 \\ 0 & \mathit{Sa} & \mathit{Ca} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)$

with $~~Sa = sin(\theta_a),~~~ Ca = cos(\theta_a)$ and $\theta_a$ being the angle of rotation of joint A

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=~^QA_P \cdot P $$
In [64]:
# calculate forward kinematic
Q_out=Ra*P_
display(Math(rf'Q =~'+latex(Q_)+rf'=~'+latex(Ra)+rf'\cdot'+latex(P_)+rf'=~'+latex(Q_out)))
$\displaystyle Q =~ \left(\begin{array}{r} \mathit{Qx} \\ \mathit{Qy} \\ \mathit{Qz} \\ 1 \end{array}\right) =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & \mathit{Ca} & -\mathit{Sa} & 0 \\ 0 & \mathit{Sa} & \mathit{Ca} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{r} \mathit{Px} \\ \mathit{Py} \\ \mathit{Pz} \\ 1 \end{array}\right) =~ \left(\begin{array}{r} \mathit{Px} \\ \mathit{Ca} \mathit{Py} - \mathit{Pz} \mathit{Sa} \\ \mathit{Ca} \mathit{Pz} + \mathit{Py} \mathit{Sa} \\ 1 \end{array}\right)$

Inverse transformation¶

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

\begin{equation} ^PA_Q=~ R_a^T \end{equation}

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

\begin{equation} P=~^PA_Q \cdot Q \end{equation}
In [4]:
# calculate inverse kinematic
P_out=Ra.transpose()*Q_
display(Math(rf'P =~'+latex(P_)+rf'=~'+latex(Ra.transpose())+rf'\cdot'+latex(Q_)+rf'=~'+latex(P_out)))
$\displaystyle P =~ \left(\begin{array}{r} \mathit{Px} \\ \mathit{Py} \\ \mathit{Pz} \\ 1 \end{array}\right) =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & \mathit{Ca} & \mathit{Sa} & 0 \\ 0 & -\mathit{Sa} & \mathit{Ca} & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{r} \mathit{Qx} \\ \mathit{Qy} \\ \mathit{Qz} \\ 1 \end{array}\right) =~ \left(\begin{array}{r} \mathit{Qx} \\ \mathit{Ca} \mathit{Qy} + \mathit{Qz} \mathit{Sa} \\ \mathit{Ca} \mathit{Qz} - \mathit{Qy} \mathit{Sa} \\ 1 \end{array}\right)$

Rotary C¶

Forward transformation¶

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

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_c$ also from the right:

\begin{equation} ^QA_P=~ R_a \cdot T_p \cdot R_c \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.

In [40]:
# define joint position matrix
var('Px','Py','Pz')
Tp=matrix([[ 1, 0, 0, Px ],
           [ 0, 1, 0, Py],
           [ 0, 0, 1, Pz],
           [ 0, 0, 0, 1 ]])


# define rotation matrix for axis C (ie rotation around z-axis)
var('Cc','Sc')
Rc=matrix([[ Cc, -Sc,   0, 0],
           [ Sc,  Cc,   0, 0],
           [ 0 ,  0 ,   1, 0],
           [ 0,   0 ,   0, 1]])


display(Math(rf'T_p =~'+latex(Tp)+'~~'+rf'R_c =~'+latex(Rc)))
$\displaystyle T_p =~ \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) ~~ R_c =~ \left(\begin{array}{rrrr} \mathit{Cc} & -\mathit{Sc} & 0 & 0 \\ \mathit{Sc} & \mathit{Cc} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)$

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

In [6]:
# calculate the forward transformation matrix
qAp=Ra*Tp*Rc
display(Math(rf'^QA_P =~~'+latex(Ra)+rf'\cdot'+latex(Tp)+rf'\cdot'+latex(Rc)))
display(Math(rf'^QA_P =~~'+latex(qAp)))
$\displaystyle ^QA_P =~~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & \mathit{Ca} & -\mathit{Sa} & 0 \\ 0 & \mathit{Sa} & \mathit{Ca} & 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} \mathit{Cc} & -\mathit{Sc} & 0 & 0 \\ \mathit{Sc} & \mathit{Cc} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)$
$\displaystyle ^QA_P =~~ \left(\begin{array}{rrrr} \mathit{Cc} & -\mathit{Sc} & 0 & \mathit{Px} \\ \mathit{Ca} \mathit{Sc} & \mathit{Ca} \mathit{Cc} & -\mathit{Sa} & \mathit{Ca} \mathit{Py} - \mathit{Pz} \mathit{Sa} \\ \mathit{Sa} \mathit{Sc} & \mathit{Cc} \mathit{Sa} & \mathit{Ca} & \mathit{Ca} \mathit{Pz} + \mathit{Py} \mathit{Sa} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Note that the tool-position (ie the vector in the 4th column of our transformation matrix $^QA_P$) does not contain the C-rotation in any form. Only the tool-orientation (ie columns 1,2 and 3) is influenced by the addition of the rotation C. This may seem wrong but we have to remember that after the C-rotation ($Rc$) 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 $Rc$). Hence for now a rotation around C simply means that a tool of lenght zero (ie a point) rotates with joint C 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 C so we will move on along the kinematic chain and add the geometric offsets ($D_x,D_y$) , the rotation of joint A ($R_a$) and the pivot-lengths of the spindle head ($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 B and C are arranged in the spindle body.
The image below shows our spindle assembly in its home position ($\theta_b =0$, $\theta_c=0$). To get a clearer idea of the situation a tool (purple cylinder) is shown protruding from the spindle nose.

In [7]:
display(Image(filename="Images/spindle_head_outside.png",
              height=300, width=300))

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

Because the rotation axes B,C and offsets in question are all hidden inside the spindle body we simplify the model to the relevant kinematic components. The white cross with the arrow pointing up shows the intersection of the rotary axes B and C (yellow sphere). This point will be refered to as the 'pivot-point' of the spindle rotary assembly. The rotary axis C (yellow line) extends vertically from the pivot-point in the machine-z direction. The rotary axis B (yellow line) extends from the pivot-point in the machine-y direction towards 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 = 50 and z-pivot = 150. In our kinematic model this represents the situation where, starting from the spindle nose, we need to travel 150 in the positive z-direction and 50 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 -50 in y and -150 in z. However once the definition is made we must keep it throughout the entire process of building the kinematic model.

In [11]:
display(Image(filename="Images/spindle_head_inside.png",
              height=300, width=300))

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.

In [12]:
# define translation matrix for the pivot lengths
var('Ly','Lz')
Tl=matrix([[ 1, 0, 0,  0 ],
           [ 0, 1, 0, -Ly],
           [ 0, 0, 1, -Lz],
           [ 0, 0, 0,  1 ]])
display(Math(rf'T_l =~'+latex(Tl)))
$\displaystyle T_l =~ \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)$

Offsets in the spindle rotary assembly¶

The rotational axes of a rotary assembly like the C/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).
We define the value for the offset in the example image as x-offset = -20. In our kinematic model this represents the situation where, starting from the rotary B axis, we need to travel 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. However once the definition is made we must keep it throughout the entire process of building the kinematic model.

In [14]:
display(Image(filename="Images/spindle_head_dx.png",
              height=300, width=300))

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

In [15]:
# define translation matrix for the pivot lengths
var('Dx')
# offsets in the spindle rotary assembly
Td=matrix([[ 1, 0, 0, -Dx],
           [ 0, 1, 0, 0],
           [ 0, 0, 1, 0],
           [ 0, 0, 0, 1 ]])
display(Math(rf'T_d =~'+latex(Td)))
$\displaystyle T_d =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & -\mathit{Dx} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)$

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:

In [16]:
# define translation matrix for the tool length
var('Dt')
Tt=matrix([[ 1, 0, 0,  0],
           [ 0, 1, 0,  0 ],
           [ 0, 0, 1, -Dt],
           [ 0, 0, 0, 1 ]])
display(Math(rf'T_t =~'+latex(Tt)))
$\displaystyle T_t =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & -\mathit{Dt} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [17]:
Tz=Tl*Tt
display(Math(rf'T_lt =~'+latex(Tl)+rf'\cdot'+latex(Tt)+rf'=~'+latex(Tz)))
$\displaystyle T_lt =~ \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} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & -\mathit{Dt} \\ 0 & 0 & 0 & 1 \end{array}\right) =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & -\mathit{Ly} \\ 0 & 0 & 1 & -\mathit{Dt} - \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Rotary C, B and A with offsets¶

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

$$ ^QA_P = R_a \cdot T_p \cdot R_c \cdot T_d \cdot R_b \cdot T_l \cdot T_t$$
In [44]:
# define rotation matrix for axis B (ie rotation around y-axis)
var('Cb','Sb')
Rb=matrix([[ Cb,  0,  Sb, 0],
           [  0,  1,   0, 0],
           [-Sb,  0 , Cb, 0],
           [  0,  0 ,  0, 1]])

display(Math(rf'R_b =~'+latex(Rb)))
$\displaystyle R_b =~ \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)$
In [46]:
# calculate the forward transformation matrix
# note: the brackets *(Tl*Tt) are only used to get a cosmetically
# prettier result since it seems to keep "(Dt+Lz)"
qAp=Ra*Tp*Rc*Td*Rb*(Tl*Tt)
display(Math(rf'^Q A_P =~'+latex(qAp)))
$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Cb} \mathit{Cc} & -\mathit{Sc} & \mathit{Cc} \mathit{Sb} & -\mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} - \mathit{Cc} \mathit{Dx} + \mathit{Ly} \mathit{Sc} + \mathit{Px} \\ \mathit{Ca} \mathit{Cb} \mathit{Sc} + \mathit{Sa} \mathit{Sb} & \mathit{Ca} \mathit{Cc} & \mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa} & -\mathit{Ca} \mathit{Cc} \mathit{Ly} - \mathit{Ca} \mathit{Dx} \mathit{Sc} - {\left(\mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Py} - \mathit{Pz} \mathit{Sa} \\ \mathit{Cb} \mathit{Sa} \mathit{Sc} - \mathit{Ca} \mathit{Sb} & \mathit{Cc} \mathit{Sa} & \mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb} & -\mathit{Cc} \mathit{Ly} \mathit{Sa} - \mathit{Dx} \mathit{Sa} \mathit{Sc} - {\left(\mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Pz} + \mathit{Py} \mathit{Sa} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [47]:
# Extract the tool-position vector Q (FORWARD KINEMATICS) from
# the fourth column of the forward transformation matrix
Qx=qAp[0][3]
Qy=qAp[1][3]
Qz=qAp[2][3]

display(Math(latex(Q_[0][0]) + rf'~=~'  + latex(Qx)))
display(Math(latex(Q_[1][0]) + rf'~=~'  + latex(Qy)))
display(Math(latex(Q_[2][0]) + rf'~=~'  + latex(Qz)))
$\displaystyle \mathit{Qx} ~=~ -\mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} - \mathit{Cc} \mathit{Dx} + \mathit{Ly} \mathit{Sc} + \mathit{Px}$
$\displaystyle \mathit{Qy} ~=~ -\mathit{Ca} \mathit{Cc} \mathit{Ly} - \mathit{Ca} \mathit{Dx} \mathit{Sc} - {\left(\mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Py} - \mathit{Pz} \mathit{Sa}$
$\displaystyle \mathit{Qz} ~=~ -\mathit{Cc} \mathit{Ly} \mathit{Sa} - \mathit{Dx} \mathit{Sa} \mathit{Sc} - {\left(\mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Pz} + \mathit{Py} \mathit{Sa}$

So with this as our core forward transformation matrix we can now check the output of this model with the spindle assembly in the home position:

$$P(Px,Py,Pz)=(0,0,0)$$$$\theta_a = \theta_b = \theta_c =0 ~~=>~~ (Ca=Cb=Cc=1, ~~Sa=Sb=Sc=0)$$

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

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

Since we started out with all joints in the home position we would expect the resulting coordinate position to be $Q(Qx,Qy,Qz)=(0,0,0)$. Hence we need to compensate for the offsets in our resulting coordinate position 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:

In [65]:
Tc=matrix([[ 1, 0, 0, Dx],
           [ 0, 1, 0, Ly ],
           [ 0, 0, 1, (Dt+Lz)],
           [ 0, 0, 0, 1 ]])
display(Math(rf'T_c =~'+latex(Tc)))
$\displaystyle T_c =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & \mathit{Dx} \\ 0 & 1 & 0 & \mathit{Ly} \\ 0 & 0 & 1 & \mathit{Dt} + \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

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

$$^QA_P = T_c \cdot R_a \cdot T_p \cdot R_c \cdot T_d \cdot R_b \cdot T_l \cdot T_t$$
In [66]:
# calculate the forward transformation matrix
# note: the brackets *(Tl*Tt) are only used to get a cosmetically
# prettier result since it seems to keep "(Dt+Lz)"
qAp=Tc*Ra*Tp*Rc*Td*Rb*(Tl*Tt)
display(Math(rf'^Q A_P =~'+latex(qAp)))
$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Cb} \mathit{Cc} & -\mathit{Sc} & \mathit{Cc} \mathit{Sb} & -\mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} - \mathit{Cc} \mathit{Dx} + \mathit{Ly} \mathit{Sc} + \mathit{Dx} + \mathit{Px} \\ \mathit{Ca} \mathit{Cb} \mathit{Sc} + \mathit{Sa} \mathit{Sb} & \mathit{Ca} \mathit{Cc} & \mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa} & -\mathit{Ca} \mathit{Cc} \mathit{Ly} - \mathit{Ca} \mathit{Dx} \mathit{Sc} - {\left(\mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Py} - \mathit{Pz} \mathit{Sa} + \mathit{Ly} \\ \mathit{Cb} \mathit{Sa} \mathit{Sc} - \mathit{Ca} \mathit{Sb} & \mathit{Cc} \mathit{Sa} & \mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb} & -\mathit{Cc} \mathit{Ly} \mathit{Sa} - \mathit{Dx} \mathit{Sa} \mathit{Sc} - {\left(\mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Pz} + \mathit{Py} \mathit{Sa} + \mathit{Dt} + \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [67]:
# Extract the tool-position vector Q (FORWARD KINEMATICS) from
# the fourth column of the forward transformation matrix
Qx=qAp[0][3]
Qy=qAp[1][3]
Qz=qAp[2][3]

display(Math(latex(Q_[0][0]) + rf'~=~'  + latex(Qx)))
display(Math(latex(Q_[1][0]) + rf'~=~'  + latex(Qy)))
display(Math(latex(Q_[2][0]) + rf'~=~'  + latex(Qz)))
$\displaystyle \mathit{Qx} ~=~ -\mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} - \mathit{Cc} \mathit{Dx} + \mathit{Ly} \mathit{Sc} + \mathit{Dx} + \mathit{Px}$
$\displaystyle \mathit{Qy} ~=~ -\mathit{Ca} \mathit{Cc} \mathit{Ly} - \mathit{Ca} \mathit{Dx} \mathit{Sc} - {\left(\mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Py} - \mathit{Pz} \mathit{Sa} + \mathit{Ly}$
$\displaystyle \mathit{Qz} ~=~ -\mathit{Cc} \mathit{Ly} \mathit{Sa} - \mathit{Dx} \mathit{Sa} \mathit{Sc} - {\left(\mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + \mathit{Ca} \mathit{Pz} + \mathit{Py} \mathit{Sa} + \mathit{Dt} + \mathit{Lz}$

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 A 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.
Let us assume that we have a setup with no TLO ($Dt=0$), no geometric offset ($Dx=0$), no pivot-length ($Ly=Lz=0$) and no offset of the rotation-axis of the rotary A to the machine reference point.
A tool positioned in the center of rotational axis 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 A table is offset from the machine reference point by $(ra_y,ra_z)$ then our joint-position would be equal to $P(0,ra_y,ra_z)$ which would give us a result of $Q(0, ra_y, ra_z)$.
So we need to subtract the offset $(ra_y,ra_z)$ from the joint-position $P=(P_x, P_y-ra_y, P_z-ra_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 $(0, ra_y,ra_z)$.

In [68]:
var('Dray','Draz')
# define offset translation matrix for the rotary c table
Tr=matrix([[ 1, 0, 0,    0 ],
           [ 0, 1, 0, -Dray],
           [ 0, 0, 1, -Draz],
           [ 0, 0, 0,    1 ]])
display(Math(rf'Tr =~'+latex(Tr)))
$\displaystyle Tr =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & -\mathit{Dray} \\ 0 & 0 & 1 & -\mathit{Draz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

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 :

In [69]:
Tpr=Tp*Tr
display(Math(rf'T_p \cdot T_r =~'+latex(Tp)+rf'\cdot'+latex(Tr)+rf'=~'+latex(Tpr)))
$\displaystyle T_p \cdot T_r =~ \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 & 0 \\ 0 & 1 & 0 & -\mathit{Dray} \\ 0 & 0 & 1 & -\mathit{Draz} \\ 0 & 0 & 0 & 1 \end{array}\right) =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & \mathit{Px} \\ 0 & 1 & 0 & -\mathit{Dray} + \mathit{Py} \\ 0 & 0 & 1 & -\mathit{Draz} + \mathit{Pz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Thus we get:

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

However some more consideration is needed for now a joint position of $P(0,ra_y,ra_z)$ will result in a coordinate position of $Q(0,0,0)$ 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(0,rp_y,rp_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:

In [70]:
Tnr=matrix([[ 1, 0, 0, 0],
            [ 0, 1, 0, Dray],
            [ 0, 0, 1, Draz],
            [ 0, 0, 0,  1  ]])
display(Math(rf'T._r =~'+latex(Tnr)))
$\displaystyle T._r =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & \mathit{Dray} \\ 0 & 0 & 1 & \mathit{Draz} \\ 0 & 0 & 0 & 1 \end{array}\right)$

Thus we get our final forward transformation matrix:

$$^QA_P = T_{-r} \cdot T_c \cdot R_a \cdot T_p \cdot T_r \cdot R_c \cdot T_d \cdot R_b \cdot T_l \cdot T_t$$
In [71]:
# calculate the forward transformation matrix
# note: the brackets *(Tl*Tt) are only used to get a cosmetically
# prettier result since it seems to keep "(Dt+Lz)"
# similar for (Tp*Tr)
qAp=Tnr*Tc*Ra*(Tp*Tr)*Rc*Td*Rb*(Tl*Tt)
display(Math(rf'^Q A_P =~'+latex(qAp)))
$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Cb} \mathit{Cc} & -\mathit{Sc} & \mathit{Cc} \mathit{Sb} & -\mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} - \mathit{Cc} \mathit{Dx} + \mathit{Ly} \mathit{Sc} + \mathit{Dx} + \mathit{Px} \\ \mathit{Ca} \mathit{Cb} \mathit{Sc} + \mathit{Sa} \mathit{Sb} & \mathit{Ca} \mathit{Cc} & \mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa} & -\mathit{Ca} \mathit{Cc} \mathit{Ly} - \mathit{Ca} \mathit{Dx} \mathit{Sc} - \mathit{Ca} {\left(\mathit{Dray} - \mathit{Py}\right)} - {\left(\mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Draz} - \mathit{Pz}\right)} \mathit{Sa} + \mathit{Dray} + \mathit{Ly} \\ \mathit{Cb} \mathit{Sa} \mathit{Sc} - \mathit{Ca} \mathit{Sb} & \mathit{Cc} \mathit{Sa} & \mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb} & -\mathit{Cc} \mathit{Ly} \mathit{Sa} - \mathit{Dx} \mathit{Sa} \mathit{Sc} - \mathit{Ca} {\left(\mathit{Draz} - \mathit{Pz}\right)} - {\left(\mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - {\left(\mathit{Dray} - \mathit{Py}\right)} \mathit{Sa} + \mathit{Draz} + \mathit{Dt} + \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [72]:
# Extract the tool-position vector Q (FORWARD KINEMATICS) from
# the fourth column of the forward transformation matrix
Qx=qAp[0][3]
Qy=qAp[1][3]
Qz=qAp[2][3]

display(Math(latex(Q_[0][0]) + rf'~=~'  + latex(Qx)))
display(Math(latex(Q_[1][0]) + rf'~=~'  + latex(Qy)))
display(Math(latex(Q_[2][0]) + rf'~=~'  + latex(Qz)))
$\displaystyle \mathit{Qx} ~=~ -\mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} - \mathit{Cc} \mathit{Dx} + \mathit{Ly} \mathit{Sc} + \mathit{Dx} + \mathit{Px}$
$\displaystyle \mathit{Qy} ~=~ -\mathit{Ca} \mathit{Cc} \mathit{Ly} - \mathit{Ca} \mathit{Dx} \mathit{Sc} - \mathit{Ca} {\left(\mathit{Dray} - \mathit{Py}\right)} - {\left(\mathit{Ca} \mathit{Sb} \mathit{Sc} - \mathit{Cb} \mathit{Sa}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} + {\left(\mathit{Draz} - \mathit{Pz}\right)} \mathit{Sa} + \mathit{Dray} + \mathit{Ly}$
$\displaystyle \mathit{Qz} ~=~ -\mathit{Cc} \mathit{Ly} \mathit{Sa} - \mathit{Dx} \mathit{Sa} \mathit{Sc} - \mathit{Ca} {\left(\mathit{Draz} - \mathit{Pz}\right)} - {\left(\mathit{Sa} \mathit{Sb} \mathit{Sc} + \mathit{Ca} \mathit{Cb}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} - {\left(\mathit{Dray} - \mathit{Py}\right)} \mathit{Sa} + \mathit{Draz} + \mathit{Dt} + \mathit{Lz}$
In [73]:
# expressions as used in xyzabc_trsrn.comp
print('TCP kinematics FORWARD')
print('Qx = ', Qx)
print('Qy = ', Qy)
print('Qz = ', Qz)

TCP kinematics FORWARD
Qx =  -Cc*(Dt + Lz)*Sb - Cc*Dx + Ly*Sc + Dx + Px
Qy =  -Ca*Cc*Ly - Ca*Dx*Sc - Ca*(Dray - Py) - (Ca*Sb*Sc - Cb*Sa)*(Dt + Lz) + (Draz - Pz)*Sa + Dray + Ly
Qz =  -Cc*Ly*Sa - Dx*Sa*Sc - Ca*(Draz - Pz) - (Sa*Sb*Sc + Ca*Cb)*(Dt + Lz) - (Dray - Py)*Sa + Draz + Dt + Lz

   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 = - Cc*(Dt + Lz)*Sb 
                      - Cc*Dx 
                      + Ly*Sc 
                      + Dx 
                      + Px;

        pos->tran.y = - Ca*Cc*Ly 
                      - Ca*Dx*Sc 
                      - Ca*(Dray - Py) 
                      - (Ca*Sb*Sc - Cb*Sa)*(Dt + Lz) 
                      + (Draz - Pz)*Sa 
                      + Dray 
                      + Ly;

        pos->tran.z = - Cc*Ly*Sa 
                      - Dx*Sa*Sc 
                      - Ca*(Draz - Pz) 
                      - (Sa*Sb*Sc + Ca*Cb)*(Dt + Lz) 
                      - (Dray - Py)*Sa 
                      + Draz 
                      + Dt 
                      + Lz; 

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

        break;

Notes:
In this kinematic model the offset of the rotational axis of the rotary A is the vector $(0,Dra_y,Dra_z)$ going from the pivot-point of the spindle assembly to the center of the rotatry-axis A, with the machine in the home position. For this to be correct we need to compensate for any geometric offsets in the YZ plane $(L_y,L_z)$ of the spindle assembly. In other words if the A-rotary-axis has the absolute machine coordinates $(0,Rot_y,Rot_z)$ then $$Dra_y = Rot_y - Ly$$ $$Dra_z = Rot_z - Lz$$ In the kinematic comp this is handled in the variable declaration(see below).

The values for 'Cv', 'Sv', 'Dx',, 'Ly', 'Lz', 'Dray', 'Draz' are constants for a given machine (unless the rotary A is removable).

The values for 'Ca', 'Sa', 'Cb', 'Sb', 'Cc', 'Sc' are recalculated from the rotary joint positions on each cycle and also stored in the respective variables in the component file.

// geometric offsets of the universal spindle head as defined in the ini file
double       Ly = *haldata->y_pivot;
double       Lz = *haldata->z_pivot;
double       Dx = *haldata->x_offset;
double     Dray = *haldata->y_rot_axis - Ly;
double     Draz = *haldata->z_rot_axis - Lz;
double       nu = *haldata->nut_angle; // degrees

// tool-length offset if G43 is used (offset as defined in the tool editor)
double       Dt = *(haldata->tool_offset_z);

// substitutions as used in mathematical documentation
// including degree -> radians angle conversion
double       Sa = sin(j[3]*TO_RAD);
double       Ca = cos(j[3]*TO_RAD);
double       Sb = sin(j[4]*TO_RAD);
double       Cb = cos(j[4]*TO_RAD);
double       Sc = sin(j[5]*TO_RAD);
double       Cc = cos(j[5]*TO_RAD);
double       Sv = sin(nu*TO_RAD);
double       Cv = cos(nu*TO_RAD);
double     CvSa = Cv*Sa;
double     SvSa = Sv*Sa;
double        t = Sv*Cv*(1-Ca);
double        u = Ca + Sv*Sv*(1-Ca);
double        v = Ca + Cv*Cv*(1-Ca);
double      Stc = sin(tc);
double      Ctc = cos(tc);

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

$$^QA_P = [T_{-r} \cdot T_c] \cdot R_a \cdot \{T_p \cdot T_r\} \cdot (R_c \cdot T_d \cdot R_b \cdot T_l \cdot T_t)$$$$^QA_P = [offset_{fo}] \cdot R_a \cdot \{input+offset_{fi}\} \cdot (R_c \cdot T_d \cdot R_b \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 A:

$$^PA_Q = [-offset_{fi}] \cdot R^T_a \cdot \{input-offset_{fo}\} \cdot (R_a \cdot R_c \cdot T_{-d} \cdot R_b \cdot T_{-l} \cdot T_{-t})$$$$^PA_Q = \{ T_{-r}\} \cdot R^T_a \cdot T_q \cdot [T_{r} \cdot T_{-c}] \cdot(R_a \cdot R_c \cdot T_{-d} \cdot R_b \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$:

In [74]:
# define coordinate position matrix
var('Qx','Qy','Qz')
Tq=matrix([[ 1, 0, 0, Qx ],
           [ 0, 1, 0, Qy],
           [ 0, 0, 1, Qz],
           [ 0, 0, 0, 1 ]])
display(Math(rf'T_q =~'+latex(Tq)))
$\displaystyle T_q =~ \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)$
In [75]:
#define inverse translation matrices
Tnc=matrix([[ 1, 0, 0, -Dx],
            [ 0, 1, 0, -Ly],
            [ 0, 0, 1, -(Dt+Lz)],
            [ 0, 0, 0, 1 ]])
display(Math(rf'T._c =~'+latex(Tnc)))

Tnt=matrix([[ 1, 0, 0, 0],
            [ 0, 1, 0, 0 ],
            [ 0, 0, 1, Dt],
            [ 0, 0, 0, 1 ]])
display(Math(rf'T._t =~'+latex(Tnt)))
Tnd=matrix([[ 1, 0, 0, Dx],
            [ 0, 1, 0,  0 ],
            [ 0, 0, 1,  0],
            [ 0, 0, 0, 1 ]])
display(Math(rf'T._d =~'+latex(Tnd)))
Tnl=matrix([[ 1, 0, 0, 0 ],
            [ 0, 1, 0, Ly],
            [ 0, 0, 1, Lz],
            [ 0, 0, 0, 1 ]])
display(Math(rf'T._l =~'+latex(Tnl)))
$\displaystyle T._c =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & -\mathit{Dx} \\ 0 & 1 & 0 & -\mathit{Ly} \\ 0 & 0 & 1 & -\mathit{Dt} - \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$
$\displaystyle T._t =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \mathit{Dt} \\ 0 & 0 & 0 & 1 \end{array}\right)$
$\displaystyle T._d =~ \left(\begin{array}{rrrr} 1 & 0 & 0 & \mathit{Dx} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)$
$\displaystyle T._l =~ \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)$
In [76]:
#qAp=Tnr*Tc*Ra*(Tp*Tr)*Rc*Td*Rb*(Tl*Tt)
pAq=Tnr*Ra.transpose()*(Tq*Tr*Tnc)*Ra*Rc*Tnd*Rb*(Tnl*Tnt)
In [77]:
display(Math(latex(Tq)+rf'\cdot'+latex(Rc.transpose())+rf'\cdot'+latex(Ra.transpose())+rf'\cdot'+latex(Tnl)))
display(Math(rf'^PA_Q=~'+latex(pAq)))
$\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{Cc} & \mathit{Sc} & 0 & 0 \\ -\mathit{Sc} & \mathit{Cc} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} 1 & 0 & 0 & 0 \\ 0 & \mathit{Ca} & \mathit{Sa} & 0 \\ 0 & -\mathit{Sa} & \mathit{Ca} & 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)$
$\displaystyle ^PA_Q=~ \left(\begin{array}{rrrr} \mathit{Cb} \mathit{Cc} & -\mathit{Sc} & \mathit{Cc} \mathit{Sb} & \mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} + \mathit{Cc} \mathit{Dx} - \mathit{Ly} \mathit{Sc} - \mathit{Dx} + \mathit{Qx} \\ {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cb} \mathit{Sc} & {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cc} & {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Sb} \mathit{Sc} & {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} \mathit{Sc} + {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cc} \mathit{Ly} + {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Dx} \mathit{Sc} - \mathit{Ca} {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} - {\left(\mathit{Draz} + \mathit{Dt} + \mathit{Lz} - \mathit{Qz}\right)} \mathit{Sa} + \mathit{Dray} \\ -{\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Sb} & 0 & {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cb} & {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cb} {\left(\mathit{Dt} + \mathit{Lz}\right)} - \mathit{Ca} {\left(\mathit{Draz} + \mathit{Dt} + \mathit{Lz} - \mathit{Qz}\right)} + {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} \mathit{Sa} + \mathit{Draz} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [78]:
# Extract the joint-position vector P (INVERSE KINEMATICS) from
# the fourth column of the inverse transformation matrix
Px=pAq[0][3]
Py=pAq[1][3]
Pz=pAq[2][3]

display(Math(latex(P_[0][0]) + rf'~=~'  + latex(Px)))
display(Math(latex(P_[1][0]) + rf'~=~'  + latex(Py)))
display(Math(latex(P_[2][0]) + rf'~=~'  + latex(Pz)))
$\displaystyle \mathit{Px} ~=~ \mathit{Cc} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} + \mathit{Cc} \mathit{Dx} - \mathit{Ly} \mathit{Sc} - \mathit{Dx} + \mathit{Qx}$
$\displaystyle \mathit{Py} ~=~ {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} {\left(\mathit{Dt} + \mathit{Lz}\right)} \mathit{Sb} \mathit{Sc} + {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cc} \mathit{Ly} + {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Dx} \mathit{Sc} - \mathit{Ca} {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} - {\left(\mathit{Draz} + \mathit{Dt} + \mathit{Lz} - \mathit{Qz}\right)} \mathit{Sa} + \mathit{Dray}$
$\displaystyle \mathit{Pz} ~=~ {\left(\mathit{Ca}^{2} + \mathit{Sa}^{2}\right)} \mathit{Cb} {\left(\mathit{Dt} + \mathit{Lz}\right)} - \mathit{Ca} {\left(\mathit{Draz} + \mathit{Dt} + \mathit{Lz} - \mathit{Qz}\right)} + {\left(\mathit{Dray} + \mathit{Ly} - \mathit{Qy}\right)} \mathit{Sa} + \mathit{Draz}$
In [80]:
# expressions as used in xyzacb_trsrt.comp
print('TCP kinematics INVERSE')
print('Px = ', Px)
print('Py = ', Py)
print('Pz = ', Pz)

TCP kinematics INVERSE
Px =  Cc*(Dt + Lz)*Sb + Cc*Dx - Ly*Sc - Dx + Qx
Py =  (Ca^2 + Sa^2)*(Dt + Lz)*Sb*Sc + (Ca^2 + Sa^2)*Cc*Ly + (Ca^2 + Sa^2)*Dx*Sc - Ca*(Dray + Ly - Qy) - (Draz + Dt + Lz - Qz)*Sa + Dray
Pz =  (Ca^2 + Sa^2)*Cb*(Dt + Lz) - Ca*(Draz + Dt + Lz - Qz) + (Dray + Ly - Qy)*Sa + Draz

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

For notes on the calculation of the various variables in the kinematics component see note above.

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

        j[0] =   Cc*(Dt + Lz)*Sb 
               + Cc*Dx 
               - Ly*Sc 
               - Dx 
               + Qx;

        j[1] =   (Dt + Lz)*Sb*Sc 
               + Cc*Ly 
               + Dx*Sc 
               - Ca*(Dray + Ly - Qy) 
               - (Draz + Dt + Lz - Qz)*Sa 
               + Dray;

        j[2] =   Cb*(Dt + Lz) 
               - Ca*(Draz + Dt + Lz - Qz) 
               + (Dray + Ly - Qy)*Sa 
               + Draz;

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

        break;

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 B 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 C 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 we start at the tool tip and follow the kinematic chain towards the work piece. Since the A-axis in the particular machine used in this paper is not invovled in the tool orientation it will be omitted in the TOOL kinematic model.

Thus our forward transformation matrix $^QA_P$ is :

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

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.
$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 would 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 works in our custom tool kinematic because the tool coordinate system is always aligned with the rotational axis of the tool.

In [84]:
# add c rotation of tool coordinate system
var('Ctc','Stc')
Rtc=matrix([[ Ctc, -Stc,   0, 0],
           [ Stc,  Ctc,   0, 0],
           [ 0 ,  0 ,   1, 0],
           [ 0,   0 ,   0, 1]])
In [85]:
display(Math(
             latex(Rtc.transpose())+rf'\cdot'
            +latex(Tl)             +rf'\cdot'
            +latex(Rb.transpose()) +rf'\cdot'
            +latex(Td)             +rf'\cdot'
            +latex(Rc.transpose()) +rf'\cdot'
            +latex(Tp)             +rf'\cdot'
            +latex(Tnd)            +rf'\cdot'
            +latex(Tnl)              
        ))
$\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{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{Dx} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \cdot \left(\begin{array}{rrrr} \mathit{Cc} & \mathit{Sc} & 0 & 0 \\ -\mathit{Sc} & \mathit{Cc} & 0 & 0 \\ 0 & 0 & 1 & 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 & 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)$
In [86]:
# calculate the forward kinematic 
# brackets are only used for cosmetically adjust the resulting formula
qAp=Rtc.transpose()*(Tl)*Rb.transpose()*Td*Rc.transpose()*(Tp*Tnd*Tnl)                                                              
display(Math(rf'^Q A_P =~'+latex(qAp)))
$\displaystyle ^Q A_P =~ \left(\begin{array}{rrrr} \mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc} & \mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc} & -\mathit{Ctc} \mathit{Sb} & -\mathit{Cb} \mathit{Ctc} \mathit{Dx} - \mathit{Ctc} {\left(\mathit{Lz} + \mathit{Pz}\right)} \mathit{Sb} + {\left(\mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} + {\left(\mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc}\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ly} \mathit{Stc} \\ -\mathit{Cb} \mathit{Cc} \mathit{Stc} - \mathit{Ctc} \mathit{Sc} & -\mathit{Cb} \mathit{Sc} \mathit{Stc} + \mathit{Cc} \mathit{Ctc} & \mathit{Sb} \mathit{Stc} & \mathit{Cb} \mathit{Dx} \mathit{Stc} + {\left(\mathit{Lz} + \mathit{Pz}\right)} \mathit{Sb} \mathit{Stc} - {\left(\mathit{Cb} \mathit{Cc} \mathit{Stc} + \mathit{Ctc} \mathit{Sc}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} - {\left(\mathit{Cb} \mathit{Sc} \mathit{Stc} - \mathit{Cc} \mathit{Ctc}\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ctc} \mathit{Ly} \\ \mathit{Cc} \mathit{Sb} & \mathit{Sb} \mathit{Sc} & \mathit{Cb} & \mathit{Cc} {\left(\mathit{Dx} + \mathit{Px}\right)} \mathit{Sb} + {\left(\mathit{Ly} + \mathit{Py}\right)} \mathit{Sb} \mathit{Sc} + \mathit{Cb} {\left(\mathit{Lz} + \mathit{Pz}\right)} - \mathit{Dx} \mathit{Sb} - \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [87]:
# Extract the tool-position vector Q (FORWARD KINEMATICS) from
# the fourth column of the forward transformation matrix
Qx=qAp[0][3]
Qy=qAp[1][3]
Qz=qAp[2][3]

display(Math(latex(Q_[0][0]) + rf'~=~'  + latex(Qx)))
display(Math(latex(Q_[1][0]) + rf'~=~'  + latex(Qy)))
display(Math(latex(Q_[2][0]) + rf'~=~'  + latex(Qz)))
$\displaystyle \mathit{Qx} ~=~ -\mathit{Cb} \mathit{Ctc} \mathit{Dx} - \mathit{Ctc} {\left(\mathit{Lz} + \mathit{Pz}\right)} \mathit{Sb} + {\left(\mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} + {\left(\mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc}\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ly} \mathit{Stc}$
$\displaystyle \mathit{Qy} ~=~ \mathit{Cb} \mathit{Dx} \mathit{Stc} + {\left(\mathit{Lz} + \mathit{Pz}\right)} \mathit{Sb} \mathit{Stc} - {\left(\mathit{Cb} \mathit{Cc} \mathit{Stc} + \mathit{Ctc} \mathit{Sc}\right)} {\left(\mathit{Dx} + \mathit{Px}\right)} - {\left(\mathit{Cb} \mathit{Sc} \mathit{Stc} - \mathit{Cc} \mathit{Ctc}\right)} {\left(\mathit{Ly} + \mathit{Py}\right)} - \mathit{Ctc} \mathit{Ly}$
$\displaystyle \mathit{Qz} ~=~ \mathit{Cc} {\left(\mathit{Dx} + \mathit{Px}\right)} \mathit{Sb} + {\left(\mathit{Ly} + \mathit{Py}\right)} \mathit{Sb} \mathit{Sc} + \mathit{Cb} {\left(\mathit{Lz} + \mathit{Pz}\right)} - \mathit{Dx} \mathit{Sb} - \mathit{Lz}$
In [88]:
# expressions as used in xyzbca_trsrn.comp
print('TOOL kinematics FORWARD')
print('Qx = ', Qx)
print('Qy = ', Qy)
print('Qz = ', Qz)

TOOL kinematics FORWARD
Qx =  -Cb*Ctc*Dx - Ctc*(Lz + Pz)*Sb + (Cb*Cc*Ctc - Sc*Stc)*(Dx + Px) + (Cb*Ctc*Sc + Cc*Stc)*(Ly + Py) - Ly*Stc
Qy =  Cb*Dx*Stc + (Lz + Pz)*Sb*Stc - (Cb*Cc*Stc + Ctc*Sc)*(Dx + Px) - (Cb*Sc*Stc - Cc*Ctc)*(Ly + Py) - Ctc*Ly
Qz =  Cc*(Dx + Px)*Sb + (Ly + Py)*Sb*Sc + Cb*(Lz + Pz) - Dx*Sb - Lz

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

        pos->tran.x =  - Cb*Ctc*Dx 
                       - Ctc*(Lz + Pz)*Sb 
                       + (Cb*Cc*Ctc - Sc*Stc)*(Dx + Px) 
                       + (Cb*Ctc*Sc + Cc*Stc)*(Ly + Py) 
                       - Ly*Stc;


        pos->tran.y =    Cb*Dx*Stc 
                       + (Lz + Pz)*Sb*Stc 
                       - (Cb*Cc*Stc + Ctc*Sc)*(Dx + Px) 
                       - (Cb*Sc*Stc - Cc*Ctc)*(Ly + Py) 
                       - Ctc*Ly;


        pos->tran.z =    Cc*(Dx + Px)*Sb 
                       + (Ly + Py)*Sb*Sc 
                       + Cb*(Lz + Pz) 
                       - Dx*Sb - Lz;

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

$$^QA_P = R^T_{tc} \cdot T_l \cdot R^T_b \cdot T_d \cdot R^T_c\ \cdot ( T_p \cdot T_{-d} \cdot T_{-l})$$$$^PA_Q = (T_d \cdot T_l ) \cdot R_c \cdot T_{-d} \cdot R_b \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.

In [89]:
# calculate the inverse kinematic 
# brackets are only used for cosmetically adjust the resulting formula
#qAp=Rtc.transpose()*(Tnl)*Rb.transpose()*Tnd*Rc.transpose()*(Tp*Td*Tl)                                                     
pAq=(Td*Tl)*Rc*Tnd*Rb*Tnl*Rtc*Tq

display(Math(rf'^PA_Q =~'+latex(pAq)))
$\displaystyle ^PA_Q =~ \left(\begin{array}{rrrr} \mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc} & -\mathit{Cb} \mathit{Cc} \mathit{Stc} - \mathit{Ctc} \mathit{Sc} & \mathit{Cc} \mathit{Sb} & \mathit{Cc} \mathit{Lz} \mathit{Sb} + \mathit{Cc} \mathit{Qz} \mathit{Sb} + \mathit{Cc} \mathit{Dx} + {\left(\mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc}\right)} \mathit{Qx} - {\left(\mathit{Cb} \mathit{Cc} \mathit{Stc} + \mathit{Ctc} \mathit{Sc}\right)} \mathit{Qy} - \mathit{Ly} \mathit{Sc} - \mathit{Dx} \\ \mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc} & -\mathit{Cb} \mathit{Sc} \mathit{Stc} + \mathit{Cc} \mathit{Ctc} & \mathit{Sb} \mathit{Sc} & \mathit{Lz} \mathit{Sb} \mathit{Sc} + \mathit{Qz} \mathit{Sb} \mathit{Sc} + \mathit{Cc} \mathit{Ly} + {\left(\mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc}\right)} \mathit{Qx} - {\left(\mathit{Cb} \mathit{Sc} \mathit{Stc} - \mathit{Cc} \mathit{Ctc}\right)} \mathit{Qy} + \mathit{Dx} \mathit{Sc} - \mathit{Ly} \\ -\mathit{Ctc} \mathit{Sb} & \mathit{Sb} \mathit{Stc} & \mathit{Cb} & -\mathit{Ctc} \mathit{Qx} \mathit{Sb} + \mathit{Qy} \mathit{Sb} \mathit{Stc} + \mathit{Cb} \mathit{Lz} + \mathit{Cb} \mathit{Qz} - \mathit{Lz} \\ 0 & 0 & 0 & 1 \end{array}\right)$
In [90]:
# Extract the joint-position vector P (INVERSE KINEMATICS) from
# the fourth column of the inverse transformation matrix
Px=pAq[0][3]
Py=pAq[1][3]
Pz=pAq[2][3]

display(Math(latex(P_[0][0]) + rf'~=~'  + latex(Px)))
display(Math(latex(P_[1][0]) + rf'~=~'  + latex(Py)))
display(Math(latex(P_[2][0]) + rf'~=~'  + latex(Pz)))
$\displaystyle \mathit{Px} ~=~ \mathit{Cc} \mathit{Lz} \mathit{Sb} + \mathit{Cc} \mathit{Qz} \mathit{Sb} + \mathit{Cc} \mathit{Dx} + {\left(\mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc}\right)} \mathit{Qx} - {\left(\mathit{Cb} \mathit{Cc} \mathit{Stc} + \mathit{Ctc} \mathit{Sc}\right)} \mathit{Qy} - \mathit{Ly} \mathit{Sc} - \mathit{Dx}$
$\displaystyle \mathit{Py} ~=~ \mathit{Lz} \mathit{Sb} \mathit{Sc} + \mathit{Qz} \mathit{Sb} \mathit{Sc} + \mathit{Cc} \mathit{Ly} + {\left(\mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc}\right)} \mathit{Qx} - {\left(\mathit{Cb} \mathit{Sc} \mathit{Stc} - \mathit{Cc} \mathit{Ctc}\right)} \mathit{Qy} + \mathit{Dx} \mathit{Sc} - \mathit{Ly}$
$\displaystyle \mathit{Pz} ~=~ -\mathit{Ctc} \mathit{Qx} \mathit{Sb} + \mathit{Qy} \mathit{Sb} \mathit{Stc} + \mathit{Cb} \mathit{Lz} + \mathit{Cb} \mathit{Qz} - \mathit{Lz}$
In [91]:
# expressions as used in xyzabc_trsrn.comp
print('TCP kinematics INVERSE')
print('Px = ', Px)
print('Py = ', Py)
print('Pz = ', Pz)

TCP kinematics INVERSE
Px =  Cc*Lz*Sb + Cc*Qz*Sb + Cc*Dx + (Cb*Cc*Ctc - Sc*Stc)*Qx - (Cb*Cc*Stc + Ctc*Sc)*Qy - Ly*Sc - Dx
Py =  Lz*Sb*Sc + Qz*Sb*Sc + Cc*Ly + (Cb*Ctc*Sc + Cc*Stc)*Qx - (Cb*Sc*Stc - Cc*Ctc)*Qy + Dx*Sc - Ly
Pz =  -Ctc*Qx*Sb + Qy*Sb*Stc + Cb*Lz + Cb*Qz - Lz

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

        j[0] =    Cc*Lz*Sb 
                + Cc*Qz*Sb 
                + Cc*Dx 
                + (Cb*Cc*Ctc - Sc*Stc)*Qx 
                - (Cb*Cc*Stc + Ctc*Sc)*Qy 
                - Ly*Sc 
                - Dx;

        j[1] =    Lz*Sb*Sc 
                + Qz*Sb*Sc 
                + Cc*Ly 
                + (Cb*Ctc*Sc + Cc*Stc)*Qx 
                - (Cb*Sc*Stc - Cc*Ctc)*Qy 
                + Dx*Sc 
                - Ly;


        j[2] =  - Ctc*Qx*Sb 
                + Qy*Sb*Stc 
                + Cb*Lz 
                + Cb*Qz 
                - 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 current 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 rotation matrix embedded in 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 basic 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 joints 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.

Handling the optional TWP offsets when switching to TWP¶

Since the optional offset of the origin of the TWP are to be applied before rotating the TWP we will need to either add these offsets to the current WCS before transforming the WCS or transform the WCS and the TWP offsets separatly and add them after.

Calculating the spindle rotary joint positions for a given tool-orientation vector¶

The transformation matrix we derived for the inverse tool kinematic contains a rotation matrix that describes the rotation of the unit vectors of the machine coordinate system (MCS)

\begin{equation} U ~=~ \left(\begin{array}{rrrr} 1, 0, 0 \\ 0, 1, 0 \\ 0, 0, 1 \\ \end{array}\right) ~ Unitvectors~of~the~MCS \end{equation}

to the tool coordinate system (TCS).

\begin{equation} K ~=~ \left(\begin{array}{rrrr} Kx_x, Ky_x, Kz_x \\ Kx_y, Ky_y, Kz_y \\ Kx_z, Ky_z, Kz_z \\ \end{array}\right) ~ Toolvector \end{equation}

To orient the tool perpendicular to the TWP requested by the user we need to find the angles of the spindle rotary joints so that the resulting tool-z vector matches the twp-z vector.

First we extract the tool-z vector from the third column of the transformation matrix in the inverse tool kinematic

In [92]:
# Tool-z orientation vector K 
var('Kz_x','Kz_y','Kz_z')
Kz_=matrix([[Kz_x],
          [Kz_y],
          [Kz_z],
          [1]])
# Tool-x orientation vector J 
var('Kx_x','Kx_y','Kx_z')
Kx_=matrix([[Kx_x],
          [Kx_y],
          [Kx_z],
          [1]])
In [93]:
# Extract the tool-z orientation vector from
# the third column of the inverse transformation matrix
Kx=pAq[0][2]
Ky=pAq[1][2]
Kz=pAq[2][2]

display(Math(latex(Kz_[0][0]) + rf'~=~'  + latex(Kx)))
display(Math(latex(Kz_[1][0]) + rf'~=~'  + latex(Ky)))
display(Math(latex(Kz_[2][0]) + rf'~=~'  + latex(Kz)))
$\displaystyle \mathit{Kz}_{x} ~=~ \mathit{Cc} \mathit{Sb}$
$\displaystyle \mathit{Kz}_{y} ~=~ \mathit{Sb} \mathit{Sc}$
$\displaystyle \mathit{Kz}_{z} ~=~ \mathit{Cb}$

Knowing that:

$$ cos² \theta + sin² \theta~=~ 1 $$

We get $$ Kz_z ~=~ C_b $$

Solving for angle of joint B

$$ \theta_B ~=~acos{Kz_z} $$

Then solving $$ Kz_x= C_cS_b$$

for $C_c$ we get

$$C_c = {Kz_x \over{S_b}}$$

and thus for the angle of joint C: $$ \theta_C = {acos{Kz_x \over{Sb}}} $$

Calculating the angle $\theta_{tc}$¶

To set the direction of the tool-x and thus tool-y orientation we added a virtual rotation around the tool-z vector to our TOOL kinematic model. Without this we would not be able to define the orientation of, say, a rectangluar pocket in the TWP plane requested by the user, instead the orientation would simply follow the rotation of the x-unit-vector of the machine coordinate system as the spindle rotary assembly orients the tool to the TWP.

Our virtual rotation $R_{tc}$ is at the start of our kinematic chain and comes as an additional rotation to the rotations of the spindle rotary assembly (ie $R_a$ and $R_c$ in our example case). In order to rotate the tool-x vector to a given orientation in the TWP plane we will first need to calculate the orientation of the tool-x vector that results from orienting the spindle to the requested TWP plane and then calculate the value for $\theta_{tc}$ required to rotate the tool-x vector to the requested orientation.

As shown above, the rotation of the x-tool vector is precisely described in the first column of the transformation matrix in the inverse TOOL kinematic. Setting our correctional rotation $\theta_{tc} = 0$ and using the joint angles $\theta_{A}, \theta_{C}$ calculated above to orient the tool to the requestd TWP the orientation of the tool-x vector can thus be calculated:

In [94]:
# Extract the the tool-x orientation vector from
# the first column of the inverse transformation matrix

Kx_x=pAq[0][0]
Kx_y=pAq[1][0]
Kx_z=pAq[2][0]

display(Math(latex(Kx_[0][0]) + rf'~=~'  + latex(Kx_x)))
display(Math(latex(Kx_[1][0]) + rf'~=~'  + latex(Kx_y)))
display(Math(latex(Kx_[2][0]) + rf'~=~'  + latex(Kx_z)))
$\displaystyle \mathit{Kx}_{x} ~=~ \mathit{Cb} \mathit{Cc} \mathit{Ctc} - \mathit{Sc} \mathit{Stc}$
$\displaystyle \mathit{Kx}_{y} ~=~ \mathit{Cb} \mathit{Ctc} \mathit{Sc} + \mathit{Cc} \mathit{Stc}$
$\displaystyle \mathit{Kx}_{z} ~=~ -\mathit{Ctc} \mathit{Sb}$

Orient the tool-x vector to lie in a plane parallel to the XY-plane of the machine coordinate system.¶

Since the tool-x vector, should lie in the xy-plane of the of the MCS the z-component of the tool-x vector will need to be equal to zero. Hence we extract the z-component from the tool-x vector and equal that to zero. Then we can solve this equation for $\theta_{tc}$ which is the only undefined variable left.

Whis is

$$ -C_{tc} S_b ~=~ 0$$

The trivial solution is $S_b = 0$ (ie the tool is vertical to the machine-xy plane)

For the more interesting case of $s_b \neq 0$ we get:

$$ \theta_{tc} = {\pi\over2} $$

Orient the tool-x vector to a given vector¶

Using the G62.2 commands the operator can request the tool-x orientation in form of a vector given as a command argument:

\begin{equation} \vec L ~=~ \left(\begin{array}{rrrr} Lx_x \\ Lx_y \\ Lx_z \end{array}\right) ~ Tool-X-vector~requested \end{equation}

To find the required value for $\theta_{tc}$ we first calculate the tool-x orientation using the formulas extracted above. Setting $\theta_{tc} = 0$ and using the joint angles $\theta_{B}, \theta_{C}$ calculated above to orient the tool to the requestd TWP we get the, as yet uncorrected, tool-x vector:

\begin{equation} \vec K ~=~ \left(\begin{array}{rrrr} Kx_x \\ Kx_y \\ Kx_z \end{array}\right) ~ Tool-X-vector~uncorrected \end{equation}

The angle between the two vectors can be calculated using the dot product

$$ \theta_{tc}~ = ~acos{\vec L ~\cdot ~ \vec K \over {|\vec L||\vec K|}} $$

since both our vectors are unit vectors this reduces to

$$ \theta_{tc}~ = ~acos(\vec L ~\cdot ~ \vec K) $$
In [ ]: