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

Aciera
Topic Author
Offline
Administrator

2 years 10 months ago #289379 by Aciera

Replied by Aciera on topic Kinematic model for a 5axis mill with universal (45° nutating) head

Correction:
~~with the substitution for 'v' we can then solve for Sa and thus for the angle of joint A~~

with the substitution for 'v' we can then solve for Ca and thus for the angle of joint A

The following user(s) said Thank You: tommylight

Please Log in or Create an account to join the conversation.

Paul01
Offline
Junior Member

2 years 10 months ago #289419 by Paul01

Replied by Paul01 on topic Kinematic model for a 5axis mill with universal (45° nutating) head

Thanks Aciera,

Your help means a lot. I wouldn't know how to do any of this on my own.

I've been trying to understand the if-else statements in 'remap.py' for the tool-orientation vector by substituting known values into the calculations, but my math is a little lacking so I still don't fully understand how the calculations work. And I presume I will need to change them if I have a 55° nutating head. For example, if Kwy = 1, A_j = pi and B_j = 0 will only work on a 45° nutating head.

The part of the calculations I understand the least are the parts of the if-else statements similar to this:

if -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_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)))

A bit of an explanation about how this works would be great. I'll post the full section of code below.

The '-(tctc-vstc)cb-(cactc-tstc)sb=0' did confuse me for a second, but there was a note in the file stating it referred to row 3, column 1 of the inverse tool kinematic. It was easy enough to understand after this.

Please Log in or Create an account to join the conversation.

Paul01
Offline
Junior Member

2 years 10 months ago - 2 years 9 months ago #289420 by Paul01

Replied by Paul01 on topic Kinematic model for a 5axis mill with universal (45° nutating) head

Warning: Spoiler!

# returns the A/B joint positions for a given tool-orientation vector
# kinematics specific
def kins_calc_jnt_angles(self, tool_z_req):
epsilon = 0.000001
print("Vector tool-z-requested",tool_z_req)
# using xyzabc-trsrn-og kinematics (ALL ANGLES ARE IN RADIANS)
(Kwx, Kwy, Kwz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2])

# in this particular kinematic there are infinite solutions for tool_z vector =[0,1,0]
# so we need to check if the requested tool_z vector is sufficiently close to it
if Kwy > 1 - epsilon:
A_j = pi # note: we can choose the rotation direction of the joint with the sign (+/-pi)
B_j = 0
return(A_j, B_j)

# note: handling of individual quadrants is required
if Kwx >= 0:
A_j = pi - acos(2*Kwy - 1)
Sa_j = sin(A_j)
Ca_j = cos(A_j)
if Kwz >= 0:
if -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_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)))
quad = 111
else: # Kwz < 0 AND Kwz < 0
B_j = (pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
+ 2*Sa_j*Sa_j + 2*Ca_j + 1)))
quad = 112
else: # Kwz < 0
if -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_j:
B_j = (pi/2 - asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
+ 2*Sa_j*Sa_j + 2*Ca_j + 1)))
quad = 121
else: # Kwz < 0 AND Kwz < 0
B_j = (pi - asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
+ 2*Sa_j*Sa_j + 2*Ca_j + 1)))
quad = 122

else: # Kwx < 0
A_j = - pi + acos(2*Kwy - 1);
Sa_j = sin(A_j);
Ca_j = cos(A_j);
if Kwz >= 0:
if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_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)))
quad = 211
else:
B_j = (- pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
+ 2*Sa_j*Sa_j + 2*Ca_j + 1)))
quad = 212
else:
if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:
B_j = (-pi/2 - asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
+ 2*Sa_j*Sa_j + 2*Ca_j + 1)))
quad = 221
else:
B_j = (- pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
+ 2*Sa_j*Sa_j + 2*Ca_j + 1)))
quad = 222
#if Kwz < 0:
# B_j = -B_j
# return(A_j, B_j)

print("QUADRANT: ",quad)
# return angles in radians
return(A_j, B_j)

Last edit: 2 years 9 months ago by Paul01.

Please Log in or Create an account to join the conversation.

Aciera
Topic Author
Offline
Administrator

2 years 10 months ago - 2 years 10 months ago #289427 by Aciera

Replied by Aciera on topic Kinematic model for a 5axis mill with universal (45° nutating) head

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!

Code:

# returns the A/B joint positions for a given tool-orientation vector
# NOTE: These formulas are specific to the machine
def kins_calc_jnt_angles(self, tool_z_req):
    epsilon = 0.000001
    print("Vector tool-z-requested",tool_z_req)
    # using xyzabc-trsrn kinematics (ALL ANGLES ARE IN RADIANS)
    (Kwx, Kwy, Kwz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2])

    # on this particular machine the tool cannot be oriented if the requested tool-z vector
    # has a negative y-component (ie Kwy)
    if Kwz > 1 - epsilon:
        return (0 ,0)

    elif Kwy < 0:
        print("Error -> Tool-z-requested is outside of this kinematic, aborting 'kins_calc_jnt_angles()'")
        return(None, None)

    # in this particular kinematic there are infinite solutions for tool_z vector =[0,1,0]
    # so we need to check if the requested tool_z vector is sufficiently close to it
    elif Kwx > 1 - epsilon:
        A_j = 0
        B_j = pi/2
        return(A_j, B_j)

    elif Kwx < -1 + epsilon:
        A_j = 0
        B_j = -pi/2
        return(A_j, B_j)

    elif Kwy > 1 - epsilon:
        A_j = pi  # note: we can choose the rotation direction of the joint with the sign (+/-pi)
        B_j = 0
        return(A_j, B_j)

    elif Kwz < -1 + epsilon:
        A_j = 0
        B_j = pi  # note: we can choose the rotation direction of the joint with the sign (+/-pi)
        return(A_j, B_j)

    # note: handling of individual quadrants is required
    if Kwx >= 0:
        A_j = pi - acos(2*Kwy - 1)
        Sa_j  = sin(A_j)
        Ca_j  = cos(A_j)
        if Kwz >= 0:
            if  -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_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)))
                quad = 111
            else: # Kwz <  0 AND Kwz < 0
                B_j = (pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 112
        else: # Kwz < 0
            A_j =  pi + acos(2*Kwy - 1);
            Sa_j  = sin(A_j);
            Ca_j  = cos(A_j);
            if  -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_j:
                B_j = (pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 121
            else: # Kwz <  0 AND Kwz < 0
                B_j = (pi - asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 122

    else: # Kwx < 0
        A_j = - pi + acos(2*Kwy - 1);
        Sa_j  = sin(A_j);
        Ca_j  = cos(A_j);
        if Kwz >= 0:
            if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_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)))
                quad = 211
            else:
                B_j = (- pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 212
        else:
            A_j = - pi - acos(2*Kwy - 1);
            Sa_j  = sin(A_j);
            Ca_j  = cos(A_j);
            if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:
                B_j = (-pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 221
            else:
                B_j = (- pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 222
    #if Kwz < 0:
    #    B_j = -B_j
    #    return(A_j, B_j)


    print("QUADRANT: ",quad)
    # return angles in radians
    return(A_j, B_j)

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: 2 years 10 months ago by Aciera.

The following user(s) said Thank You: tommylight

Please Log in or Create an account to join the conversation.

Aciera
Topic Author
Offline
Administrator

2 years 9 months ago #289593 by Aciera

Replied by Aciera on topic Kinematic model for a 5axis mill with universal (45° nutating) head

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

Please Log in or Create an account to join the conversation.

Paul01
Offline
Junior Member

2 years 9 months ago #289647 by Paul01

Replied by Paul01 on topic Kinematic model for a 5axis mill with universal (45° nutating) head

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

Please Log in or Create an account to join the conversation.

Time to create page: 0.072 seconds

Forum Menu

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