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. 

@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


 

1. In the 'trt' and my 'tdr' kinematics the joint position and the coordinate position for the rotaries are directly linked:
and 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.


 

@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

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.

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