# Kinematics for XYZAB mill

05 Feb 2023 09:10 #263743 by Aciera
Replied by Aciera on topic Kinematics for XYZAB mill
@ automata

1. Considered rotation point center in XYZ i.e., x-rot-center, y-rot-center, z-rot-center. Here we have also implicitly considered that A-rot-center =0 degrees and B-rot-center = 0 degrees
2. Now the G54 offsets are set such that the work coordinate system zero coincides the kinematics calculations zero point.
3. For forward kinematics what are the reference frames and transformations that are considered?
4. How would we derive the inverse kinematics?

I'm not quite clear on what you mean in points 1 and 2 but in my paper I tried to show how an offset between the machine reference point and the rotation point of the rotary assembly can be dealt with as vector translations of the input values before they are fed into the 'core' transformation matrix and the output values after the 'core' transformation matrix. To work in tool coordinates (eg 'tilted work plane') the active work offset as stored in the numbered parameter list needs to be transformed to the tool coordinate system.

3. As in the paper you linked a solution would be to create the transformation matrix in a 'factory' function that has the basic rotations and translation matrices defined and combines them based on parameter values entered by the user. Each of the twelve kinematic chains would have a set of rotary offset parameters unique to the particular configuration. So the core transformation matrix consists of a transformation matrix (for the internal rotary offsets)  in between two basic rotations plus some translation to the left and right. Additionally each of the twelve configuration would also need to provide the functions to calculate the joint angles for a given tool orientation vector.

4. In my paper I also tried to show that, since we know all the basic rotations and translations that the transformation is built from, we can derive the inverse transformation in exactly the same way as we derive the forward transformation only going from the spindle side to the work side using the transposed rotation matrices and the negative vector translations. In this sense the forward and inverse transformations can be derived 'independently' because we know the kinematic chain.

The resulting forward and inverse transformations could then be passed on to the real time kinematics component in the form of two transformation matrices of 12 values each.
Overall I don't see anything overly difficult in creating such a universal 5-axis kinematic, mind you the devil usually ends up sitting in the details.
The following user(s) said Thank You: rodw

06 Feb 2023 06:17 #263797 by automata
Replied by automata on topic Kinematics for XYZAB mill
@Aciera, awesome work with the complete derivation. It is really helpful for understanding.
The way I am thinking about organization of the kinematics is:
1. Make all 12 Kinematics functions available in the kins module.
2. Make HAL pins for the 3 rotation center offsets and the 3 center offsets available and use only those pins in the equations which are required.
3. We will make an assumption that the Tool length offset will be applied in the Z axis only. This assumption will restrict us to machines that have the tool mounted along the machine Z axis.

First we kick off a generic component for the 4 table-table type of machines AC, BC, BA and AB. We have the kins solved for 3 of them (AC, BC and BA) : . I'll try to derive the last one too and make a generic component for the table table type of machines.

Regarding the equations I have a few questions:
1. I was wondering what the A B words in the Gcode mean in terms of geometry. After reading Section 3 and Section 5.3 of the 5 axis document by Rudy (linuxcnc.org/docs/html/motion/5-axis-kinematics.html) Equations 33, 34 and 35 , I was unbder the impression that the words A and B would represent the vector K somehow.

2. The vector K = [Kx, Ky, Kz] which is used to represent the tool orientation, has only 2 independent components since it is supposed to be a normalized unit vector. So given 2 values (A and B with their respective geometric meaning in terms of the tool vector direction) , we should be able to derive the vecotr K first and then find the joint positions for joint[4] and joint[5].

3. Instead for forward and inverse kinematics in all 3 kinematics components for table-table 5 axis kins we have:
github.com/LinuxCNC/linuxcnc/blob/master...b_tdr_kins.comp#L190
github.com/LinuxCNC/linuxcnc/blob/master...b_tdr_kins.comp#L251
github.com/LinuxCNC/linuxcnc/blob/master...tics/trtfuncs.c#L181
github.com/LinuxCNC/linuxcnc/blob/master...tics/trtfuncs.c#L232
github.com/LinuxCNC/linuxcnc/blob/master...tics/trtfuncs.c#L285
github.com/LinuxCNC/linuxcnc/blob/master...tics/trtfuncs.c#L332

4. This shows that the ABandC words on the gcode line are directly taken as the respective joint positions in degrees.

5. The implication of using the ABC words directly as the joint positions, would mean that the CAM system needs to have knowledge of the form of Equation 32 in the document: linuxcnc.org/docs/html/motion/5-axis-kinematics.html. This necissitates writing a post processor for each machine type instead of directly using CLData (cutter location data) like the nci file from mastercam.

6. I was hoping to decouple the CAM system from the machine and directly work on the CLData making the need for a postprocessor redundant.

-automata

06 Feb 2023 10:19 - 06 Feb 2023 10:24 #263809 by Aciera
Replied by Aciera on topic Kinematics for XYZAB mill
1. In the 'trt' and my 'tdr' kinematics the joint position and the coordinate position for the rotaries are directly linked:
```            pos->a      = j[3];
pos->b      = j[4];```
and
```            j[3] = pos->a;
j[4] = pos->b;```
So, as you noticed, the A and B words in the Gcode are the joint positions in degrees.
The tool orientation vector K in Rudy's document is not utilized in either 'trt' or 'tdr' kinematics. These examples are basically somewhat 'dumb' in that the TCP function is purely CAM dependent.
They are still usable for 3+2 operation because the tool remains oriented along the z axis regardless of the rotary position but for that one doesn't need any custom kinematic.

2. If you want to use tool-vector command then you need to calculate the joint positions in the kinematics which is what my 'tilted work plane' needs as well.

5. Exactly, TCP mode is only as useful as the CAM behind it.

6. For this the controller needs to be able to calculate the rotary joint angles required to orient the tool to the commanded tool orientation vector on a given machine kinematic. Note that here lies the real work because the controller needs to handle any ambiguities (singularities) the machine kinematic might have. This is much more complex than the TCP kinematics currently implemented in the 'trt' and 'tdr' kinematics. I find it rather difficult to find information on the math postprocessors use for this task.

Last edit: 06 Feb 2023 10:24 by Aciera.

06 Feb 2023 14:07 #263830 by automata
Replied by automata on topic Kinematics for XYZAB mill
@aciera,
Thanks for the prompt response.
For interpolation of Rotation matrices, we can refer to this video by Prof. P. Corke: robotacademy.net.au/lesson/interpolating-rotation-in-3d/

Specifying Gcode point XYZ[ABC] means we are expecting a linear/circular interpolation between the start point and end point. Orientation CANNOT be interpolated using orientation vectors. However orientation can be interpolated once we convert it to angles using RPY or Eulers angles. The problem now is that the definition of linear interpolation also varies based on which Euler system or RPY or other transformation you have chosen to use (in connivance with the CAM system).
Further the video explains how Quaternion representation, in the general case, provides the shortest path for the interpolation.

Look at CLData vocabulary from Generative Machining ASCII CLDATA Output Specification: bdml.stanford.edu/twiki/pub/Manufacturin...nfo/NormesClfile.pdf
This is similar to ISO 4343 standard which specifies a standard for machining language commands.

The Multixis GOTO command is specified as 3 coordinates of XYZ position + 3 coordinates orientation vector along the tool axis.
Now you cannot interpolate a vector like we do linear interpolation on the XYZ position.
The motion planner in Linuxcnc interpolates the 9 coordinates it is given as axis positions.

So when we are supplied the XYZ,IJK data like in ISO4343, we first need to calculate the ABC axes positions from the XYZIJK data and then only can those points XYZABC be forwarded to the motion planner for interpolation.

If we do not adhere to the above procedure, we will need to take support from the CAM system and compute the ABC angles in the post processor by giving the CAM system adequate information about the machine including the kinematics type and the offsets. This means we need to take extra support from the CAM and it also means that to specify a sufficiently accurate SWARF tool path, we need to break the path into smaller and smaller segments.

6. For this the controller needs to be able to calculate the rotary joint angles required to orient the tool to the commanded tool orientation vector on a given machine kinematic. Note that here lies the real work because the controller needs to handle any ambiguities (singularities) the machine kinematic might have. This is much more complex than the TCP kinematics currently implemented in the 'trt' and 'tdr' kinematics. I find it rather difficult to find information on the math postprocessors use for this task.

The computation from tool orientation vector to RPY or eulers angles is trivial. Ensuring that the shortest path taken during interpolation does not generate any singularity may be somewhat of a challenge but it is still solvable in the kinematics. This type of singularity is called "gimble lock" and is inherent in ANY 3 angle representation of a rotation matrix. This video gives an excellent explaination of this type of singularity: robotacademy.net.au/lesson/singularity-i...ion-angle-sequences/
The way to avoid it is stay away from the orientation where the second rotation angle is close to pi/2. In robotics, we avoid it by making the definition of the angles from rotation matrix have the second rotation angle = pi/2 point in a least used direction of the robot (i.e., upwards).
We could do the same for our machine tool angles from the tool orientation vector! Tools generally don't point in the Z+ direction (they point in Z- direction). So we can use XZX Euler angles for representing the orientation. I am trying to read up on this to be able to better articulate what I am thinking.
Will appreciate any comments on my thought process for trying to eliminate CAM post processor and also ensure we can do 5 axis simultaneous machining with Linuxcnc
-automata

06 Feb 2023 17:08 #263851 by Aciera
Replied by Aciera on topic Kinematics for XYZAB mill
Ok, I think i see where you are headed now. With tilted work planes I'm still in the realm of userspace python remaps to orient the tool to the requested orientation and then use tool kinematics to move the tool perpendicular to that work plane using standard gcode and canned cycles. So it is basically geared to 3+2 axis operation without needing a CAM for everything.
What you are proposing is TCP kinematics with vector programming. That requires realtime postprocessing functions to find and interpolate joint positions. I also think that would require a customized interpreter on top of everything else.
Certainly very interesting but WAY beyond my horizon. I'm already struggling to figure out how I can orient the tool of my rotary/nutating sim config parallel to the primary rotary without my python code crashing.

06 Feb 2023 18:37 #263857 by automata
Replied by automata on topic Kinematics for XYZAB mill
Hi aciera,
I'll put in my 0.02 cents to this G68.2 TWP issue. If i am reiterating what you already know, please pardon my pedagogy.
For a table-table 5 axis machine, the problem can be simplified to finding the 2 angles of the 2 rotary joints such that the XYZ axis of the work co-ordinate frame coincide with machine XYZ axes... You first align the plane normal to the machine Z axis and then you still have to choose the XY orientation to align with the machine axes. We still have to choose the origin point of the new plane.
For the general case, which includes nutating head as well as head- table type machines, we need to find the plane normal vector.
Assume we are going to use the 3 point method to specify the plane and the points are P1 P2 P3 and they are not collinear.
Calculate the plane normal Zn as the cross product of the vectors P1-P2, P1- P3.
​​​Now you still need to select the Xn axis direction. Point it as P1-P2 ( random choice).
Now you have Xn and Zn axis of the work co-ordinate system defined. Normalise the Xn and Zn to unit vectors.
Calculate Yn as the cross product of Xn and Zn.
Rotation Matrix R is [Xn Yn Zn]
Assuming P1 is the origin of the new work co-ordinate system ( random choice), homogenous transformation of the new coordinate system isH =  [Xn Yn Zn P1]
[ 0.   0.   0.  1]
H is a 4x4 Matrix.
Note that P1 should lie on the new plane.

Given rotation Matrix R,  can we calculate the A and C angles of the nutating head? If yes set the A and C angles to the joints. If not we need some more work to get A and C.

​​​​​​In the kinematics code, use H inverse to calculate the XYZ positions asP = H-1• PnH inverse is simple to calculateH-1   = [RT -RT P1]
.           [ 0 0 0  1]
My lecture notes referenced from a few messages ago have the exact description on the derivation.Can we try this in simulation with some simple cases?
-automata

06 Feb 2023 19:07 #263860 by Aciera
Replied by Aciera on topic Kinematics for XYZAB mill
No problem at all, nothing wrong with sharing ideas.
My tilted work plane remap works just as you described. With G68.2 and it's different modes the user defines a tool z-axis and x-axis. The custom tool kinematic is then used to calculate the joint positions and the tool is then oriented using a g53.x command. Still needs some work to get the details working though. As soon as it's halfway presentable I'll share it in the other thread.

07 Feb 2023 15:04 #263944 by automata
Replied by automata on topic Kinematics for XYZAB mill
hi Aciera,
This is in continuation of my idea for separating the machine kinematics from the CAM system and have the CAM system specify the CLData with the tool orientation vector independent of the machine kinematics.
From the Fanuc 5 axis document I shared on the tilted working plane thread (forum.linuxcnc.org/20-g-code/47984-tilted-working-plane#263524, pdfcoffee.com/qdownload/fanuc-5-axis-pdf-free.html ) , G43.4 and G43.5 commands are used for different methods of specifying the tool orientation when using 5 axis commands.
When using G43.4, the ABC words in the further G0G1G2G3 lines mean the position of the rotary axes. The tdr and trt kinematics from linuxcnc will work fine when using this type of output from the CAM system.
When using G43.5, the IJK words in the further G0G1G2G3 lines mean the tool orientation vector specification as explained by Rudy in  linuxcnc.org/docs/html/motion/5-axis-kinematics.html . From the definition, it looks to me that the tool orientation vector needs to be normalized. A better explaination for G43.5 is available at www.linkedin.com/pulse/fanuc-g435-rtcp-t...-5-axis-tim-markoski by Tim Markoski.

Using G43.4, we will still need support from the CAM system for the kinematics where the CAM will resolve the Angles ABC from the tool orientation data.

For  becoming truly independent of the CAM system for kinematics support, we will need to implement G43.5 where the tool orientation vector will be programmed into the Gcode motion line. For that we will have to allow IJK values in G0G1 moves (maybe somehow repourpose the IJK words in G2 G2 moves too OR allow G2G3 with R format arcs only when in G43.5 mode) and compute the ABC from the tool orientation vector in IJK. Then interpolate the ABC with the regular XYZ words.
Note that we can do the IJK to ABC conversion in a "pre-processor" using a filter program which has access to the machine kinematics that are available on the HAL fabric.

The main physics challenge I see is that there is no easy way to specify linear interpolation for rotation angles using Euler angles (except using Quaternions). So we will always have to slice the gcode lines to smaller and smaller resolutions to meet the linearity criteria for angles just like we can approximate any smooth curve with a polyline (series of small G1 moves). But this is a problem literally everyone has to contend with.

I would appreciate any comments on my line of thinking.

Regards,
-automata

07 Feb 2023 15:58 #263952 by Aciera
Replied by Aciera on topic Kinematics for XYZAB mill
Vector programming gives you independence from the postprocessor but you still need a CAM that calculates the tool path and orientation. Hence why I'm more interested in 3+2 and tilted work planes that can be used for many or even most parts machined on a 5axis machine. The only real benefit of vector programming is that the controller does it's own postprocessing and the same program can be used on different controllers. (I used to think that the controller does this in real time while it runs the program but maybe it actually does it when the program is loaded?)
For interpolation one would want to do the calculations with quaternions, also less hassle with gimbal-lock. Building a custom interpreter for linuxcnc doesn't seem to be a terribly difficult thing to do.
The main challenge in my eyes would be to come up with an algorithm that guarantees smooth interpolation of tool vector changes. In my sim with the A/B rotation/nutation spindle head there can be different solutions for a given tool-vector. If the tool is to be oriented along the machine y (ie along the rotary B axis) then there are an infinite number of solutions since the B axis can be in any position.
My guess is that there are numerical algorithms that can be checked for convergence but I really have no idea and as I noted before I have not found a good paper about the mathematical methods used for postprocessing.

13 Feb 2023 10:21 #264413 by automata
Replied by automata on topic Kinematics for XYZAB mill
Hi,
I wanted to run my understanding of the Kinematics module with the community and maybe get some more insight into the organization of the kinematics modules.
The question I have I how does Linuxcnc accommodate G5x, G43 and G92 offsets in the final position values given to the kinematics module?
1. G5x, G92 and G43 offsets are applied and stored in the Gcode interpreter. The task and planner modules have no knowledge of the offset values.
2. The Task (milltask) module will take axis values from the Gcode module in what it thinks is Machine Coordinate System (MCS). Let us call this coordinate system as the "Assumed" MCS (AMCS)
3. Trajectory planning is done by the realtime motion component and it is done in the AMCS space.
3. The kinematics module using switchkins assumes the G5x coordinate frame has its origin at the rot_point offset which is given in the original machine MCS coordinate frame. This is done by setting the G54 offset to the rotation center point at the start of the Gcode in the example NGC file made by Aciera in the XYZAB sim config.
4. The Tool length offset is applied along the Z axis of the workpiece by the Gcode interpreter.
5. The Inverse Kinematics function calculates the joint positions from the assumed AMCS positions handed to it via the motion/trajectory planner module.

The formula used by the XYZBC inverse kins is:
```qx = pos->tran.x - x_rot_point - dx;
qy = pos->tran.y - y_rot_point;
qz = pos->tran.z - z_rot_point - dz - dt;

j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz+ x_rot_point;
j[1] = ca*qy + sa*qz + y_rot_point;
j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz + z_rot_point + dt;
j[3] = pos->a;
j[4] = pos->b;```
My inferences are as follows:
1. First we corrected the assumption that the Gcode module made about the G5x and G43 and AB axes center offsets and substracted them from the AMCS. This aligned the center of the workpiece coordinate system to the rotation center.
2. Then we orient the the workpiece coordinate frame to align the axes with the original MCS.
3. Then reapply the G5x offset along the MCS axes and the tool Z offset along the J2 axis.

My Questions:
1. We compensated for G5x and G43 offsets in the inverse kinematics. What about G92 offset? OR does that not need to be compensated in this way? I am comming up with some mechanism to check the G92 offset in the sim config.
2. The G43 offset is compensated only along the Z axis. What if the G43 offset is applied along other axes too? For this also I am going to do some trials on the sim config
3. If we have to remove rotate and re-apply the G5x and tool offsets in the inverse kinematics, can we say that there is an inherent flaw in the method of inverse kinematics and the location of application (gcode interpreter) of offsets in Linuxcnc? Should the offsets be handled in the kinematics module?
4. The position velocity and acceleration limits being applied to the axes by the motion planner will be very poor approximations of the correct values (which aer measured along the joints). Although joints-axes separation from V2.8 onwards does address this issue to a certain extent, the velocity and acceleration limits should be handled in a more refined way?

I would appreciate any comments and will surely help in furthering my understanding of the kinematics moduleand overall structure of LinuxCNC.
-automata