# Kinematic model for a 5axis mill with universal (nutating) head

31 Dec 2023 09:06 - 04 Jan 2024 11:00 #289420 by Paul01
Warning: Spoiler!
Last edit: 04 Jan 2024 11:00 by Paul01.

31 Dec 2023 11:59 - 31 Dec 2023 12:06 #289427 by Aciera
Ok, the point is that once we derive our formulas for the joint angles (A_j and B_j) using the 3rd column of the tranformation matrix as described in my last post we have something like this.

A_j = acos(2*Kwy - 1)
Sa_j = sin(A_j)
Ca_j = cos(A_j)
B_j = asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j + 2*Sa_j*Sa_j + 2*Ca_j + 1))

If you plug that into the remap and test the simulation you will notice that there are ranges of tool-orientations where the resulting angles are offset to what you need to get the spindle correctly oriented. In some ranges it will fit in others it is the complement of what you need or the you need to change the sign of the value. These 'areas' is what I call 'quadrants' in the remap. A lot of this came through trial and error along the lines of 'from here to here need to add 'pi' and from there to there I need to subtract half a pi and change the sign'.

Unfortunately I don't recall how I came up with the conditionals:
if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:

I'll see if that comes back to me.
Here is a somewhat expanded, later version:

Warning: Spoiler!

NB: clearly some of the comments in the code make no sense. Like this one:
# Kwz <  0 AND Kwz < 0
These are a remnant of the stage where I thought I could keep the quadrants apart by checking whether individual elements of the tool-vector were positive or negative. Alas it's not quite so simple.

Last edit: 31 Dec 2023 12:06 by Aciera.
The following user(s) said Thank You: tommylight

02 Jan 2024 19:59 #289593 by Aciera

Unfortunately I don't recall how I came up with the conditionals:
if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:

I've had some time to look at this again and it seems that this condition is always true.
So maybe try to simply remove those and also the formulas for 'quadrants'  112,122,212 and 222

03 Jan 2024 10:29 #289647 by Paul01
Good to know. The expanded, later version will also help me out too.
It's been a steep learning curve jumping into 5 axis kinematics. Until a month or two ago my machine was only a 3 axis. So it'll take me a bit of time to implement everything correctly.
As a side note, a large portion of the programming I do on the 5 axis machines at my work is just 2D machining on angled faces, so I'm glad TWP's are being worked on in LinuxCNC.
I wonder also if Fusion 360's post can convert tilted work plane operations to XYZABC coordinates and keep the original work coordinate system. This will help me out while I'm working through the 'remap.py' file

03 Jan 2024 14:30 - 03 Jan 2024 17:44 #289660 by Aciera
Just as a disclaimer:
I really don't know much about 5axis machining. The machines I have worked on only had manual swivel heads. However TWP has already proved useful even for that, if only in it's most basic form.

Keep in mind that, on a real machine, consideration needs to be given to the conditioning of the angular values as calculated in the remap since real joints may not be able to turn 360°. I've only made a crude attempt at this.

Implementing, a much simplified version of TWP, has shown that a fair bit of work needs to be invested to make this as user friendly as possible. Mainly properly resetting the controller after an abort while TWP is active needs to be considered.
Since I posted the config, I have removed the TWP offset from the kinematic and included its handling in the remap (mostly). This avoids offsetting G53 (absolute machine coordinate moves) by TWP offset values when TWP is active.
Also, I have invested a lot of work in making the preview aware of TWP and the calculation of limit violations using the preview data.
Last edit: 03 Jan 2024 17:44 by Aciera.

03 Jan 2024 15:48 #289670
Regarding resetting the controller. On a Heidenhain control when you have a G7 work plane rotation, stopping in the middle of a program will leave the work plane tilted and you have to call an empty "G7" with no options to clear it.

Also It will prevent you from calling a G18, G19 etc tool plane while a G7 work plane rotation is active.
The following user(s) said Thank You: Aciera

03 Jan 2024 17:41 - 03 Jan 2024 17:43 #289678 by Aciera
I just updated the openening post with a generic version of the original config. The nutation angle is now settable from 0-90° using a slider in the side panel. Also the documentation has been updated and expanded.

Also It will prevent you from calling a G18, G19 etc tool plane while a G7 work plane rotation is active.

This is a somewhat weak point of using python remaps to introduce new Gcode as Linuxcnc really has no idea about the new functionality  and so we cannot keep the operator from using any Gcodes that may not be compatible with our functionality. All we could do is have a helper component monitoring the python status channel and abort in case of a conflict.
On the other hand if we have an error while executing gcode with TWP active then Linuxcnc will revert to G54 and we will need to deal with things in a custom 'on_abort' file.

Anybody who wants to implement this on a real machine is going to have to spend a lot of thought to avoid 'awkward ' situations. The way it is is really just fit for a simulation configuration.
Last edit: 03 Jan 2024 17:43 by Aciera.
The following user(s) said Thank You: tommylight

04 Jan 2024 10:58 #289752 by Paul01
Extremely impressive update.
The following user(s) said Thank You: Aciera

06 Jan 2024 14:12 #289964 by Aciera
The folder in the opening post now also contains a complete generic config for a machine with a B rotary table and C rotating / A nutating spindle:

##### Attachments:
The following user(s) said Thank You: tommylight, Paul01

12 Jan 2024 01:40 #290466 by Paul01
Hey Aciera,
I have my machine running based on your original post. Now I'm going back to implement TWP's, so I'm using your updated generic-xyzbca files. The only difference is that my A axis is on the XZ plane not the YZ plane. See image attached for reference.

Other than a different rotation matrix for A (and Lx instead of Ly), it's much the same as your files. The r, s, t values are also the same.
However, I've clearly done something wrong in my TWP maths as I'm not getting the correct results, so some help would be much appreciated.

This is what I have for the rotary joints. The C axis generally doesn't rotate correctly to the TWP.

And this is what I have for the Tool-X vector. Also no behaving correctly.

I'm not great with maths so this is a little beyond my expertise.