How to correctly compute direct kinematics for a delta robot?

java math computational-geometry robotics kinematics
George Profenza picture George Profenza · Aug 10, 2013 · Viewed 14.7k times · Source

I'm trying to put together a simple simulation for a delta robot and I'd like to use forward kinematics (direct kinematics) to compute the end effector's position in space by passing 3 angles.

I've started with the Trossen Robotics Forum Delta Robot Tutorial and I can understand most of the math, but not all. I'm lost at the last part in forward kinematics, when trying to compute the point where the 3 sphere's intersect. I've looked at spherical coordinates in general but couldn't work out the two angles used to find to rotate towards (to E(x,y,z)). I see they're solving the equation of a sphere, but that's where I get lost.

delta robot direct kinematics

delta robot direct kinematics

delta robot direct kinematics

A delta robot is a parallel robot (meaning the base and the end effector(head) always stay parallel). The base and end effector are equilateral triangles and the legs are (typically) placed at the middle of the triangle's sides.

The side of the base of the delta robot is marked f. The side of the effector of the delta robot is marked e. The upper part of the leg is marked rf and the lower side re.

The origin(O) is at the centre of the base triangle. The servo motors are at the middle of the base triangle's sides (F1,F2,F3). The joints are marked J1,J2,J3. The lower legs join the end effector at points E1,E2,E3 and E is the centre of the end effector triangle.

I can easily compute points F1,F2,F3 and J1,J2,J3. It's E1,E2,E3 I'm having issues with. From the explanations, I understand that point J1 gets translate inwards a bit (by half the end effector's median) to J1' and it becomes the centre of a sphere with radius re (lower leg length). Doing this for all joints will result in 3 spheres intersecting in the same place: E(x,y,z). By solving the sphere equation we find E(x,y,z).

There is also a formula explained:

dk equation 1

dk equation 2 but this is where I get lost. My math skills aren't great. Could someone please explain those in a simpler manner, for the less math savvy of us ?

I've also used the sample code provided which (if you have a WebGL enabled browser) you can run here. Click and drag to rotate the scene. To control the three angles use q/Q, w/W,e/E to decrease/increase angles.

Full code listing:

//Rhino measurements in cm
final float e = 21;//end effector side
final float f = 60.33;//base side
final float rf = 67.5;//upper leg length - radius of upper sphere
final float re = 95;//lower leg length - redius of lower sphere (with offset will join in E(x,y,z))

final float sqrt3 = sqrt(3.0);
final float sin120 = sqrt3/2.0;   
final float cos120 = -0.5;        
final float tan60 = sqrt3;
final float sin30 = 0.5;
final float tan30 = 1/sqrt3;
final float a120 = TWO_PI/3;
final float a60 = TWO_PI/6;

//bounds
final float minX = -200;
final float maxX = 200;
final float minY = -200;
final float maxY = 200;
final float minZ = -200;
final float maxZ = -10;
final float maxT = 54;
final float minT = -21;

float xp = 0;
float yp = 0;
float zp =-45;
float t1 = 0;//theta
float t2 = 0;
float t3 = 0;

float prevX;
float prevY;
float prevZ;
float prevT1;
float prevT2;
float prevT3;

boolean validPosition;
//cheap arcball
PVector offset,cameraRotation = new PVector(),cameraTargetRotation = new PVector();

void setup() {
  size(900,600,P3D);
}

void draw() {
  background(192);
  pushMatrix();
  translate(width * .5,height * .5,300);
  //rotateY(map(mouseX,0,width,-PI,PI));

  if (mousePressed && (mouseX > 300)){
    cameraTargetRotation.x += -float(mouseY-pmouseY);
    cameraTargetRotation.y +=  float(mouseX-pmouseX);
  }
  rotateX(radians(cameraRotation.x -= (cameraRotation.x - cameraTargetRotation.x) * .35));
  rotateY(radians(cameraRotation.y -= (cameraRotation.y - cameraTargetRotation.y) * .35));

  stroke(0);
  et(f,color(255));
  drawPoint(new PVector(),2,color(255,0,255));
  float[] t = new float[]{t1,t2,t3};
  for(int i = 0 ; i < 3; i++){
    float a = HALF_PI+(radians(120)*i);
    float r1 = f / 1.25 * tan(radians(30));
    float r2 = e / 1.25 * tan(radians(30));
    PVector F = new PVector(cos(a) * r1,sin(a) * r1,0);
    PVector E = new PVector(cos(a) * r2,sin(a) * r2,0);
    E.add(xp,yp,zp);
    //J = F * rxMat
    PMatrix3D m = new PMatrix3D();
    m.translate(F.x,F.y,F.z);
    m.rotateZ(a);
    m.rotateY(radians(t[i]));
    m.translate(rf,0,0);

    PVector J = new PVector();
    m.mult(new PVector(),J);
    line(F.x,F.y,F.z,J.x,J.y,J.z);
    line(E.x,E.y,E.z,J.x,J.y,J.z);
    drawPoint(F,2,color(255,0,0));
    drawPoint(J,2,color(255,255,0));
    drawPoint(E,2,color(0,255,0));
    //println(dist(F.x,F.y,F.z,J.x,J.y,J.z)+"\t"+rf);
    println(dist(E.x,E.y,E.z,J.x,J.y,J.z)+"\t"+re);//length should not change
  }
  pushMatrix();
    translate(xp,yp,zp);
    drawPoint(new PVector(),2,color(0,255,255));
    et(e,color(255));
    popMatrix();
  popMatrix(); 
}
void drawPoint(PVector p,float s,color c){
  pushMatrix();
    translate(p.x,p.y,p.z);
    fill(c);
    box(s);
  popMatrix();
}
void et(float r,color c){//draw equilateral triangle, r is radius ( median), c is colour
  pushMatrix();
  rotateZ(-HALF_PI);
  fill(c);
  beginShape();
  for(int i = 0 ; i < 3; i++)
    vertex(cos(a120*i) * r,sin(a120*i) * r,0);
  endShape(CLOSE);
  popMatrix();
}
void keyPressed(){
  float amt = 3;
  if(key == 'q') t1 -= amt;
  if(key == 'Q') t1 += amt;
  if(key == 'w') t2 -= amt;
  if(key == 'W') t2 += amt;
  if(key == 'e') t3 -= amt;
  if(key == 'E') t3 += amt;
  t1 = constrain(t1,minT,maxT);
  t2 = constrain(t2,minT,maxT);
  t3 = constrain(t3,minT,maxT);
  dk();
}

void ik() {
  if (xp < minX) { xp = minX; }
  if (xp > maxX) { xp = maxX; }
  if (yp < minX) { yp = minX; }
  if (yp > maxX) { yp = maxX; }
  if (zp < minZ) { zp = minZ; }
  if (zp > maxZ) { zp = maxZ; }

  validPosition = true;
  //set the first angle
  float theta1 = rotateYZ(xp, yp, zp);
  if (theta1 != 999) {
    float theta2 = rotateYZ(xp*cos120 + yp*sin120, yp*cos120-xp*sin120, zp);  // rotate coords to +120 deg
    if (theta2 != 999) {
      float theta3 = rotateYZ(xp*cos120 - yp*sin120, yp*cos120+xp*sin120, zp);  // rotate coords to -120 deg
      if (theta3 != 999) {
        //we succeeded - point exists
        if (theta1 <= maxT && theta2 <= maxT && theta3 <= maxT && theta1 >= minT && theta2 >= minT && theta3 >= minT ) { //bounds check
          t1 = theta1;
          t2 = theta2;
          t3 = theta3;
        } else {
          validPosition = false;
        }

      } else {
        validPosition = false;
      }
    } else {
      validPosition = false;
    }
  } else {
    validPosition = false;
  }

  //uh oh, we failed, revert to our last known good positions
  if ( !validPosition ) {
    xp = prevX;
    yp = prevY;
    zp = prevZ;
  }

}

void dk() {
  validPosition = true;

  float t = (f-e)*tan30/2;
  float dtr = PI/(float)180.0;

  float theta1 = dtr*t1;
  float theta2 = dtr*t2;
  float theta3 = dtr*t3;

  float y1 = -(t + rf*cos(theta1));
  float z1 = -rf*sin(theta1);

  float y2 = (t + rf*cos(theta2))*sin30;
  float x2 = y2*tan60;
  float z2 = -rf*sin(theta2);

  float y3 = (t + rf*cos(theta3))*sin30;
  float x3 = -y3*tan60;
  float z3 = -rf*sin(theta3);

  float dnm = (y2-y1)*x3-(y3-y1)*x2;

  float w1 = y1*y1 + z1*z1;
  float w2 = x2*x2 + y2*y2 + z2*z2;
  float w3 = x3*x3 + y3*y3 + z3*z3;

  // x = (a1*z + b1)/dnm
  float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
  float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

  // y = (a2*z + b2)/dnm;
  float a2 = -(z2-z1)*x3+(z3-z1)*x2;
  float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

  // a*z^2 + b*z + c = 0
  float a = a1*a1 + a2*a2 + dnm*dnm;
  float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
  float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

  // discriminant
  float d = b*b - (float)4.0*a*c;
  if (d < 0) { validPosition = false; }

  zp = -(float)0.5*(b+sqrt(d))/a;
  xp = (a1*zp + b1)/dnm;
  yp = (a2*zp + b2)/dnm;

  if (xp >= minX && xp <= maxX&& yp >= minX && yp <= maxX && zp >= minZ & zp <= maxZ) {  //bounds check
  } else {
    validPosition = false;
  }

  if ( !validPosition ) {    
    xp = prevX;
    yp = prevY;
    zp = prevZ;
    t1 = prevT1;
    t2 = prevT2;
    t3 = prevT3;  
  }

}

void  storePrev() {
  prevX = xp;
  prevY = yp;
  prevZ = zp;
  prevT1 = t1;
  prevT2 = t2;
  prevT3 = t3;
}

float rotateYZ(float x0, float y0, float z0) {
  float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
  y0 -= 0.5 * 0.57735    * e;    // shift center to edge
  // z = a + b*y
  float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
  float b = (y1-y0)/z0;
  // discriminant
  float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); 
  if (d < 0) return 999; // non-existing point
  float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
  float zj = a + b*yj;
  return 180.0*atan(-zj/(y1 - yj))/PI + ((yj>y1)?180.0:0.0);
}

The problem is, when visualizing, the lower part changes length (as you can see in the printed message0 and it shouldn't, which further adds to my confusion.

I've used the supplied C code in Java/Processing, but the programming language is least important.

[Edit by spektre]

I just had to add this picture (for didactic reasons).

  • the lined nonsense is not the best way for grasping the kinematics abilities
  • as I understand the base with the motors is on this image on the upper triangle plane
  • and the tool is on the bottom triangle plane

delta robot

Answer

Spektre picture Spektre · Jan 9, 2014

I would do it as follows (algebraic representation of graphic solution):

  1. compute F1,F2,F3;

  2. solve system

    // spheres from Ji to Ei ... parallelograms (use lower Z half sphere)
(x1-J1.x)^2 + (y1-J1.y)^2 +(z1-J1.z)^2 = re^2 
(x2-J2.x)^2 + (y2-J2.y)^2 +(z2-J2.z)^2 = re^2
(x3-J3.x)^2 + (y3-J3.y)^2 +(z3-J3.z)^2 = re^2
// Ei lies on the sphere
E1=(x1,y1,z1)
E2=(x2,y2,z2)
E3=(x3,y3,z3)
// Ei is parallel to Fi ... coordinate system must be adjusted 
// so base triangles are parallel with XY-plane
z1=z2
z1=z3
z2=z3
// distance between any Ei Ej must be always q
// else it is invalid position (kinematics get stuck or even damage)
|E1-E2|=q
|E1-E3|=q
|E2-E3|=q
// midpoint is just average of Ei
E=(E1+E2+E3)/3
    • where q is the joint distance |Ei-E| which is constant

[Notes]

Do not solve it manually

  • use derive or something to obtain algebraic solution
  • and use only valid solution
  • its quadratic system so there will be most likely more solutions so you have to check for the correct one

Just a silly question why don't you solve inverse kinematics

  • it is most likely what you need (if you just don't do a visualization only)
  • and also is a bit simpler in this case

Also when you use just direct kinematics

  • I am not entirely convinced that you should drive all 3 joints
  • most likely drive just 2 of them
  • and compute the 3.th so the kinematics stay in valid position

[Edit1]

There is one simplification that just appear to me:

  1. Ti = translate Ji towards the Z axis by q (parallel to XY plane)

  2. now if you just need to find intersection of 3 spheres from Ti

    • this point is E

  3. so Ei is now simple translation of E (inverse from the Ji translation)

PS. I hope you know how to compute angles when you have all the points ...

Related questions

Math.random() explanation

This is a pretty simple Java (though probably applicable to all programming) question: Math.random() returns a number between zero and one. If I want to return an integer between zero and hundred, I would do: (int) Math.floor(Math.…

Read More
How to evaluate a math expression given in string form?

I'm trying to write a Java routine to evaluate math expressions from String values like: "5+3" "10-40" "(1+10)*3" I want to avoid a lot of if-then-else statements. How can I do this?

Read More
Calculating powers of integers

Is there any other way in Java to calculate a power of an integer? I use Math.pow(a, b) now, but it returns a double, and that is usually a lot of work, and looks less clean when you …

Read More