Rostock Kinematics Document

7947 views
Skip to first unread message

HopingForRobotCars

unread,
Dec 8, 2013, 7:17:09 PM12/8/13
to delt...@googlegroups.com
I have created a document discussing Johann's Rostock style delta printer. It is in the attached pdf.  I hope this helps more people understand how the delta printers work.

Steve
Rostock Delta Kinematics.pdf

Ian Lee, Sr.

unread,
Dec 8, 2013, 9:04:43 PM12/8/13
to delt...@googlegroups.com
Wow!  This looks like great stuff.  Looking forward to having time to sit down and study every word.  Thanks!

HopingForRobotCars

unread,
Dec 8, 2013, 9:49:28 PM12/8/13
to delt...@googlegroups.com
Ian,
You're welcome.  I've been working on it off and on for several months.  The last hold-up was the illustrations and then I found Johann's OpenSCAD model and that problem was solved.

Steve

Pezgarden now

unread,
Dec 9, 2013, 2:19:12 PM12/9/13
to delt...@googlegroups.com

thank you!

HopingForRobotCars

unread,
Dec 9, 2013, 3:25:22 PM12/9/13
to delt...@googlegroups.com
You're welcome!

I think the images at the end showing error profiles and print areas are something of interest I haven't seen elsewhere.

Steve

MrWisotzkey

unread,
Dec 9, 2013, 4:51:23 PM12/9/13
to delt...@googlegroups.com
Truly fantastic!  Plan to read and study so I can look like a real smartypants when someone wants to talk turkey about the printer!

You win +1 internets

JW

On Sunday, December 8, 2013 7:17:09 PM UTC-5, HopingForRobotCars wrote:

Bob Weiss

unread,
Dec 9, 2013, 5:20:21 PM12/9/13
to delt...@googlegroups.com
Very well done!!! Thank you for creating and sharing that document. I already went through the code to understand it myself earlier when I was getting the lineardelta kinematics for linuxcnc working for my printer. I was confused quite a few times until I drew it out on paper to visualize the equations better.

This should help plenty of people understand it better..

:)

Bob

Michael Paauwe

unread,
Dec 9, 2013, 5:37:43 PM12/9/13
to delt...@googlegroups.com
That's really cool.  I did this exercise a year or so ago but only published some snippets here and possibly not quite as thoroughly as you have.
I followed almost exactly the same route you did.
A few things I did differently though.
In solving the forward kinematics I used trilateration and found the intersection of 3 spheres centred on the carriages.  This resulted in a very similar amount of computation.
In assessing the error I divided the error by the step size to normalise the result and make a comparison with a cartesian printer easier.

I posted some graphics that look just like some of yours.

Cheers for taking the time to publish this.

Regards,
Mike Paauwe
Message has been deleted

HopingForRobotCars

unread,
Dec 9, 2013, 6:42:32 PM12/9/13
to delt...@googlegroups.com
Mike,
I kind of did the normalizing in my head.  It is the reason why I choose 0.01mm as the error.  One just needs to mentally shift two decimal places to normalize. 

And I now realize that I should have discussed the cartesian factors of 1.0 in one dimension, 1.414 (sqrt(2)) in two dimensions and 1.732 (sqrt(3)) in three dimensions.  As expected, the delta has the same order of magnitude, but distributed differently.  But normalizing is a little tricky for the delta.  One can apply the normalized factor to any size of error for the cartesian system, since there is a linear relationship.  That is not true of the delta, since it is a second order system.  It is only meaningful to compare errors of the same order of magnitude (relative to system size) on both systems.

Steve

Tayfun Türek

unread,
Dec 9, 2013, 6:56:39 PM12/9/13
to delt...@googlegroups.com
Dear Steve,

Thank you for creating and sharing the document, it is very informative
and inspiring. May I ask what software did you use to create graphics 
at the end of document, I can use it if it is open-source in my work.

Tayfun.



10 Aralık 2013 Salı tarihinde HopingForRobotCars adlı kullanıcı şöyle yazdı:
--
You received this message because you are subscribed to the Google Groups "Delta robot 3D printers" group.
To unsubscribe from this group and stop receiving emails from it, send an email to deltabot+u...@googlegroups.com.
To post to this group, send email to delt...@googlegroups.com.
Visit this group at http://groups.google.com/group/deltabot.
For more options, visit https://groups.google.com/groups/opt_out.

HopingForRobotCars

unread,
Dec 9, 2013, 7:10:33 PM12/9/13
to delt...@googlegroups.com
I used a Java program I created myself in Eclipse.

Steve
To unsubscribe from this group and stop receiving emails from it, send an email to deltabot+unsubscribe@googlegroups.com.

HopingForRobotCars

unread,
Dec 9, 2013, 11:16:05 PM12/9/13
to delt...@googlegroups.com
Mike,
I have looked at the trilateration formulas.  It is a better solution.  The circumcenter solution only works when the three arms are of equal length.  The trilateration solution works for different lengths.  I am particularly interested in a possible solution to printing houses.  I believe that it is possible to use three cranes instead of the present gantry crane solution.  In this case the lengths of the arms are the variables to control and the inverse kinematics are fairly simple.  Now I have a simple forward kinematics solution as well.

Steve

On Monday, December 9, 2013 5:37:43 PM UTC-5, Michael Paauwe wrote:

Miguel Sánchez

unread,
Dec 25, 2013, 8:07:35 AM12/25/13
to delt...@googlegroups.com
Hi Steve,

Won't you need to reconsider the math as the long beams of effector arms will form a catenary arch? If effectors are not a straigt line then things get messy math-wise, didn't they?

Steven Graves

unread,
Dec 28, 2013, 1:33:36 AM12/28/13
to delt...@googlegroups.com

Miguel,
You are right. It is solvable of course. I'm not sure how much accuracy they need, but the Skycam at sporting events is based on the same principle.

Steve

--
You received this message because you are subscribed to a topic in the Google Groups "Delta robot 3D printers" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/deltabot/V6ATBdT43eU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to deltabot+u...@googlegroups.com.

Steven Graves

unread,
Dec 28, 2013, 1:55:19 AM12/28/13
to delt...@googlegroups.com

Miguel,
A simpler solution might be to add feedback and close the loop.  One would already need to add a mechanism to put the cranes under computer control. One could also add laser or sonar to measure the straight line distance between the top of each crane and the effector. In fact, one can imagine a number of configurations for the feedback system. Ultimately, one just needs to know the position to calculate the straight line lengths. To move to a different position one would adjust the crane associated with each line.

Steve

Message has been deleted

Richard, Printrbot Simple, building Mini Kossel

unread,
Jan 2, 2014, 10:51:45 PM1/2/14
to delt...@googlegroups.com
On Thursday, January 2, 2014 7:39:39 PM UTC-8, Richard, Printrbot Simple, building Mini Kossel wrote:
On Sunday, December 8, 2013 4:17:09 PM UTC-8, HopingForRobotCars wrote:
I have created a document discussing Johann's Rostock style delta printer. It is in the attached pdf.  I hope this helps more people understand how the delta printers work.

Steve

I am interested in the math behind the current that the wizard is manipulating.

 
Downloaded it and found the math beyond my skill level……oh, well.
 

Gfast Gao

unread,
May 2, 2014, 5:40:55 AM5/2/14
to delt...@googlegroups.com
Hi Steve,

bravo!

This is the best explanation about Deltabot I've ever seen.

But I can't got the idea about variable Acx, Acy, Aex, Aey. So I ruined to understand the most part of your explanation. 

Can you give me some tip what are they?

在 2013年12月9日星期一UTC+1上午1时17分09秒,Steve Graves写道:

Gfast Gao

unread,
May 2, 2014, 6:20:07 AM5/2/14
to delt...@googlegroups.com
I got the idea now. Thanks.

But there is another question:

/ (Acx - Apx)^2 + (Acy - Apy)^2 = Ad^2
/ Acx = X - Aex       Acy = Y - Aey
/ Why it will be: (X + Aex - Apx) ^2 + (Y+Aey - Apy)^2 = Ad^2

Thanks!

Su

在 2014年5月2日星期五UTC+2上午11时40分55秒,Gfast Gao写道:

Gfast Gao

unread,
May 2, 2014, 6:59:38 AM5/2/14
to delt...@googlegroups.com
Sorry, 

Besides the question above I would like to know:

Why Bvx, Bvy => R*sin(30), -R*cos(30)
but not Bvx, Bvy => R*cos(30), -R*sin(30)

Is there some one checked this point?


Dank you.

Su

在 2014年5月2日星期五UTC+2下午12时20分07秒,Gfast Gao写道:

Steven Graves

unread,
May 4, 2014, 10:40:57 PM5/4/14
to delt...@googlegroups.com
Su,
Sorry for the delay, I don't check my gmail often. 

Someone caught a mistake in the Z calculations. The variable I call Hcz is actually the distance the head extends below the effector and should more properly be called Hez. I have made some changes in the document.  The new version is attached. I suspect this mistake is what was causing you the problem in your first question.

You are right about (X + Aex - Apx) ^2 + (Y+Aey - Apy)^2 = Ad^2.  It is correct but the intermediate step should be Acx = X + Aex       Acy = Y + Aey not Acx = X - Aex       Acy = Y - Aey.

Thank you for the catch, it is in the new version.

You are also right about Bvx, Bvy => R*sin(30), -R*cos(30) it should be Bvx, Bvy => R*cos(30), -R*sin(30).  The math worked out because the distance from 0,0 is the same for both points.  It turns out that 0,0 is not a good test point.

Thank you for that catch as well.  I really appreciate you thoroughly reading my document.  You are now a part of it.  I will always think of you when I read those sections!  So far there have been three mistakes in my document, it looks like you found them all.  Since I have new stuff in the attached document, I hope you will find any mistakes I have in the new stuff as well.

Thank you again for your help.

Thanks,
Steve 




--
You received this message because you are subscribed to a topic in the Google Groups "Delta robot 3D printers" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/deltabot/V6ATBdT43eU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to deltabot+u...@googlegroups.com.
To post to this group, send email to delt...@googlegroups.com.
Visit this group at http://groups.google.com/group/deltabot.
For more options, visit https://groups.google.com/d/optout.

Rostock Delta Kinematics 3.pdf

3D_Kudu

unread,
Jun 24, 2014, 5:44:13 PM6/24/14
to delt...@googlegroups.com
Wow, this is an awesome resource Steve! Thanks. I've been ruminating over the linear delta kinematics for a bit now, and had arrived at a number of the same conclusions about the inverse kinematics, but hadn't quite pieced it all together yet. I was about to break down and start dissecting the Marlin firmware when I came across this thread, so I can only imagine how many hours worth of analysis you've saved me.

Ultimately, I need a model to evaluate the forces involved in moving the effector around the build volume to help me spec things like motors, belts, arms, and rod ends (magnetic or traditional) for my big delta. Do you know if a tool like this exists? If not, I don't mind working on one, but I'd rather not reinvent the wheel if there is something useful that's already out there.

Brad Norman

unread,
Oct 11, 2014, 4:03:23 PM10/11/14
to delt...@googlegroups.com
Steve -

Thanks for the great document!

I am interested in determining the conical or linear volume lost as a Delta reaches the top side of its build envelope.  See attached graphics...

With each arm at the max extents upwards the nozzle is centered.  As 1 or 2 arms are lowered with the 3rd remaining at the max extent Z height is lost.  I am curious how much and how to represent this loss of build volume.

Thanks,

Brad


Nick Starno

unread,
Oct 12, 2014, 1:10:30 AM10/12/14
to delt...@googlegroups.com
Brad,

Here is a visual representation of what the true build volume looks like.  I modeled this by taking the full range of motion (largely defined by the joints) of each arm and combining the intersection of all three.  Turns out it is not conical at all and has three ridges between each tower.  I thought it looked strange at first, but it matches how my end effector moves if I manipulate it by hand.
delta-build-volume.PNG

Brad Norman

unread,
Oct 12, 2014, 10:41:37 AM10/12/14
to delt...@googlegroups.com
Nick -

I can confirm your findings.  I am interested if any math gurus could model and predict the Z height where loss of full volume occurs.  Your method of displaying the volume is good!

Thanks - Brad

Aaron Birenboim

unread,
Oct 13, 2014, 11:41:05 AM10/13/14
to delt...@googlegroups.com
I, too started looking into this... and have on-line notes...  http://boim.com/MarlinUtil/CalDoc/Calibration.html
I used Steve's forward kinematics to speed up my searches.  thanks.
Still working on this to develop an alternate calibration procedure.  

Aaron Birenboim

unread,
Oct 14, 2014, 10:16:58 AM10/14/14
to delt...@googlegroups.com

to try and spread the word... here are my basic kinematics computations in octave  (may work in MATLAB too)
cart2delta.m
delta2cart.m

Brad Norman

unread,
Oct 14, 2014, 12:32:22 PM10/14/14
to delt...@googlegroups.com
Aaron -

Nice work. I will have to look at the RichieC branch of Marlin.

Any experience with FSR and RichCattel's leveling?  It seems his code adjusts both rod_length & delta_radius.  Still testing.

B/R - Brad

Aaron Birenboim

unread,
Oct 15, 2014, 6:24:21 PM10/15/14
to delt...@googlegroups.com
Brad asked about print volumes...

this is for a typical delta with diag-rod roughly 2x delta radius.  Scale to your printer size (roughly).
The height shown is short, to make the plot prettier.
Typically, the actual height would be 3-4 times as high, but the lid would have the same shape...
just shifted up 300-500mm


I posted links to my kinematics code in octave before.  the forward kinematics are ported from Steve's java.
here's the code for the volume plot
plotBuildVolume.m

Aaron Birenboim

unread,
Oct 15, 2014, 6:27:35 PM10/15/14
to delt...@googlegroups.com
scale is hard to tell from this plot.  The edges of the build volume drop down roughly 30% of the delta radius from the tip.
The low point of the dip is towards each tower.

Aaron Birenboim

unread,
Oct 16, 2014, 8:58:43 AM10/16/14
to delt...@googlegroups.com


On Wednesday, October 15, 2014 4:24:21 PM UTC-6, Aaron Birenboim wrote:
Brad asked about print volumes...

Here's the key formulas...

If we define Hmax to be the maximum height of the delta-rod axis, at the carriage, e.g.  max height of delta rod tops above print bed,
with Marlin model DELTA_RADIUS, and DELTA_PRINTABLE_RADIUS, DELTA_DIAGONAL_ROD we get:

     Zmax = Hmax - sqrt( DELTA_DIAGONAL_ROD^2 - DELTA_RADIUS^2)    ad volume center

However, at the edge near a tower we get:

    Zmax = Hmax - sqrt( DELTA_DIAGONAL_ROD^2 - (DELTA_RADIUS - DELTA_PRINTABLE_RADIUS)^2 )

It would be more honest to publish the second, lower max print height, but then say that it can print C mm higher in the center.

        C =  sqrt( DELTA_DIAGONAL_ROD^2 - (DELTA_RADIUS - DELTA_PRINTABLE_RADIUS)^2 ) - sqrt( DELTA_DIAGONAL_ROD^2 - DELTA_RADIUS^2)

Brad Norman

unread,
Oct 16, 2014, 10:14:53 AM10/16/14
to delt...@googlegroups.com
Aaron -

Thanks for taking the time to model the upper build volume.  Downloading Octave for a better understanding.

Brad

Brad Norman

unread,
Oct 16, 2014, 10:16:27 AM10/16/14
to delt...@googlegroups.com
I will apply a 30% calc and then verify on our actual printers.  Thanks again for quantifying and modeling the true build volume of a Deltabot.  - Brad

Brad Norman

unread,
Oct 16, 2014, 10:20:53 AM10/16/14
to delt...@googlegroups.com
I agree that build volume should be represented as a lower cylinder with approximately 30% additional Z height that centers in the build envelope at MAX Z.  Not sure if any other published Deltabots are doing this.  Would be interesting to have the community determine the accuracy of published specs of various Deltabot printers.  Again, great work - Brad

RatInDaHat

unread,
Oct 16, 2014, 4:41:21 PM10/16/14
to delt...@googlegroups.com
That work is interesting and in-depth, but i'm wondering if you guys made it too difficult. There are two angles you can look at as well as the length of the arms. You can determine the center height and angle, as well as the angle of the highest arm if you lower the other two. The head can never go lower than the arm would allow when it is vertical. Just food for thought. 

Øystein Becker

unread,
Oct 16, 2014, 7:13:45 PM10/16/14
to delt...@googlegroups.com
Thank you! Great work!

AndrewBCN

unread,
Jan 16, 2015, 5:50:36 AM1/16/15
to delt...@googlegroups.com
Dear Mr. Graves,
First I would like to thank you for this impressive document. It is as yet the simplest, clearest explanation I have found on the web for the linear delta kinematics as found in the Rostock and Kossel 3D printers. The excellent illustrations really help a lot.
I would like to ask you for permission to upload it to the reprap.org wiki (obviously with proper attribution to you) and also to provide a link to the file on the Wikipedia "Delta robot" page https://en.wikipedia.org/wiki/Delta_robot
I am in the process of designing a Rostock-like delta printer and your paper was an essential piece of information to me in the design process, particularly your various pointers to the Marlin variables that need to be configured.
I understand you have posted a couple of different versions of your paper, is there any newer version or is the last one posted in this thread the definitive version?
Best regards,
Andrew

David Fruehwald

unread,
Jan 16, 2015, 12:20:51 PM1/16/15
to delt...@googlegroups.com
I agree, in my opinion the average person wants to know the build height of the lowest Z that creates a flat topped cylinder and not maximum z which can only be achieved in a few places.

Brad Norman

unread,
Jan 16, 2015, 12:27:22 PM1/16/15
to delt...@googlegroups.com
Also, agree...

I wish all kit manufacturers agreed to publish Z flat and not Max Z.  It is hard to publish the lower Z flat value when others are not.  Apple to oranges.

We will have 2 values in our specs, once finalized... Max Z and Max Z flat. 

Happy printing,


Brad Norman
UltiBots LLC

On Fri, Jan 16, 2015 at 12:20 PM, David Fruehwald <dfrue...@gmail.com> wrote:
I agree, in my opinion the average person wants to know the build height of the lowest Z that creates a flat topped cylinder and not maximum z which can only be achieved in a few places.

David Fruehwald

unread,
Jan 16, 2015, 12:49:16 PM1/16/15
to delt...@googlegroups.com
I think your idea of publishing both is a great idea. Unfortunately a newbie like myself sees maximum z and things that is the height of the entire build area until you get a printer and realize that max z only works in the center and that the edges have a lower heights.

Ryan Carlyle

unread,
Jan 16, 2015, 5:01:25 PM1/16/15
to delt...@googlegroups.com
Max flat Z is SUPER simple, you just position one arm straight down and measure between the nozzle tip and build plate. Totally agree though, it's a much better indicator of build volume than max center Z. 

ping cheong lo

unread,
Mar 25, 2015, 8:48:03 AM3/25/15
to delt...@googlegroups.com
Hi Steve,

Thanks very much.i can build my own robot now.

在 2013年12月9日星期一 UTC+8上午8:17:09,Steve Graves写道:

deltapenguin

unread,
Mar 25, 2015, 6:20:03 PM3/25/15
to delt...@googlegroups.com

Thanks a lot!

Marco Antonini

unread,
Apr 24, 2015, 3:39:26 AM4/24/15
to delt...@googlegroups.com

Thank you, really very interesting!
I'm trying with Matlab, work well  :)

my results are that my machine seems to print up to 350 mm, in the chart the green plane intercepts the max printable height:


Ryan Carlyle

unread,
Apr 24, 2015, 10:00:43 AM4/24/15
to delt...@googlegroups.com
It's also quite easy to find max printable height by moving the effector manually so that one arm more or less vertical with its carriage in its home position, and measuring from the nozzle to the build plate. That'll get you +/- a couple millimeters.

Raymond Ma

unread,
May 9, 2015, 3:12:12 PM5/9/15
to delt...@googlegroups.com
I made a python script based on Steve's document to provide some alternative visualizations of the delta performance. I detail some of the basic results here: http://blog.rymnd.com/articles/delta-kinematics-1/

Hopefully it'll be useful to others here as well. I haven't fully tested the script, but it's been useful for me to better understand my Rostock MAX and help me think about how to dimension a future delta design. If there are any egregious mistakes, please let me know.

Thanks again, Steve, for the very helpful document!
DeltaPrinter.py

AndrewBCN

unread,
May 10, 2015, 3:13:37 AM5/10/15
to delt...@googlegroups.com
@ Raymond

Excellent blog post, a must read for anyone building or designing a linear delta robot. Thanks for sharing!

Cheers,
Andrew

Mustafa Ayyıldız

unread,
Jun 3, 2015, 9:51:02 AM6/3/15
to delt...@googlegroups.com
Hi

We need kinematic equations of Linear delta robot.

Reverse and forward kinematics formulas write matlab code.

Thanks 




10 Mayıs 2015 Pazar 10:13:37 UTC+3 tarihinde AndrewBCN yazdı:

Dejay

unread,
Jun 4, 2015, 2:03:09 AM6/4/15
to delt...@googlegroups.com
Never used matlab and not sure if this helps you but you could take a look into the cpp code of some firmwares:


// Calculate the motor position for a single tower from a Cartesian coordinate
float DeltaParameters::Transform(const float machinePos[AXES], size_t axis) const
{
return machinePos[Z_AXIS]
         + sqrt(D2 - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis]));
}


 
void DeltaParameters::InverseTransform(float Ha, float Hb, float Hc, float machinePos[AXES]) const
{
const float Fa = coreFa + fsquare(Ha);
const float Fb = coreFb + fsquare(Hb);
const float Fc = coreFc + fsquare(Hc);
// debugPrintf("Ha=%f Hb=%f Hc=%f Fa=%f Fb=%f Fc=%f Xbc=%f Xca=%f Xab=%f Ybc=%f Yca=%f Yab=%f\n",
// Ha, Hb, Hc, Fa, Fb, Fc, Xbc, Xca, Xab, Ybc, Yca, Yab);
// Setup PQRSU such that x = -(S - uz)/P, y = (P - Rz)/Q
const float P = (Xbc * Fa) + (Xca * Fb) + (Xab * Fc);
const float S = (Ybc * Fa) + (Yca * Fb) + (Yab * Fc);
const float R = 2 * ((Xbc * Ha) + (Xca * Hb) + (Xab * Hc));
const float U = 2 * ((Ybc * Ha) + (Yca * Hb) + (Yab * Hc));
// debugPrintf("P= %f R=%f S=%f U=%f Q=%f\n", P, R, S, U, Q);
const float R2 = fsquare(R), U2 = fsquare(U);
float A = U2 + R2 + Q2;
float minusHalfB = S * U + P * R + Ha * Q2 + towerX[A_AXIS] * U * Q - towerY[A_AXIS] * R * Q;
float C = fsquare(S + towerX[A_AXIS] * Q) + fsquare(P - towerY[A_AXIS] * Q) + (fsquare(Ha) - D2) * Q2;
// debugPrintf("A=%f minusHalfB=%f C=%f\n", A, minusHalfB, C);
float z = (minusHalfB - sqrtf(fsquare(minusHalfB) - A * C)) / A;
machinePos[X_AXIS] = (U * z - S) / Q;
machinePos[Y_AXIS] = (P - R * z) / Q;
machinePos[Z_AXIS] = z;
}

Daniel Langstaff

unread,
Feb 21, 2016, 8:40:17 PM2/21/16
to Delta robot 3D printers
You sir are awesome!  You just saved me 2 hours of deriving it myself.  I'm building one from scratch and am trying to create a BOM.  This will speed up the calculations a lot.

Daniel Langstaff

unread,
Feb 21, 2016, 8:52:35 PM2/21/16
to Delta robot 3D printers
This is looking at the problem upside down and backwards.  Doing it this way will either get you terrible quality, or very long build times.  I have a simple solution, but I was an idiot and signed an NDA.  I will share as soon as the terms are expired.  I am of the personal belief that this sort of technology should not be monopolized.  Yet even without a monopoly there is still lots of room for large profits.  The fact that Rostock is now public domain tickles me pink.

On Monday, December 9, 2013 at 11:16:05 PM UTC-5, Steve Graves wrote:
Mike,
I have looked at the trilateration formulas.  It is a better solution.  The circumcenter solution only works when the three arms are of equal length.  The trilateration solution works for different lengths.  I am particularly interested in a possible solution to printing houses.  I believe that it is possible to use three cranes instead of the present gantry crane solution.  In this case the lengths of the arms are the variables to control and the inverse kinematics are fairly simple.  Now I have a simple forward kinematics solution as well.

Steve


On Monday, December 9, 2013 5:37:43 PM UTC-5, Michael Paauwe wrote:
That's really cool.  I did this exercise a year or so ago but only published some snippets here and possibly not quite as thoroughly as you have.
I followed almost exactly the same route you did.
A few things I did differently though.
In solving the forward kinematics I used trilateration and found the intersection of 3 spheres centred on the carriages.  This resulted in a very similar amount of computation.
In assessing the error I divided the error by the step size to normalise the result and make a comparison with a cartesian printer easier.

I posted some graphics that look just like some of yours.

Cheers for taking the time to publish this.

Regards,
Mike Paauwe



Thinkyhead

unread,
Jun 1, 2016, 8:34:43 AM6/1/16
to Delta robot 3D printers
Hey Steve, I'm implementing the Forward Kinematics for Marlin Firmware, and I have a question about the sample code.

At the beginning of the function, there's a loop where these variables are assigned:

dColP[iIdx][0] = dCol[iIdx][0];
dColP[iIdx][1] = dCol[iIdx][1];
dColP[iIdx][2] = dZ[iIdx];

I don't see the dCol[][] array defined anywhere. Is that the correct variable? And where is it supposed to be defined?

Thanks!
—Scott

Steven Graves

unread,
Jun 2, 2016, 7:57:54 PM6/2/16
to delt...@googlegroups.com
Thinkyhead,
Sorry for the delay.  I hardly ever check my gmail account.  I have too many accounts!

That code is from a Java Application I wrote to play around with the delta stuff.  The code is below.

I don't know exactly what the code does now.  It has a bunch of modes of operation.  I was playing around with seeing how all sorts of different errors affected the accuracy of a delta.  This code was not written for public consumption so I would hard code various modes (I think I have UI placeholders for maybe someday making them changeable from the UI). The dCol variables are globals that could be set to various values representing the column positions.  I have moved them into any number of configurations.  The standard delta configuration based on the dRadius value is generated by the calcValues function.


<code>
package com.highergraphics.delta3d;
/**
 * This app will show a color topography map of the X-Y error for a delta CNC router/3D printer.
 * The error will be calculated by varying each carriage around its ideal value and finding the maximum error.
 * All combinations of carriage variations will be tried.
 * 
 * The inputs to the system are column spacing, effector arm length, carriage error value.
 * We were assuming a zero offset, which is the same as using a "virtual column position".
 * But now we have added Effector platform offset and carriage offset as inputs.  Now they
 * are used to calculate the virtual column position and the column spacing input is
 * the real column positions.
 * 
 */

import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.EventQueue;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.ComponentEvent;
import java.awt.event.ComponentListener;
import java.awt.event.WindowEvent;
import java.awt.event.WindowStateListener;
import java.awt.geom.AffineTransform;
import java.awt.geom.Rectangle2D;
import java.awt.image.BufferedImage;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.Properties;

import javax.imageio.ImageIO;
import javax.swing.JFileChooser;
import javax.swing.JFrame;
import javax.swing.JMenu;
import javax.swing.JMenuBar;
import javax.swing.JMenuItem;
import javax.swing.JPanel;
import javax.swing.SwingConstants;
import javax.swing.SwingWorker;
import javax.swing.filechooser.FileNameExtensionFilter;
import java.awt.FlowLayout;
import javax.swing.JTextField;
import java.awt.GridBagLayout;
import java.awt.GridBagConstraints;
import java.awt.Insets;
import javax.swing.JLabel;
import javax.swing.JRadioButton;
import javax.swing.border.EtchedBorder;
import javax.swing.ButtonGroup;
import javax.swing.JProgressBar;

public class HGdelta3DErrApp {

private JFrame frame;

/**
* Launch the application.
*/
public static void main(String[] args) {
EventQueue.invokeLater(new Runnable() {
public void run() {
try {
HGdelta3DErrApp window = new HGdelta3DErrApp();
window.frame.setVisible(true);
} catch (Exception e) {
e.printStackTrace();
}
}
});
}

/**
* Create the application.
*/
public HGdelta3DErrApp() {
initialize();
readIni();
}

/**
* Initialize the contents of the frame.
*/
@SuppressWarnings("serial")
private void initialize() {
frame = new JFrame();
frame.setBounds(100, 100, 483, 431);
frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
frame.getContentPane().setLayout(new BorderLayout(0, 0));
JMenuBar menuBar = new JMenuBar();
frame.getContentPane().add(menuBar, BorderLayout.NORTH);
JMenu mnFile = new JMenu("File");
menuBar.add(mnFile);
JMenuItem mntmSaveAs = new JMenuItem("Save As...");
mntmSaveAs.addActionListener(new ActionListener() {
public void actionPerformed(ActionEvent arg0) {
saveFile();
}
});
mnFile.add(mntmSaveAs);
JPanel pnlProgress = new JPanel();
frame.getContentPane().add(pnlProgress, BorderLayout.SOUTH);
GridBagLayout gbl_pnlProgress = new GridBagLayout();
gbl_pnlProgress.columnWidths = new int[]{0, 160, 146, 0};
gbl_pnlProgress.rowHeights = new int[]{14, 0};
gbl_pnlProgress.columnWeights = new double[]{0.0, 0.0, 0.0, Double.MIN_VALUE};
gbl_pnlProgress.rowWeights = new double[]{0.0, Double.MIN_VALUE};
pnlProgress.setLayout(gbl_pnlProgress);
lblProgress = new JLabel("1st pass calculating errors");
GridBagConstraints gbc_lblProgress = new GridBagConstraints();
gbc_lblProgress.insets = new Insets(0, 10, 0, 5);
gbc_lblProgress.gridx = 0;
gbc_lblProgress.gridy = 0;
pnlProgress.add(lblProgress, gbc_lblProgress);
progressBar = new JProgressBar();
GridBagConstraints gbc_progressBar = new GridBagConstraints();
gbc_progressBar.gridwidth = 2;
gbc_progressBar.fill = GridBagConstraints.HORIZONTAL;
gbc_progressBar.insets = new Insets(5, 20, 0, 0);
gbc_progressBar.gridx = 1;
gbc_progressBar.gridy = 0;
pnlProgress.add(progressBar, gbc_progressBar);
pnlImg = new JPanel(){
@Override
public void paintComponent(Graphics arg0) {
super.paintComponent(arg0);
if(theImage == null)return;
paintImg(arg0, pnlImg.getBounds());
}
};
frame.getContentPane().add(pnlImg, BorderLayout.CENTER);
pnlImg.setLayout(new FlowLayout(FlowLayout.CENTER, 5, 5));
calcValues();
frame.setBounds(0, 0, 800, 600);
hgCtlDiag.setBounds(0, 0, 550, 400);
hgCtlDiag.addComponentListener(new ComponentListener() {
@Override
public void componentShown(ComponentEvent e) {
}
@Override
public void componentResized(ComponentEvent e) {
}
@Override
public void componentMoved(ComponentEvent e) {
}
@Override
public void componentHidden(ComponentEvent e) {
System.exit(0);
}
});
hgCtlDiag.getBtnCalc().addActionListener(new ActionListener() {
@Override
public void actionPerformed(ActionEvent e) {
hgCtlDiagCmd(); }
});
hgCtlDiag.setVisible(true);
}
HGdeltaErrCtrlDialog hgCtlDiag = new HGdeltaErrCtrlDialog();
boolean bUnitsMM = true;
double dRadius = 124;
double dColSpace = dRadius * Math.sqrt(3d);
double dArmLen = 175;
int iMode = 4;
double dTestStep = 0.5;
double dError = 0.01;
boolean bSingle = false;
void hgCtlDiagCmd(){
bUnitsMM = hgCtlDiag.areUnitsMM();
dRadius = hgCtlDiag.getRadius() - hgCtlDiag.getEffectorOffset() - hgCtlDiag.getCarriageOffset();
dColSpace = dRadius * Math.sqrt(3d);
dArmLen = hgCtlDiag.getArmLen();
dArmSqrd = dArmLen * dArmLen;
dError = hgCtlDiag.getCarriageError();
bSingle = hgCtlDiag.isSingleCarriage();
dTestStep = hgCtlDiag.getTestStep();
if(hgCtlDiag.getRdbtnErrorInX().isSelected()){
iMode = 0;
}else if(hgCtlDiag.getRdbtnErrorInY().isSelected()){
iMode = 1;
}else if(hgCtlDiag.getRdbtnErrorInZ().isSelected()){
iMode = 2;
}else if(hgCtlDiag.getRdbtnErrorInXY().isSelected()){
iMode = 3;
}else{
iMode = 4;
createImageTask();
}
void createImageTask(){
SwingWorker<Void, Void> sw = new SwingWorker<Void, Void>() {
@Override
protected Void doInBackground() throws Exception {
createImage();
return null;
}
};
sw.execute();
}
void createImage(){
double dRange = 2 * dArmLen - dColSpace;
if(dRange < 1.5 * dColSpace)dRange = 1.5 * dColSpace;
dErrArray = new double[(int)(dRange/dTestStep)][(int)(dRange/dTestStep)];
double dMinX = dRange;
double dMinY = dRange;
double dMaxX = -dRange;
double dMaxY = -dRange;
int iMinI = 0;
int iMaxI = 0;
int iMinJ = 0;
int iMaxJ = 0;
double dMinErr = 100;
double dMaxErr = 0;
lblProgress.setText("1st pass - calculating errors");
progressBar.setStringPainted(true);
progressBar.setMaximum(100);
progressBar.setValue(0);
int iCnt = 0;
int iMax = (int)(dRange/dTestStep);
for(int i=0; i < iMax; i++){
for(int j=0; j < iMax; j++){
double dTestX = i*dTestStep - dRange/2;
double dTestY = dRange/2 - j*dTestStep;
double dErr = calcError(new double[]{dTestX, dTestY, 0}, iMode);
dErrArray[i][j] = dErr;
if(dErr >= 0){
if(dTestX < dMinX){
iMinI = i;
dMinX = dTestX;
}
if(dTestY < dMinY){
iMaxJ = j;
dMinY = dTestY;
}
if(dTestX > dMaxX){
iMaxI = i;
dMaxX = dTestX;
}
if(dTestY > dMaxY){
iMinJ = j;
dMaxY = dTestY;
}
if(dErr < dMinErr)dMinErr = dErr;
if(dErr > dMaxErr)dMaxErr = dErr;
}
}
iCnt++;
progressBar.setValue((int) (100 * (double)iCnt/(double)iMax));
progressBar.repaint();
}
if(dMinY > dCol[0][1] - 1){
dMinY =  dCol[0][1] - 1;
iMaxJ = (int) ((dRange/2 - dMinY)/dTestStep);
}
if(dMaxY < dCol[2][1] + 1){
dMaxY = dCol[2][1] + 1;
iMinJ = (int) ((dRange/2 - dMaxY)/dTestStep);
}
int iHeader = 90;
int iVBorder = 10;
int iHBorder = 10;
int iFooter = 50;
int iImgWidth = iMaxI - iMinI + 1; 
int iImgHeight = iMaxJ - iMinJ + 1;
if(2 * iHBorder + iImgWidth < 540)iHBorder = (540 - iImgWidth)/2;
int iWidth = 2 * iHBorder + iImgWidth;
int iHeight = iHeader + 2 * iVBorder + iImgHeight + iFooter;
theImage = new BufferedImage(iWidth, iHeight, BufferedImage.TYPE_INT_RGB);
Graphics2D g2 = theImage.createGraphics();
        RenderingHints rh = new RenderingHints(RenderingHints.KEY_RENDERING, RenderingHints.VALUE_RENDER_QUALITY);
        rh.put(RenderingHints.KEY_DITHERING, RenderingHints.VALUE_DITHER_ENABLE);
        rh.put(RenderingHints.KEY_ALPHA_INTERPOLATION, RenderingHints.VALUE_ALPHA_INTERPOLATION_QUALITY);
        rh.put(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
g2.setColor(Color.white);
g2.fillRect(0, 0, iWidth, iHeight);
lblProgress.setText("2nd pass - generating image");
progressBar.setMaximum(100);
progressBar.setValue(0);
iMax = (iMaxI - iMinI) * (iMaxJ - iMinJ);
iCnt = 0;
for(int i=iMinI; i <= iMaxI; i++){
for(int j=iMinJ; j <= iMaxJ; j++){
double dErr = dErrArray[i][j];
Color colorPix = Color.white;
if(dErr >= 0){
dErr = (dErr - dMinErr)/(dMaxErr - dMinErr);
colorPix = getColor(dErr);
}
theImage.setRGB(i - iMinI + iHBorder, j - iMinJ + iHeader + iVBorder, colorPix.getRGB());
iCnt++;
progressBar.setValue((int) (100 * (double)iCnt/(double)iMax));
progressBar.repaint();
//pnlImg.repaint();
}
}
g2.setColor(Color.black);
for(int iIdx = 0; iIdx < 3; iIdx++){
int x = iHBorder + (int) ((dCol[iIdx][0] - dMinX)/dTestStep);
int y = iHeader + iVBorder + (int) ((dMaxY - dCol[iIdx][1])/dTestStep);
g2.fillOval(x - 10, y-10, 20, 20);
}
g2.drawString(sTitle, 250, 20);
String sUnits = "in.";
if(bUnitsMM)sUnits = "mm";
String sCol = String.format("Virtual Column Spacing - %.2f %s", new Object[]{dColSpace, sUnits});
g2.drawString(sCol, 20, 20);
String sRad = String.format("Virtual Column Radius - %.2f %s", new Object[]{dRadius, sUnits});
g2.drawString(sRad, 20, 35);
String sArm = String.format("Arm Length - %.2f %s", new Object[]{dArmLen, sUnits});
g2.drawString(sArm, 20, 50);
String sAcmeErr = String.format("Carriage Error - %.4f %s", new Object[]{dError, sUnits});
g2.drawString(sAcmeErr, 20, 65);
String sErrorMode = "Error Mode - ";
if(bSingle){
sErrorMode += "Single Column";
}else{
sErrorMode += "Multiple Columns";
}
g2.drawString(sErrorMode, 20, 80);
int iFooterTop = iHeader + 2 * iVBorder + iImgHeight;
for(int iIdx = 0; iIdx < 11; iIdx++){
g2.drawString(String.format("%.4f", new Object[]{dMaxErr - iIdx * 0.10 * (dMaxErr - dMinErr)}), 20 + iIdx * 45, iFooterTop + 15);
}
for(int iIdx = 0; iIdx < 10; iIdx++){
double dLegend = dMaxErr - (((double)iIdx * 0.10) + 0.05) * (dMaxErr - dMinErr);
g2.setColor(getColor((dLegend - dMinErr)/(dMaxErr - dMinErr)));
g2.fillRect(52 + iIdx * 45, iFooterTop + 20, 20, 20);
}
pnlImg.repaint();
}
Color getColor(double dErr){
int[] iMaxColor = {0, 0, 0};
int[] iMinColor = {0, 0, 0};
dErr = Math.floor(10*dErr)/10;
int[] iColor = {0, 0, 0};
if(dErr > 0.75){
iMaxColor = new int[]{255,0,0};
iMinColor = new int[]{255,255,0};
dErr -= 0.75;
}else if(dErr > 0.5){
iMaxColor = new int[]{255,255,0};
iMinColor = new int[]{0,255,255};
dErr -= 0.5;
}else if(dErr > 0.25){
iMaxColor = new int[]{0,255,255};
iMinColor = new int[]{0,0,255};
dErr -= 0.25;
}else{
iMaxColor = new int[]{0,0,255};
iMinColor = new int[]{255,0,255};
}
dErr *= 4;
for(int iIdx = 0; iIdx < 3; iIdx++){
iColor[iIdx] = iMinColor[iIdx] + (int)(dErr * (double)(iMaxColor[iIdx]- iMinColor[iIdx]));
}
return new Color(iColor[0], iColor[1], iColor[2]);
}
/**
* We are changing the way the image is laid out.
*/
//double dMaxErr = 0.0025;
double[][] dErrArray;
double[][] dCol = new double[3][3];
// double[] dColX = new double[3];
// double[] dColY = new double[3];
double dArmSqrd = dArmLen * dArmLen;
double dBaseZ = Math.sqrt(dArmSqrd - Math.pow(dRadius, 2)); 
/**
* Calculate the locations of the columns based on column spacing.
* The columns are equally spaced (120 degrees apart) on a circle 
* centered at 0,0 with one column on the positive y axis.
*/
void calcValues(){
// dColX[0] = 0;
// dColY[0] = dRadius;
// dColX[1] = dRadius * Math.cos(Math.PI/6);
// dColY[1] = -dRadius * Math.cos(Math.PI/3);
// dColX[2] = -dColX[1];
// dColY[2] = dColY[1];
dCol[0][0] = -dRadius * Math.cos(Math.PI/6);
dCol[0][1] = -dRadius * Math.sin(Math.PI/6);
dCol[0][2] = 0;
dCol[1][0] = dRadius * Math.cos(Math.PI/6);
dCol[1][1] = -dRadius * Math.sin(Math.PI/6);
dCol[1][2] = 0;
dCol[2][0] = 0;
dCol[2][1] = dRadius;
dCol[2][2] = 0;
}
/**
* This calculates the error at a given x,y location.  The possible modes are
*  X error
*  Y error
*  X-Y error
*  Total error
*  
* @param dX
* @param dY
* @param iMode
* @return
*/
boolean bExit = true;
String sTitle = "";
String sMode = "";
double calcError(double[] dP, int iMode){
for(int iIdx = 0; iIdx < 3; iIdx++){
double dTemp = vectorDotProd(dCol[iIdx], dP);
if(dTemp >= dRadius * dRadius)return -1;
}
//First we calculate the exact values for each col Z
//double[] dP = {dX, dY, 0};
double[] dZ = new double[3];
for(int iIdx = 0; iIdx < 3; iIdx++){
//dZ[iIdx] = Math.sqrt(dArmSqrd - (Math.pow((dColX[iIdx] - dX), 2) + Math.pow((dColY[iIdx] - dY), 2)));
dZ[iIdx] = dArmSqrd - (Math.pow((dCol[iIdx][0] - dP[0]), 2) + Math.pow((dCol[iIdx][1] - dP[1]), 2));
if(dZ[iIdx] < 0) return -1;
dZ[iIdx] = Math.sqrt(dZ[iIdx]);
}
//if(bExit)return 0;
// System.out.println(String.format("DZ =>%.2f, %.2f, %.2f", new Object[]{dZ[0], dZ[1], dZ[2]}));
//Now add error to each d and find total error
double dErr = 0;
for(double i = -1; i < 2; i++){
for(double j = -1; j < 2; j++){
for(double k = -1; k < 2; k++){
if(i == 0 && j == 0 && k == 0)continue; //No error
if(bSingle){
//Only one column error at a time
if(i != 0 && j != 0)continue;
if(i != 0 && k != 0)continue;
if(j != 0 && k != 0)continue;
}
// double[] dErrPos2 = forwardKinematicstl(new double[]{dZ[0] + i*dError, dZ[1] + j*dError, dZ[2] + k*dError});
double[] dErrPos = forwardKinematics(new double[]{dZ[0] + i*dError, dZ[1] + j*dError, dZ[2] + k*dError});
// for(int iIdx = 0; iIdx < 3; iIdx++){
// double dDiff = Math.abs(dErrPos[iIdx] - dErrPos2[iIdx]);
// if(dDiff > 0.0001){
// System.out.println("Error of " + Double.toString(dDiff));
// }
// }
double dThisErr = 0;
switch(iMode){
case 0:
dThisErr = Math.abs(dP[0] - dErrPos[0]);
sMode = "X";
sTitle = "Max Error in X";
break;
case 1:
dThisErr = Math.abs(dP[1] - dErrPos[1]);
sMode = "Y";
sTitle = "Max Error in Y";
break;
case 2:
dThisErr = Math.abs(dP[2] - dErrPos[2]);
sMode = "Z";
sTitle = "Max Error in Z";
break;
case 3:
dThisErr = Math.sqrt(Math.pow(dP[0] - dErrPos[0], 2) + Math.pow(dP[1] - dErrPos[1], 2));
sMode = "X-Y";
sTitle = "Max Error in X-Y";
break;
case 4:
dThisErr = Math.sqrt(Math.pow(dP[0] - dErrPos[0], 2) + Math.pow(dP[1] - dErrPos[1], 2) + Math.pow(dErrPos[2], 2));
sMode = "X-Y-Z";
sTitle = "Max Error in X-Y-Z";
break;
}
if(dErr < dThisErr)dErr = dThisErr;
}
}
}
return dErr;
//double[] dLoc = forwardKinematics(dZ);
// String sIn = "";
// String sOut = "";
// String sDiff = "";
// for(int iIdx = 0; iIdx < 3; iIdx++){
// //sOut += Double.toString(dP[iIdx] - dLoc[iIdx]) + " ";
// sIn += String.format("IN =>%.2f, ", new Object[]{dP[iIdx]});
// sOut += String.format("OUT=>%.2f, ", new Object[]{dLoc[iIdx]});
// sDiff += String.format("DIF=>%.2f, ", new Object[]{dP[iIdx] - dLoc[iIdx]});
// }
//// System.out.println(sIn);
//// System.out.println(sOut);
// System.out.println(sDiff);
}
/*
* The forward kinematics calculate the intersection of three spheres centered on the points
* on each column with the given Z value. The equations for these spheres are:
* (dX - dX0)^2 + (dY - dY0)^2 + (dZ - dZ0)^2 = dArmSqrd
* (dX - dX1)^2 + (dY - dY1)^2 + (dZ - dZ1)^2 = dArmSqrd
* (dX - dX2)^2 + (dY - dY2)^2 + (dZ - dZ2)^2 = dArmSqrd
* We have three equations with three unknowns, but because of the squares, the solutions aren't trivial.
* So we need to find a frame of reference that simplifies the problem.  Upon inspection we can see that we
* have a tetrahedron where we know 3 points (and therefore the lengths the sides between those points) and
* length of the sides (dArmLen) from each of those points to the fourth unknown point.  Starting with the
* three points as the base of an irregular pyramid (tetrahedron) and using vectors we can find the fourth
* point. Because we have three equal sides we have three equilateral triangles. With the unknown point at
* the apex of these equilateral triangles.  One property of equilateral triangles is that a line at a right
* angle to the midpoint of the base will intersect the apex. If we view the tetrahedron from a point at a
* right angle to the plane including the three bases, this property still applies.  So the point where a
* line at right angles to this plane intersects the apex (unknown point) is at the intersection of the
* three lines going at right angles to the midpoints of the bases.  In reality, the third line is redundant,
* one only needs the intersection of two lines to define a point.  With the lengths of these lines one can
* also calculate the length of the right angle line from this point to the apex. Using vector math one can
* define the point in the original coordinate system.
*/
double[] forwardKinematics(double[] dZ){
double[][] dColP = new double[3][3];
for(int iIdx = 0; iIdx < 3; iIdx++){
dColP[iIdx][0] = dCol[iIdx][0];
dColP[iIdx][1] = dCol[iIdx][1];
dColP[iIdx][2] = dZ[iIdx];
}
//dColP has the three points on the columns
//As discussed in Wikipedia "Trilateration" we are establishing a new coordinate
//system in the plane of the three carriage points.  This system will have the
//origin at dColP[0] and dColP[1] is on the x axis.  dColP[2] is in the X-Y
//plane with a Z component of zero.  We will define unit vectors in this coordinate
//system in our original coordinate system.  Then when we calculate the Xnew, Ynew
//and Znew values, we can translate back into the original system by moving along
//those unit vectors by the corresponding values.
//We try to use the variable names from the Wikipedia article.
//Create a vector in old coords along x axis of new coord
double[] p12 = vectorSub(dColP[1], dColP[0]);
//Get the Magnitude of vector.
double d = vectorMag(p12);
//Create unit vector by dividing by magnitude.
double[] ex = vectorMult(p12, 1/d);
//Now find vector from the origin of the new system to the third point
double[] p13 = vectorSub(dColP[2], dColP[0]);
//Now use dot product to find the component of this vector on the X axis
double i = vectorDotProd(ex, p13);
//Now create a vector along the x axis that represents the x component of p13
double[] iex = vectorMult(ex, i);
//Now subtract the X component away from the original vector leaving only the Y
//component.  We use the variable that will be the unit vector after we scale it.
double[] ey = vectorSub(p13, iex);
//The magnitude of Y component
double j = vectorMag(ey);
//Now make vector a unit vector
ey = vectorMult(ey, 1/j);
//The cross product of the unit x and y is the unit z
double[] ez = vectorCrossProd(ex, ey);
//Now we have the d, i and j values defined in Wikipedia. We can plug them into
//the equations defined in Wikipedia for Xnew, Ynew and Znew
//Xnew = (r1^2 - r2^2 - d^2)/2*d = (L^2 - L^2 - d^2)/2d = d/2; 
double Xnew = d/2;
//Ynew = (r1^2 - r3^2 + i^2 + j^2)/2*j - i * Xnew/j
//     = ((L^2 - L^2 + i^2 + j^2)/2 - i * Xnew)/j = ((i^2 + j^2)/2 - i * Xnew)/j
double Ynew = ((i * i + j * j)/2 - i * Xnew)/j;
//Znew = sqrt(r1^2 - Xnew^2 - Ynew^2) = sqrt(L^2 - Xnew^2 - Ynew^2)
double Znew = Math.sqrt(dArmSqrd - Xnew * Xnew - Ynew * Ynew);
//Now we can start from the origin in the old coords and add vectors in the old
//coords that represent the Xnew, Ynew and Znew to find the point in the old system
double[] cartesian = vectorAdd(dColP[0], vectorMult(ex, Xnew));
cartesian = vectorAdd(cartesian, vectorMult(ey, Ynew));
cartesian = vectorAdd(cartesian, vectorMult(ez, -Znew));
return cartesian;
}
/**
* See "Barycentric coordinates from cross- and dot-products" for "Circumscribed circle"
* entry in wikipedia for formula used for circumcenter of triangle
* @param dZ
* @return
*/
double[] forwardKinematicsCC(double[] dZ){
double[] cartesian = new double[3];
double[][] dColP = new double[3][3];
for(int iIdx = 0; iIdx < 3; iIdx++){
dColP[iIdx][0] = dCol[iIdx][0];
dColP[iIdx][1] = dCol[iIdx][1];
dColP[iIdx][2] = dZ[iIdx];
}
//dColP has the three points on the columns
double[] dv01 = vectorSub(dColP[1], dColP[0]);
double[] dv02 = vectorSub(dColP[2], dColP[0]);
double[] dv12 = vectorSub(dColP[2], dColP[1]);
double dMag01 = vectorMag(dv01);
double dMag02 = vectorMag(dv02);
double dMag12 = vectorMag(dv12);
double[] dvZ = vectorCrossProd(dv02, dv01);
double dMagZ = vectorMag(dvZ);
double dDeterminate = 2 * Math.pow(dMagZ, 2);
double alpha = Math.pow(dMag12, 2) * vectorDotProd(dv01, dv02)/dDeterminate; 
double beta = -Math.pow(dMag02, 2) * vectorDotProd(dv12, dv01)/dDeterminate; 
double gamma = Math.pow(dMag01, 2) * vectorDotProd(dv02, dv12)/dDeterminate; 
double[] pCircumcenter = vectorAdd(vectorMult(dColP[0], alpha), vectorMult(dColP[1], beta));
pCircumcenter = vectorAdd(pCircumcenter, vectorMult(dColP[2], gamma));
double[] dvCircumcenter = vectorSub(pCircumcenter, dColP[0]);
double dMag2Circumcenter = vectorMag(dvCircumcenter);
double dZLen = Math.sqrt(Math.pow(dArmLen, 2) - Math.pow(dMag2Circumcenter, 2));
cartesian = vectorAdd(pCircumcenter, vectorMult(dvZ, dZLen/dMagZ));
return cartesian;
}

double vectorMag(double[] vVect){
   //result = (a(0) ^ 2 + a(1) ^ 2 + a(2) ^ 2) ^ 0.5
return Math.sqrt(Math.pow(vVect[0], 2) + Math.pow(vVect[1], 2) + Math.pow(vVect[2], 2));
}

double[] vectorMult(double[] vVect, double dScalar){
double[] vRet = new double[vVect.length];
for(int iIdx = 0; iIdx < vRet.length; iIdx++){
vRet[iIdx] = vVect[iIdx] * dScalar;
}
return vRet;
}
// Sub vectorDotProd(a() As Double, b() As Double, result As Double)
//    result = a(0) * b(0) + a(1) * b(1) + a(2) * b(2)
//    End Sub
double vectorDotProd(double[] a, double[] b){
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
// Sub vectorCrossProd(a() As Double, b() As Double, result() As Double)
//    result(0) = a(1) * b(2) - a(2) * b(1)
//    result(1) = a(2) * b(0) - a(0) * b(2)
//    result(2) = a(0) * b(1) - a(1) * b(0)
double[] vectorCrossProd(double[] a, double[] b){
double[] vRet = new double[a.length];
vRet[0] = a[1] * b [2] - a[2] * b[1];
vRet[1] = a[2] * b [0] - a[0] * b[2];
vRet[2] = a[0] * b [1] - a[1] * b[0];
return vRet;
}

double[] vectorAdd(double[] a, double[] b){
double[] vRet = new double[a.length];
for(int iIdx = 0; iIdx < vRet.length; iIdx++){
vRet[iIdx] = a[iIdx] + b[iIdx];
}
return vRet;
}
// Sub vectorSub(a() As Double, b() As Double, result() As Double)
//  result(0) = a(0) - b(0)
//  result(1) = a(1) - b(1)
//  result(2) = a(2) - b(2)
double[] vectorSub(double[] a, double[] b){
double[] vRet = new double[a.length];
for(int iIdx = 0; iIdx < vRet.length; iIdx++){
vRet[iIdx] = a[iIdx] - b[iIdx];
}
return vRet;
}
BufferedImage theImage = null;
private JPanel pnlImg;
public void paintImg(Graphics arg0, Rectangle2D thisBounds) {
if(theImage == null)return;
// TODO Auto-generated method stub
Graphics2D g2 = (Graphics2D)arg0;
double dMenuSpace = 20;
//Determine map between images, 1st scaling
double dScale = (double)thisBounds.getWidth()/(double)theImage.getWidth();
if(dScale > (double)(thisBounds.getHeight() - dMenuSpace)/(double)theImage.getHeight())dScale = ((double)thisBounds.getHeight() - dMenuSpace)/(double)theImage.getHeight();
//Now translate origins in scaled dimensions
// double dXtrans = (double)thisBounds.getX() - dScale * (double)numImg.getMinX();
// double dYtrans = (double)thisBounds.getY() - dScale * (double)numImg.getMinY();
// double dXtrans = - dScale * (double)theImage.getMinX();
// double dYtrans = dScale * (double)theImage.getHeight();
AffineTransform mapImg = AffineTransform.getTranslateInstance(0, dMenuSpace);
mapImg.concatenate(AffineTransform.getScaleInstance(dScale, dScale));
g2.setTransform(mapImg);
g2.drawImage(theImage, null, (int)thisBounds.getX(), (int)thisBounds.getY());
}
String defDir = "";
private final ButtonGroup btngrpCols = new ButtonGroup();
private final ButtonGroup btngrpUnits = new ButtonGroup();
private JProgressBar progressBar;
private JLabel lblProgress;
void saveFile(){
        JFileChooser chooser = new JFileChooser();
        FileNameExtensionFilter filter = null;
        filter = new FileNameExtensionFilter("PNG files", new String []{"png"});
        chooser.setFileFilter(filter);
        String sColMode = "MultiCol";
        if(bSingle){
        sColMode = "SingleCol";
        }
        String sAutoFileName = String.format("Col%.2fArm%.2fErr%.4f%s%s.png", new Object[]{dColSpace, dArmLen, dError, sColMode, sMode});
        File defFile = new File(defDir + "\\" + sAutoFileName);
        chooser.setCurrentDirectory(new File(defDir));
        chooser.setSelectedFile(defFile);
        //if(fCurrentFile != null)chooser.setSelectedFile(fCurrentFile);
        //chooser.setCurrentDirectory(new File("C:\\Users\\Steve\\Documents\\Higher Graphics\\Customers\\S\\Steve"));
        //chooser.setCurrentDirectory(new File("C:\\Users\\Steve\\Documents\\Higher Graphics\\Plot\\BP\\BPI-356"));
        int returnVal = chooser.showSaveDialog(null);
        if(returnVal == JFileChooser.APPROVE_OPTION) {
            defDir = chooser.getCurrentDirectory().getAbsolutePath();
            //Here we get the shapes and create decals to output
            //DecalPart topPart = new DecalPart();
            File file = chooser.getSelectedFile();
            saveSelectedFile(file);
            writeIni();
        }
}
    private void saveSelectedFile(File file){
    try {
ImageIO.write(theImage, "PNG", file);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
    }
    public void writeIni()
    {
        File iniFile = new File("delta.ini");
        try
        {
            Properties processIni = new Properties();
            if(iniFile.exists())
            {
                FileInputStream fis = new FileInputStream(iniFile);
                processIni.load(fis);
            }
            FileOutputStream fos = new FileOutputStream(iniFile);
            processIni.setProperty("PATH", defDir);
            processIni.store(fos, "");
            fos.close();
        }
        catch(IOException ex) { }
    }
    
    void readIni(){
        File iniFile = new File("delta.ini");
        if(iniFile.exists()){
            try
            {
                FileInputStream fis = new FileInputStream(iniFile);
                Properties processIni = new Properties();
                processIni.load(fis);
                defDir = processIni.getProperty("PATH");
            }
            catch(IOException ex)
            {
            }
        }
    }
public JProgressBar getProgressBar() {
return progressBar;
}
public JLabel getLblProgress() {
return lblProgress;
}
}

</code>


--
You received this message because you are subscribed to a topic in the Google Groups "Delta robot 3D printers" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/deltabot/V6ATBdT43eU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to deltabot+u...@googlegroups.com.
To post to this group, send email to delt...@googlegroups.com.
Reply all
Reply to author
Forward
0 new messages