Working on tower lean - could use some trig help!
Posted: Sat Mar 28, 2015 4:03 am
I'm working on an AI calibration system for linear delta printers (Rostock and all variants) that runs on Smoothie firmware. So far, the algorithm manages 14 variables: endstops, arm length, delta radius, individual tower distance/rotation offsets, and build surface normal. The routine uses a machine learning algorithm called simulated annealing to allow the variables to slowly "creep" into their most likely values, given measurements taken by probing the print surface. It has produced significant improvements in calibration on my printer and others.
I've found that as I give the annealer more variables to mess with, the better the result it produces. However, the results - while good - show that there is still more work to be done. I've already given it almost every variable I can think of, except for one: Tower lean. The firmware assumes each tower is exactly perpendicular (90 degrees) relative to the build surface, but unless your printer was assembled by robots, this is probably not the case. Furthermore, the lean, even if small, can have a significant impact on positional accuracy. If you move the effector out from the center to the edge, all the carriages have to move, and one may have to move as much as ~140mm. Multiply that by the lean of the towers, and even if the lean is small, you're still multiplying that by up to 140mm just on one tower! Your carriages may wind up some percent of a millimeter off from where the printer thinks they are. Therefore, tower lean seems like the logical choice for the next variable type to add to the annealer. If there are improvements to be found by compensating for tower lean, doing so should help significantly.
That brings us to the trig part!
The idea I had was to store a unit vector containing the lean of the tower relative to its origin, which is one of its endpoints (either the bottom or the top of the tower). To get the true { x, y, z } of the carriage, you take its elevation (distance from its origin), multiply that by the unit vector, and you get your true coordinates. That's pretty simple, and it works fine for the forward kinematics (effector X,Y,Z given carriage elevations) because we already have the elevations.
However, it doesn't work for the inverse kinematics (carriage elevations given effector X,Y,Z) because you have to already know the elevation (distance from the carriage's origin) before you can get the carriage's true XYZ. In fact, it throws off the whole equation pretty soundly.
Here's the regular version, without any math for tower lean:
When we ask the printer to move the effector, the inverse kinematics solver uses the last of those equations to figure out the carriage positions necessary to push the effector to the given coordinates. It assumes that the elevation, distance, and arm length are all legs of a right triangle that lives on a plane that's exactly perpendicular to the build surface. Functionally, this describes a sphere whose coordinates could be traced out by the effector's hinge, bounded by whatever range of motion the carriage's hinge allows. The effect of doing this with three towers is that you have three spheres with the same radii, and moving individual spheres up or down causes their convergence - where the effector is - to move around.
In the real world, the triangle may be either right or scalene. Moreover, unless your printer was built by an expensive robot with perfect accuracy, it is scalene. The plane of the triangle is not perpendicular to the build surface. Additionally, the trick of only calculating the carriage positions for the { X, Y } coordinates, and then adding Z to all three carriage positions, no longer works because the towers aren't exactly vertical. You might add 100mm of Z, but only get (for example) 98.8mm above the surface, because the tower just isn't straight up and down!
In the diagram above, the Dist variable is measured from the carriage if it was at the effector's height, to the effector. This works because a towers's XY is always the same, at every elevation. To do this properly in a robot where the tower's XY changes with elevation because of lean, we can make no assumption about whether the carriage is at the same XY at effector height that it would be at carriage height. Therefore, we would instead have to measure Dist as the horizontal distance between the carriage's elevation on the tower to the effector, and use a different variable to describe the true vertical distance from the carriage hinge to the effector hinge, which I call ZTrue:
Now, here's the trouble. I already know arm length (AL). If I knew Dist, I could figure out that ZTrue = sqrt(Dist^2 + AL^2). However, I cannot know Dist until I know ZTrue. Likewise, I cannot know ZTrue until I know Dist.
I don't know how to continue, and I've been researching this for most of the last 12 hours, so my brain is totally fried. What's the next step? How do I find the elevation of a sphere along an arbitrarily leaning tower, such that the distance from the center of the sphere to a point at radius AL (arm length) is equal to the horizontal distance between the sphere center and the desired effector hinge location? (Is that even the right question?)
I've found that as I give the annealer more variables to mess with, the better the result it produces. However, the results - while good - show that there is still more work to be done. I've already given it almost every variable I can think of, except for one: Tower lean. The firmware assumes each tower is exactly perpendicular (90 degrees) relative to the build surface, but unless your printer was assembled by robots, this is probably not the case. Furthermore, the lean, even if small, can have a significant impact on positional accuracy. If you move the effector out from the center to the edge, all the carriages have to move, and one may have to move as much as ~140mm. Multiply that by the lean of the towers, and even if the lean is small, you're still multiplying that by up to 140mm just on one tower! Your carriages may wind up some percent of a millimeter off from where the printer thinks they are. Therefore, tower lean seems like the logical choice for the next variable type to add to the annealer. If there are improvements to be found by compensating for tower lean, doing so should help significantly.
That brings us to the trig part!
The idea I had was to store a unit vector containing the lean of the tower relative to its origin, which is one of its endpoints (either the bottom or the top of the tower). To get the true { x, y, z } of the carriage, you take its elevation (distance from its origin), multiply that by the unit vector, and you get your true coordinates. That's pretty simple, and it works fine for the forward kinematics (effector X,Y,Z given carriage elevations) because we already have the elevations.
However, it doesn't work for the inverse kinematics (carriage elevations given effector X,Y,Z) because you have to already know the elevation (distance from the carriage's origin) before you can get the carriage's true XYZ. In fact, it throws off the whole equation pretty soundly.
Here's the regular version, without any math for tower lean:
Code: Select all
.<--- Carriage
|\
| \
| \ AL = Arm Length
| \ | = Elevation = Vertical distance from effector's hinge to carriage's hinge (the vertical axis in this diagram)
| \
.-----.<--- Effector
Dist
Because of an old dude named Pythagoras, we know the following:
Elev = sqrt(AL^2 + Dist^2)
AL = sqrt(Dist^2 + Elev^2)
Dist = sqrt(Elev^2 + AL^2)
Dist^2 = (X - twr[X])^2 + (Y - twr[Y])^2 = AL^2 - Elev^2
Elev = sqrt( AL^2 - (cartesian[X]-twr[X])^2 - (cartesian[Y]-twr[Y])^2 ) + cartesian[Z]
In the real world, the triangle may be either right or scalene. Moreover, unless your printer was built by an expensive robot with perfect accuracy, it is scalene. The plane of the triangle is not perpendicular to the build surface. Additionally, the trick of only calculating the carriage positions for the { X, Y } coordinates, and then adding Z to all three carriage positions, no longer works because the towers aren't exactly vertical. You might add 100mm of Z, but only get (for example) 98.8mm above the surface, because the tower just isn't straight up and down!
In the diagram above, the Dist variable is measured from the carriage if it was at the effector's height, to the effector. This works because a towers's XY is always the same, at every elevation. To do this properly in a robot where the tower's XY changes with elevation because of lean, we can make no assumption about whether the carriage is at the same XY at effector height that it would be at carriage height. Therefore, we would instead have to measure Dist as the horizontal distance between the carriage's elevation on the tower to the effector, and use a different variable to describe the true vertical distance from the carriage hinge to the effector hinge, which I call ZTrue:
Code: Select all
v--- Carriage
.----- <--- Dist
\ |
\ |
\ | \ = AL
\ | | = ZTrue
\|
. <--- Effector
I don't know how to continue, and I've been researching this for most of the last 12 hours, so my brain is totally fried. What's the next step? How do I find the elevation of a sphere along an arbitrarily leaning tower, such that the distance from the center of the sphere to a point at radius AL (arm length) is equal to the horizontal distance between the sphere center and the desired effector hinge location? (Is that even the right question?)
Therefore, let us throw away the "zero point" and use oTWR to define the origin of the tower.