Maths/Code for forward kinematics of a 5-bar SCARA

Hi

I'm building a parallel SCARA similar to the Wally, except I'm planning on driving the bases of of the arms rather than the elbows.

My goal is to make a really cheap, mostly 3d printable printer that's as quiet as possible.

So just to make things extra interesting, I'm planning on using DC motors with optical encoders and really high gearing so I can get the accuracy/precision required. I started on some servos that use 25mm to 36mm DC motors, opto-interrupters and a chain of planetary gear stages based on emmet's gear bearings. They seem to work OK and I should move on to the next phase which would be to build a 2d plotter or something out of two of them. Here's a video of what I have so far (https://www.youtube.com/watch?v=D-2n4X12mcs), some pics of how it all fits together (http://imgur.com/a/fUezk) and some seriously messy openscad code (https://github.com/lerouxb/scarabot). Yes I'm planning on making it much smaller and I have some ideas for getting the arms fairly sturdy

I started on some javascript using paper.js (https://github.com/lerouxb/scarabot/blob/master/src/app.js) that will visualise the forward and inverse kinematics so I can get a feel for the build area and the precision across it. I found this paper which seems to have all the maths that I'll need in the first three pages (https://www.academia.edu/10259240/Kinematic_Analysis_of_Five_Bar_Mechanism_in_Industrial_Robotics) but for some reason the forward kinematics is kicking out non-sensical values. I'm testing it by basically giving it the parameters describing a regular pentagon (equal sides, all the internal angles 108 degrees each) as a sanity check, expecting the angles formed at A, B, C, D and E to all be 108 degrees, but I'm getting different values. If I draw it, plotting point C (the end effector) either via the left arm or right arm, I get different results. (http://cl.ly/1M0G2C0I2o1h)So I'm starting to think there might be an error in that paper and I haven't done much trigonometry since the 90s, so I can't find it myself and now I've been frustratingly stuck for days already and I really don't want to abandon this project just yet.

Does anyone know of some articles/papers/code somewhere that describes the forward kinematics in fairly basic or easy to follow/implement trigonometry or code? I can find the c code for the inverse kinematics from the various hacked up firmwares out there and just base things off that, but the firmwares don't contain code for the forward kinematics because that's not an operation they ever have to perform.

Any help would be greatly appreciated.

Edited 2 time(s). Last edit at 01/31/2016 09:58AM by lerouxb.

The big things you need to know:
-how to convert from cartesian to polar coordiantes
-how to apply the cosine rule


if you look at [wiki.fablab-nuernberg.de] page 12-14 the kinematics for a simplyfied version (4 link robot arm) are described (in german, but the maths should be readable)

For something more similar to your approach look at the inverse kinematics for the Plotclock [wiki.fablab-nuernberg.de] :

// length of arms
#define L1 35 //both lower arms
#define L2 55.1 //left upper arm to pen
#define L3 13.2 //pen to left upper arm joint
#define L4 45 //reight upper arm

// origin points of left and right servo 
#define O1X 24//22
#define O1Y -25
#define O2X 49//47
#define O2Y -25

double return_angle(double a, double b, double c) {
  // cosine rule for angle between c and a
  return acos((a * a + c * c - b * b) / (2 * a * c));
}

void set_XY(double Tx, double Ty)  // Tx, Ty = Target X,Y
{
  delay(1);
  double dx, dy, c, a1, a2, Hx, Hy;

// Calcutlating the left servo angle

  // calculate triangle between pen, servoLeft and arm joint
  // cartesian dx/dy
  dx = Tx - O1X;
  dy = Ty - O1Y;

  // polar lemgth (c) and angle (a1)
  c = sqrt(dx * dx + dy * dy); // 
  a1 = atan2(dy, dx); //
  a2 = return_angle(L1, L2, c);

  servo2.writeMicroseconds(floor(((a2 + a1 - M_PI) * SERVOFAKTORLEFT) + SERVOLEFTNULL));


// Calcutlating the right servo angle

  // calculate joint arm point for triangle of the right servo arm (joint near the pen)
  a2 = return_angle(L2, L1, c);
  Hx = Tx + L3 * cos((a1 - a2 + 0.621) + M_PI); //36,5°
  Hy = Ty + L3 * sin((a1 - a2 + 0.621) + M_PI);

  // calculate triangle between pen joint, servoRight and arm joint
  dx = Hx - O2X;
  dy = Hy - O2Y;

  c = sqrt(dx * dx + dy * dy);
  a1 = atan2(dy, dx);
  a2 = return_angle(L1, L4, c);

  servo3.writeMicroseconds(floor(((a1 - a2) * SERVOFAKTORRIGHT) + SERVORIGHTNULL));

}

Plotclock code at:
[www.thingiverse.com] (Version 1.03!)

Thanks I managed to figure it out. I found some maths for something http://forums.reprap.org/read.php?185,490216,page=2]very similar[/url] on this forum and then adapted it for mine.


Here's a video of my forward and reverse kinematics progress in operation.

I'll put the code up on github soon.