Working on tower lean - could use some trig help!

[626Pilot]

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:

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]

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:

Code: Select all

v--- Carriage
.----- <--- Dist
 \    |
  \   |
   \  |  \ = AL
    \ |  | = ZTrue
     \|
      . <--- Effector

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?)

[mhackney]

626Pilot, thanks for disrupting my thought processing today! I spent about an hour thinking about this too and couldn't see a solution. I have a friend, Chris (Eagle has met him) who is a genius at this stuff, I'll forward him this thread and put him out of commission for a few days while he thinks about it!

Cheers,
Michael

[mhackney]

And this was the reply I got from Chris:

[img]https://imgs.xkcd.com/comics/nerd_sniping.png[/img]

In other words "he's on it"!

[chrisbennet]

Michael, you are a bad, bad, man.

[Eaglezsoar]

Seems like Chris has a good sense of humor!

[mhackney]

Sorry Chris, "desperate times call for desperate measures"!

[teoman]

I was reading up on how to use the fuzzy logic toolbox for matlab today. And one of the examples was about determining the inverse kinematics of a robot arm if the forward kinematics was known.

Here is one part from the documentation:

Why Use Fuzzy Logic?

For simple structures like the two-joint robotic arm, it is possible to mathematically deduce the angles at the joints given the desired location of the tip of the arm. However with more complex structures (eg: n-joint robotic arms operating in a 3-dimensional input space) deducing a mathematical solution for the inverse kinematics may prove challenging.

Using fuzzy logic, we can construct a Fuzzy Inference System that deduces the inverse kinematics if the forward kinematics of the problem is known, hence sidestepping the need to develop an analytical solution. Also, the fuzzy solution is easily understandable and does not require special background knowledge to comprehend and evaluate it.

In the following section, a broad outline for developing such a solution is described, and later, the detailed steps are elaborated.

I shall try to have a look at it when i find time.

[626Pilot]

And this was the reply I got from Chris:

[img]https://imgs.xkcd.com/comics/nerd_sniping.png[/img]

In other words "he's on it"!

Thanks! That comic describes me perfectly. That's why I never sleep!

I was reading up on how to use the fuzzy logic toolbox for matlab today. And one of the examples was about determining the inverse kinematics of a robot arm if the forward kinematics was known.

I could find the answer with a binary search, but it would take too long. IK is very time-critical code. It has to do a bunch of floating point stuff, on a CPU with no FPU, hundreds of times a second. To achieve decent performance with that would require something like a neural network. (I'd have a great time explaining the RAM usage to wolfmanjm.) I believe an analytical solution would be much easier.

[626Pilot]

I've been thinking more on this, on ways to simplify the problem so it's easier to think about.

Forward kinematics (FK) translate carriage elevations into a Cartesian position. This part is easy, and has already been patched to work with the new tower lean vectors.

Inverse kinematics (IK) translate a Cartesian position into carriage elevations. This is the hard part!

The fundamental problem that a delta printer's IK function has to solve is to figure out where to put the centers of three spheres such that their radii all intersect on the desired point in Cartesian space. The rigid arms of each tower act as constraints on the arms of the other towers, so that the arms always have to point at the convergence of the spheres. There are some fiddly things about this (carriage and effector offsets). Mathematically, those offsets have the same effect as increasing or decreasing the radius of the towers out from the print center.

So, for example, these configurations are identical, as far as the IK is concerned:

• Tower radius = 130mm; effector offset = 15mm; carriage offset = 15mm
• Tower radius = 100mm; effector offset = 0mm; carriage offset = 0mm

In other words, we can just subtract the offsets from the tower radii - the math will work out the same. Also, we can throw away the two-arms-per-tower model and assume only one arm per tower that connects the center of the carriage to the center of the effector. (The mathematical model doesn't have to account for wobble, because the arms do that in the real world.)

This gives us the flag-shaped model I illustrated above:

Code: Select all

v--- Carriage
.----- <--- Dist
 \    |
  \   |
   \  |  \ = AL
    \ |  | = ZTrue
     \|
      . <--- Effector

Each tower's zero point is the carriage position at which the effector is at { 0, 0, 0 }. The carriages can move up or down from their zero points. If you're on layer 0 and you need to deposit some plastic 100mm from the center, at least one carriage will have to move significantly below the zero point, so don't think of it as something where all the distances have to be positive.

In my kinematic model, each tower's lean is described by a unit vector that describes the offset in X, Y, and Z if you travel 1mm away from the origin (tower's zero point). If the lean parameters were { leanX = 1.2 degrees, leanY = 1.9 degrees }, the unit vector would look something approximately like { 0.01, -0.07, 0.98 }. This indicates that if you move up 1mm from the origin, you wind up not at { 0, 0, 1 }, but at { 0.01, -0.07, 0.98 }. If you move 1mm down from the origin, you get the same array, but all the values are sign-flipped.

The other thing this implies is that there's a constant unit vector describing the deviation from upright for each tower, in this case { -0.01, 0.07, -0.02 }. Therefore, we know that if we move n millimeters up or down, the deflection from the expected position in all three axes is n * { -0.01, 0.07, -0.02 }. That gives us the ability to figure out the true coordinates of the carriages at any elevation, positive or negative. If all we cared about was forward kinematics, that would be great, because FK only needs to know the carriage elevations.

At this point, I have enough to get this information:

• Predicted XYZ, actual XYZ, and deviation between the two, for all carriages
• Requested effector XYZ (G1 X10 Y10 Z10, for example), the effector XYZ if we use the old IK, and the deviation between the two positions

Determining actual carriage XYZ is computationally cheap: multiply elevation by unit vector and you get your coordinates. However, determining effector XYZ given carriage locations requires an expensive call to the forward kinematics, which does a ton of floating-point math to figure it out. There may or may not be enough CPU to do this hundreds of times a second. Let's assume it would be better not to call on FK unless it's unavoidable.

This problem reminds me of the kinematics for a traditional delta, where there are servomotors on top that actuate hinged arms: http://stackoverflow.com/questions/1816 ... elta-robot

Delta kinematics.png (16.62 KiB) Viewed 13399 times

It occurs to me that the traditional delta is actually solving a problem very similar to the problem this thread is about. Linear deltas are just a simplified version of the original design. Perhaps there are things they have in common, trigonometrically, that can be used to figure out a solution.

One partial solution would be to use the unit vector's Z component to correct for carriage elevation. If the carriage Z changes by 0.998 for every 1mm of elevation commanded, I can choose to move the carriage by 1.002mm for every millimeter commanded. That will correct it in the Z dimension, possibly taking some error out in the process. However, it would amount to "pushing error around" instead of eliminating it.

[626Pilot]

I have an untested solution.

Suppose you have a single tower connecting to a single side of the effector via a single arm. You know the tower's zero point and lean unit vector, and the length of the arm. Here are the variables:

• Pos[], the desired Cartesian effector coordinates
• ZP[], coordinates of the tower's zero point (carriage position when the effector is at { 0, 0, 0 })
• Dist, the distance from the tower's zero point to the effector hinge the tower's arm connects to
• AL, length of the arm
• Elev, the elevation of the carriage above or below the effector, along the major axis of the tower (which we must assume is leaning)

The distance between Pos[] and ZP[] tells us Dist.
...And we already know AL.
...And therefore, because of Pythagoras, we know that Elev = sqrt(AL^2 + Dist^2).

The tower's changing X and Y make them impossible to use as a reference point, without creating interdependencies between towers (which I want to avoid, if possible). However, each tower's ZP[] is fixed. Its X and Y are always the same. Therefore, using it as a reference point, we only have to figure out how far the carriage has to be from the zero point to achieve the desired Elev, which in turn causes Dist to be the desired value.

My understanding of the physics is that as long as Dist is correct for all three towers, Pos[] will be at the lower of two convergences between the spheres. (Or the upper convergence, if we had an upside-down delta with a moving platform under a fixed hot end at the top of the printer.)

What I don't know is whether I've missed something! Does anyone see any obvious problems with this solution?

[626Pilot]

The above solution won't work because Pythagoras' theorem only works on right triangles. That means we have to change the model to follow the Law of Cosines, which Pythagoras' theorem implements only for right triangles.


Variables
-------------------------------------------------------------------------------------------------------------
TWR = Tower, a line with origin oTWR and a unit vector vTWR.
ARM = The arm, a line with length AL = 280.
EFF = Effector, the point on the effector that connects to ARM.
CAR = Carriage, the point on TWR that connects to ARM.
O_E = A line from oTWR to EFF with distance distOE = (3D distance from oTWR to EFF).
ELEV = Elevation of carriage "above" oTWR along TWR's axis (which is not straight up and down).

A = Line from oTWR to EFF.
B = Line from CAR to EFF - the arm - which has length AL.
C = Distance from oTWR to CAR, which I call the Elevation. (Elevation is not EFF.Z.) This is what we must ultimately solve for.
a° = Angle between CAR and ARM.
b° = Angle between TWR at oTWR and A.
c° = Angle between ARM and A.


Equations
-------------------------------------------------------------------------------------------------------------
The Law of Cosines gives us this formula:
C^2 = A^2 + B^2 + 2*A*B*cos(c°)

Code: Select all

   .<--- CAR
a°|\          / = Side A: O_E
   | \         \ = Side B: ARM
   |  \ B     | = Side C: ELEV
   |   \       - = Shortest (horizontal) line from EFF to TWR
C |    \c°
   .-----.<--- EFF
   |    /      
   |  / A
b°|/
   .<--- oTWR (origin of line TWR, which follows unit vector vTWR)

Rewritten for our variables:
ELEV^2 = distOE^2 + AL^2 + 2 * distOE * AL * cos(c°)

I know distOE and AL, but not c°. I either have to find that, or do something else that will get me closer to the solution. I can't know c° before I know ELEV.

If we place a sphere with center EFF and radius AL, it will intersect TWR at one or two points. (Two unless the arm is exactly horizontal.) The higher of the two points is the location of CAR. The distance from CAR to oTWR is the elevation.

Therefore, to the best of my knowledge, I have to solve for the intersection of EFF's sphere with TWR. That comes next.

[teoman]

are you assuming that you know X Y and Z at this moment? Oh and is your tower lean a parameter? (And are you assuming the tower can lean just towards the center or does it include a sideways lean).

If for the inverse kinematics you have knowledge of X, Y , Z position of the carriage and at this point you know the tilt, then it can be solved.

[626Pilot]

are you assuming that you know X Y and Z at this moment? Oh and is your tower lean a parameter? (And are you assuming the tower can lean just towards the center or does it include a sideways lean).

If for the inverse kinematics you have knowledge of X, Y , Z position of the carriage and at this point you know the tilt, then it can be solved.

X, Y and Z are inputs to the inverse kinematics. They are the desired Cartesian coordinates of the effector. The tower lean parameter is in { degrees from X, degrees from Y }. That gets turned into a unit vector and multiplied by the distance from the tower's origin (oTWR).

[teoman]

I think i have found a solution, but you need to calculate 2 angles.
Both of these angles have to be on the vertical plane (relative to the base) that passes through the arm.

The first angle is beta, it is calculated using X, Y, Z and the origin of the tower.

The second angle is alpha. Alpha is the angle between the real tower and the virtual (perfect tower) projected on to the previously mentioned plane.

Using the law of cosine would make ELEV the position along the real tower of the carriage.

AL^2 = ELEV^2 * + D^2 - ELEV * D * cos(90 - alpha - beta)

[addisonElliott]

Hello,
I came across this topic and it instantly caught my attention. I enjoy problems like this, so I am glad to help in any way. On that note, I may have made some faulty assumptions so feel free to correct me there; I am a bit unsure which variables you do and do not have. I am a college student who recently completed Calculus 3 which involves quite a bit of this stuff.

My equation requires you have access to these variables:

• Desired X/Y/Z of the effector (x, y, z)
  Lean unit vector for each tower (v)
  Arm Length (AL)

You are looking for the carriage elevation needed to satisfy these conditions. I will name this variable n. It will represent the number of millimeters up or down from the zero point in which will satisfy this. The carriage elevation will be different for each tower since there is some lean. I'm not sure if this is a safe assumption.

This is the definition of a zero point I will be using:

Each tower's zero point is the carriage position at which the effector is at { 0, 0, 0 }. The carriages can move up or down from their zero points. If you're on layer 0 and you need to deposit some plastic 100mm from the center, at least one carriage will have to move significantly below the zero point, so don't think of it as something where all the distances have to be positive.

You mentioned that it is essentially three spheres centered somewhere on the towers that intersect at the point.
So to find the center of the sphere it is the carriage elevation times the unit lean vector. I will name this variable, which is a vector, S

Code: Select all

 S = n * v (Carriage elevation times unit lean vector gives you center of sphere)

Since a vector times a number is just the number times each component(i, j, k). I will write it like this:
Sx = n * vi
Sy = n * vj
Sz = n * vk

We can now write an equation for a sphere centered at S, and radius AL.
(x - n * Sx)^2 + (y - n * Sy)^2 + (z - n * Sz)^2 = AL^2
The only unknown in this equation is n, so we just need to solve for n. 

Here comes the fun part, I went ahead and solved it for n so it would be easy to code. Note: This is a mess with a bunch of variables, but all of them are just numbers so its not so bad. I started by FOILing all of the squares.

This yields:
x^2 - 2xn(Sx) + n^2 * (Sx)^2 + y^2 - 2ny(Sy) + n^2 * (Sy) ^2 + z^2 - 2zn(Sz) + n^2 * (Sz)^2 = AL^2

This is a quadratic, so I subtracted AL^2 over, and then factored a n^2 out of what I can, and a n out of what I can:
((Sx)^2 + (Sy)^2 + (Sz)^2)n^2 + (-2x(Sx) -2y(Sy) -2z(Sz))n + (x^2 + y^2 + z^2 - AL^2) = 0

Now use quadratic formula to solve for n. Note: There is two answers, but one is going to be out of range(like off the tower, which is ridiculous)'
I'm going to define some variables to make this easier.
a = ((Sz)^2 + (Sy)^2 + (Sz)^2)
b = -2(x(Sz) + y(Sy) + z(Sz))
c = (x^2 + y^2 + z^2 - AL^2)

n = (-b +- sqrt(b^2 - 4ac)) / 2a

That should give you the needed carriage elevation to give you the x/y/z. This is quite a bit of math, so I do not know if this will load down the Smoothie. I have been rattling my head thinking of easier ways, but I was unable to.

On a separate note, I think you may have mixed up Pythagoras's Theorem(or maybe I'm missing something).

Code: Select all

    Elev = sqrt(AL^2 + Dist^2)  
      AL = sqrt(Dist^2 + Elev^2)
    Dist = sqrt(Elev^2 + AL^2)

The hypotenuse squared is equal to the sum of each leg squared. So the first equation is correct, but the last two should be this instead:

Code: Select all

    Elev = sqrt(AL^2 + Dist^2)  
      AL = sqrt(Elev^2 - Dist^2)
    Dist = sqrt(Elev^2 - AL^2)

[addisonElliott]

I think i have found a solution, but you need to calculate 2 angles.
Both of these angles have to be on the vertical plane (relative to the base) that passes through the arm.

The first angle is beta, it is calculated using X, Y, Z and the origin of the tower.

The second angle is alpha. Alpha is the angle between the real tower and the virtual (perfect tower) projected on to the previously mentioned plane.

Using the law of cosine would make ELEV the position along the real tower of the carriage.

AL^2 = ELEV^2 * + D^2 - ELEV * D * cos(90 - alpha - beta)

Law of cosines is: a^2 = b^2 + c^2 - 2bccos(theta).
So there should be a 2 there.

What is D^2 in the equation? Maybe I am missing something. If you know D as well, then that works great.

If this equation does work, there is an easy way to find alpha. In calc three, you learn that the dot product of two vectors is the lengths of each one times the cosine of the angle between them. This can be manipulated to solve for the angle. http://www.ltcconline.net/greenl/course ... otcros.htm

v = virtual tower vector = <0, 0, 1>
w = lean unit vector
Alpha is the angle between them.

Absolute value signs around a vector means the length of it. Its simple to get using Pythagoras's theorem.

Code: Select all

v * w = |v||w|cos(alpha)

Length of each of these vectors is 1.
<Wx, Wy, Wz> * <0, 0, 1> = 1 * 1 * cos(alpha)

Wx * 0 + Wy * 0 + Wz * 1 = cos(alpha)
Wz = cos(alpha)

alpha = cos-1(Wz)

So if you want alpha, you should be able to take the inverse cosine of the z component of the unit lean vector

[teoman]

D is the distance from the tower origin to the effector.

It is the hypothenus of the right angled triagnle with one side being Z and the other being the distance of the X,Y point projected on to the base and the tower origin.

Yes you are right. I forgot the 2 over there.

[teoman]

Direct unit vector may not work as you still have to project it to the vertical plane i described.

[626Pilot]

I think i have found a solution, but you need to calculate 2 angles.
Both of these angles have to be on the vertical plane (relative to the base) that passes through the arm.

I get some of this, but the angle of alpha can't be assumed to be in the plane of the arm unless the lean makes it perfectly line up with the "ideal" tower - an unusual case. In other words, if you draw a line from the ideal tower to the leaning tower to the effector, you get a triangle that is not co-planar with the arm. Or maybe I'm wrong? My understanding of trig is paltry and unrefined. My grades for math were almost never above C in high school or college - no joke. Business statistics is the only math class I can remember getting a B in, and that's only because they had fancy learning tools where I could move the curves around in a Flash app. (For some reason, that made everything totally obvious to me in a way nothing else could.) I always found programming to be a lot easier than math.

This is the definition of a zero point I will be using:

Each tower's zero point is the carriage position at which the effector is at { 0, 0, 0 }. The carriages can move up or down from their zero points. If you're on layer 0 and you need to deposit some plastic 100mm from the center, at least one carriage will have to move significantly below the zero point, so don't think of it as something where all the distances have to be positive.

I think it's time to refine the variables a little. I've been talking about a "zero point" and a "tower origin" (oTWR). They are not useful in tandem - we have to pick one, and not the other, to be a corner of the triangle. Also, you have to have already solved the IK equation to know where the zero point is, so using that in this context seems... undefined. Therefore, let us throw away the "zero point" and use oTWR to define the origin of the tower.

oTWR can theoretically be at the homed position (top-of-tower), or at homed minus declared printer height (closer to mid-tower). However, using "homed - declared height" is troublesome. Printer height is not fixed - it will be slightly different if the hot end is removed and reinstalled, for example. The lean vector is relative to an X and Y based on the tower's angle (~90, ~220 or ~330) and radius from the printer's center (~140mm), but because of lean, X and Y will be thrown off if the Z component is changed. Therefore, it would be easier to put oTWR in a position which does not change after calibration - that being the homed position, way up at the top. (That position WILL change during calibration, as we adjust the endstop offsets, but we'll always have concrete offset values before we need to run IK.) With oTWR at the top, we can set the printer height to whatever the heck we want, and we won't have to recalibrate or fiddle with the numbers in any other way.

That makes the geometry for one tower look like this:

Code: Select all

   *<--- oTWR (position of carriage after homing)
E |\ b°
l  | \
e |  \
v |   \  Dist
   |    \
   *,    \
a°   \   \
         \  \
     AL   \ \  c°
             `* <--- EFF

Dist is the 3-distance from oTWR to the desired effector XYZ.
Elev is the distance from oTWR along TWR's lean vector to where the carriage needs to be, which I will call "down-ish."

Given AL and Dist, we want to solve for Elev.

You mentioned that it is essentially three spheres centered somewhere on the towers that intersect at the point.

...

Now use quadratic formula to solve for n. Note: There is two answers, but one is going to be out of range(like off the tower, which is ridiculous)'
I'm going to define some variables to make this easier.
a = ((Sz)^2 + (Sy)^2 + (Sz)^2)
b = -2(x(Sz) + y(Sy) + z(Sz))
c = (x^2 + y^2 + z^2 - AL^2)

n = (-b +- sqrt(b^2 - 4ac)) / 2a

These depend on knowing S, but S is a part of what I have to solve for, so I don't know its value yet. If I do know S, all I have to do is measure its distance from oTWR. Because both points lie on the tower's line, the distance is the elevation. I may be sensing commutativeness where there is none, but I think if you put a sphere of radius=AL at the effector's desired coordinates, its upper or only intersection with each tower would be the carriage position. Therefore, we'd want to compute the upper (or only) intersection of that AL-sphere with the tower, given its origin and lean unit-vector.

The hypotenuse squared is equal to the sum of each leg squared. So the first equation is correct, but the last two should be this instead:

Code: Select all

    Elev = sqrt(AL^2 + Dist^2)  
      AL = sqrt(Elev^2 - Dist^2)
    Dist = sqrt(Elev^2 - AL^2)

I think you're right. My trig is weaker than I'd like.

[addisonElliott]

I attempted to project the tower lean vector onto the plane teoman describes and it was some pretty messy math, but then again a lot of this is. I might give it another attempt and see what the CAS system comes up with.

Anyways, a couple of hours after I posted a response I realized that my solution was not fully correct because I did not use the same coordinate system. I have since redone it so that the coordinate system is consistent. It is nontheless very ugly, but it should work. I am not sure how much processing power it would take to calculate this many times a second.

I made the center of the bed to be (0, 0, 0) and defined everything from there.

Here are the variables I used:

• Arm length (AL)
  Desired X/Y/Z (x1, y1, z1)
  Tower top coordinates (oTwr)
  Unit lean vector (v)

Code: Select all

So instead of making the spheres on the carriages I decided to center it on the desired X/Y/Z like you suggested. This gives the equation:
(x - x1)^2 + (y - y1)^2 + (z - z1)^2 = AL^2

Now we want to plug in for x/y/z so that it is a coordinate on the tower. So, we need to get the vector of the tower. From calculus 3, the equation to make a vector is:
r = (Vector from origin to some starting point) + t * (vector direction)

Since oTwr is the coordinates of the tower at the TOP, I negate the unit lean vector and use the oTwr as the position. This is a vector from the tower top that points downwards along. t is the number of millimeters down from oTwr.
This translates to:
r = oTwr - t * v
r = <oTwrx - t * vx, oTwry - t * vy, oTwrz - t * vz>

I now plug in each of the components of r and solve for t and this will tell us how many millimeters from the top of the tower we need to go to reach that X/Y/Z.

(oTwrx - t * vx - x1)^2 + (oTwry - t * vy - y1)^2 + (oTwrz - t * vz - z1)^2 = AL^2

I now plugged this into a CAS system and let it solve for t. I defined a couple of more variables to make this look nicer, but this is what it says:

a = vx^2 + vy^2 + vz^2
b = 2(vx(x1 - oTwrx) + vy(y1 - oTwry) + vz(z1 - oTwrz))
c = (oTwrx - x1)^2 + (oTwry - y1)^2 + (oTwrz - z1)^2

t = (-b +- sqrt(b^2 - 4ac)) / 2a

I used CAS to verify that this does not simplify into something nicer and it does not. Everything is constants so that is a plus, but there is quite a bit of squaring and square rooting, so that will most likely require FPUs.

You would just run this for each tower replacing vx,vy,vz,oTwrx,oTwry,oTwrz to get your three numbers. This will represent the number of millimeters to move down from the top.

[626Pilot]

Thanks for working on this. It's beginning to make sense now.

I have another issue, somewhat easier. I had the math necessary to turn lean about a tower into a unit vector in a spreadsheet that got lost when I was moving some files around. I tried to recreate it today, but I'm getting weird results! The lean components are x and y. The formula I was using to convert this into a unit vector was as follows:

vec.x = cos(radians(90-x)) * cos(radians(90-y))
vec.y = sin(radians(90-x)) * cos(radians(90-y))
vec.z = sin(radians(90-y))

The 90-x is so that a lean specified as 3.5 degrees means that the tower is at 86.5 degrees relative to the surface (90 - 3.5).

However, when I run leans of X=60 and Y=60 (so each tower is at an angle of 30 degrees above the surface from both angles) I get a vector that looks like this:
v.x=0.750000
v.y=0.433013
v.z=0.500000

In the spreadsheet I'm using, the numbers are negated (because the vector points down), but for simplicity I'm skipping that step. I also verified that vec.x^2 + vec.y^2 + vec.z^2 = 1.

If the tower is leaning 60 degrees north and 60 degrees east, why would the unit vector have different X and Y components? Why wouldn't they be the same? I think my math might be off, but this is a question that's difficult to ask Google. I've been asking, "convert Euler angles to vector". Maybe I'm using the wrong formula. :/

[addisonElliott]

It is no problem; I enjoy helping out.

As for your issue, I see what you are saying and I am not sure where you are getting those equations. This doesn't mean they're wrong, it just means I've never taken enough time to understand Euler angles/quaternions to be of any help with diagnosing your formula.

However, I agree with you that your answer does not seem right. I have thought of a formula to solve for this, but it seems extremely simple and I am not sure if it is correct. I tried a couple of test points and the answers seemed reasonable.

vec.x = cos(radians(90 - x))
vec.y = cos(radians(90 - y))
vec.z = sqrt(1 - vec.x ^2 - vec.y ^ 2)

Here's a bit of an explanation of where I got this:

Code: Select all

In my statics class, there is a formula that says each component of a vector is equal to the length of the vector times the cosine of the angle between the axis and the vector.
Fx = Fcos(x)
Fy = Fcos(y)
Fz = Fcos(z)

In our case, the length of the vector will be 1, so that makes:
Fx = cos(x)
Fy = cos(y)
Fz = cos(z)

To create a vector, you can take two points and subtract the final point minus the initial point.
Our initial point is (0, 0, 0)
Our final point is (Fx, Fy, Fz)

That gives us the vector: <Fx, Fy, Fz>
Or: <cos(x), cos(y), cos(z)>

We do not know z, but we know the length of this vector is 1 so we can use Pythagoras's theorem.
cos(x)^2 + cos(y)^2 + cos(z)^2 = 1
cos(z)^2 = 1 - cos(x)^2 - cos(y)^2
cos(z) = sqrt(1 - cos(x)^2 - cos(y)^2)

This gives us the end result:
<cos(x), cos(y), sqrt(1 - cos(x)^2 - cos(y)^2)>
x and y would be 90 minus their value in this case

x = 60, y = 60
vec.x = 0.5
vec.y = 0.5
vec.z = 0.7071

x = 3.5, y = 7
vec.x = 0.061
vec.y = 0.122
vec.z = 0.991

[Polygonhell]

I think 626 Pilots equations are probably correct in this case, though I haven't verified them by hand.
The problem with euler angles is they are applied in succession, i.e. you apply one rotation and then apply a second to the rotated coordinate frame, and yes order of application matters. This is why the resulting vector is not symmetric, and why the resulting y coordinate is a combination of both rotations since I assume in this case the rotation around the X axis is applied first. You can trivially compute/verify the equations by composing 2 2D rotation matrices.
Generally Euler angles are best avoided in any real math, they have degeneracies (the so called gimbal lock problem), it's why the seemingly over specified quaternion is useful.
Personally I'd probably phrase the math in terms of the tower vectors rather than the angles.

[626Pilot]

I ran some angles through this:

vec.x = cos(radians(90 - x))
vec.y = cos(radians(90 - y))
vec.z = sqrt(1 - vec.x ^2 - vec.y ^ 2)

It worked for 45 and 90, but for any other angle, sqrt(v.x^2 + v.y^2 + v.z^2) != 1, so it can't represent a unit vector.

I think 626 Pilots equations are probably correct in this case, though I haven't verified them by hand.
The problem with euler angles is they are applied in succession, i.e. you apply one rotation and then apply a second to the rotated coordinate frame, and yes order of application matters. This is why the resulting vector is not symmetric, and why the resulting y coordinate is a combination of both rotations since I assume in this case the rotation around the X axis is applied first. You can trivially compute/verify the equations by composing 2 2D rotation matrices.
Generally Euler angles are best avoided in any real math, they have degeneracies (the so called gimbal lock problem), it's why the seemingly over specified quaternion is useful.
Personally I'd probably phrase the math in terms of the tower vectors rather than the angles.

That makes sense. The formula I've been using returns crazy numbers, for example:

Lean.X = 10, Lean.Y = -20; Lean vector (upside down) = { 0.059, 0.337, -0.94 }; projected tower bottom = { 18.411, 104.415, -291.305 }
Lean.X = -20, Lean.Y = 10; Lean vector (upside down) = { -0.059, 0.163, 0.985 }; projected tower bottom = { 18.411, -50.585, -305.29 }

I attached the spreadsheet where I did this, so you can see it for yourself.

Notice that the two above lines have the same lean values - I've just flipped them. This means that both of their vectors point in directions that deviate from vertical by EXACTLY the same amount. One would assume that, therefore, the projected Z at the bottom of the tower would be the same - how could it be otherwise? Nevertheless, the Z values for the ends of the towers ARE different by almost 15 millimeters. I suppose this has to do with the "orderedness" of Euler angles, and provides good evidence that quaternions are the way to go.

This leaves the question: how do you turn the angles of X and Y relative to horizontal, into a quaternion, and is it going to overtax the CPU? I looked at some source code that turns Euler angles into quaternions and it's completely unreadable:

Code: Select all

  u0 = sqrt(cos(e2*PI/180)*cos(e1*PI/180)+cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)* cos(e3*PI/180)+1)/2;
  u1 = (cos(e1*PI/180)*sin(e3*PI/180)+cos(e2*PI/180)*sin(e3*PI/180)+sin(e2*PI/180)*sin(e1*PI/180)*cos(e3*PI/180))/sqrt(cos(e2*PI/180)* cos(e1*PI/180)+cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)*cos(e3*PI/180)+1)/2;
  u2 = (sin(e2*PI/180)*sin(e3*PI/180)-cos(e2*PI/180)*sin(e1*PI/180)*cos(e3*PI/180)-sin(e1*PI/180))/sqrt(cos(e2*PI/180)*cos(e1*PI/180)+ cos(e2*PI/180)*cos(e3*PI/180)-sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)*cos(e3*PI/180)+1)/2;
  u3 = (sin(e2*PI/180)*cos(e1*PI/180)+sin(e2*PI/180)*cos(e3*PI/180)+cos(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180))/sqrt(cos(e2*PI/180)* cos(e1*PI/180)+cos(e2*PI/180)*cos(e3*PI/180)
sin(e2*PI/180)*sin(e1*PI/180)*sin(e3*PI/180)+cos(e1*PI/180)*cos(e3*PI/180)+1)/2;

I find this hideous. I asked my brain to parse the above code, and it did the equivalent of throwing up two middle fingers.

Personally I'd probably phrase the math in terms of the tower vectors rather than the angles.

I don't know how to get a lean vector without starting from lean angles.

Let me explain how this will be used. I will give the annealer a reasonable range (let's say -2 to 2 degrees of lean in the X and Y directions). It will walk around that defined space, using a binary search coupled with the ability to simulate printer kinematics to figure out whether it needs to increase or decrease each lean angle's value. I can't just give the annealer an entire unit vector's space to walk (<-1, -1, 0> to <1, 1, 1>) because almost every combination of numbers it tries will not produce a vector of magnitude 1. It might theoretically converge on something very close to 1, but I need a magnitude that is 1. By insisting on starting with only X and Y lean angles, and turning that into a unit vector, I can let the annealer adjust the two variables that are closest to the physical reality of the printer; and in doing so, the annealer will not have to explore the solution space containing 999999999999999 invalid unit vectors and relatively few valid unit vectors. It will only be producing valid unit vectors.

I've attached the spreadsheet I'm using, if anyone wants to mess with the numbers.

[Polygonhell]

You use the X and Y portions of the vector as the inputs to the annealer rather than the angles, you compute Z as sqrt(X*X + Y*Y).
If you want to clamp X and Y to reasonable values you clamp them to the sin of the desired angle so for say +/- 10 degrees you's clamp abs(X) and Y to be < 0.17364817766

Your optimizer should end up with approximate tower vectors, but you need to phrase the other math in terms of the tower vectors rather than the angles, I suspect that you will find the other math turns out easier/cheaper without the angles. All the sin/cos stuff just goes away and you end up with a bunch of dot and cross products instead.

What this doesn't let you do is deal with tower rotation in addition for that you need to use a quaternion, the trivial explanation of a quat is it's basically equivalent to a rotation about an arbitrary axis, and in fact toy can trivially compute one from that, but that's a somewhat toy explanation.

When I worked in games we always said if you deal with geometry and convert it to angles to solve a problem you're doing it wrong.