- Posts: 4819
- Thank you received: 2177
Kinematic model for a 5axis mill with universal (nutating) head
- Aciera
- Offline
- Administrator
-
Less
More
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
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
-
Less
More
- Posts: 25
- Thank you received: 5
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.
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
-
Less
More
- Posts: 25
- Thank you received: 5
2 years 10 months ago - 2 years 9 months ago #289420
by Paul01
# 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)
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
- Offline
- Administrator
-
Less
More
- Posts: 4819
- Thank you received: 2177
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:
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.
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
- Offline
- Administrator
-
Less
More
- Posts: 4819
- Thank you received: 2177
2 years 9 months ago #289593
by Aciera
So maybe try to simply remove those and also the formulas for 'quadrants' 112,122,212 and 222
Replied by Aciera on topic Kinematic model for a 5axis mill with universal (45° nutating) head
I've had some time to look at this again and it seems that this condition is always true.Unfortunately I don't recall how I came up with the conditionals:
if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:
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
-
Less
More
- Posts: 25
- Thank you received: 5
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
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.142 seconds