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.

Let me know your thoughts.
-automata


 

@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

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
 

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              

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