How to determine rotation of 2 arm segments based on target point?

Question

I have a 2-segments robotic arm that needs to reach a specific point by setting up its joints (angles).

Here is a drawing of the setup:

My arm is positioned in the middle of the sketch so my origin is (width/2,0). These are the values that I know: Lengths: First segment (L1): 140 mm. Second segment (L2): 180 mm. Distance between the point of origin and the target point. (using dist() ).

These lengths form a triangle. So, using laws of trigonometric I can find the angles of the triangle which are the angles I want to use to position my arm so it reaches the target point.

Now, I want to draw the triangle on screen using Processing environment for simulation. I applied some transformations to draw the triangle but I am not getting the correct drawing.

This is my code:

void draw(){ 
background(100);
translate(width/2,0); // origin

// target
float penX = mouseX-width/2; 
float penY = mouseY;

// draw points
ellipse(penX, penY, 20, 20);
ellipse(0,0,20,20);
line(0,0,penX,penY);

// distance from origin to target
float distance = dist(0,0,penX,penY);

// first angle (part of S1)
float b = asin(penY/distance);
arc(0,0,100,100,0,b);

// second angle (part of S1)
float a = acos((pow(L1,2)+pow(distance,2)-pow(L2,2))/(2*L1*distance));

// Angle representing first joint
S1 = a + b; // first joint angle

// Angle representing second joint
S2 = acos((pow(L1,2)+pow(L2,2)-pow(distance,2))/(2*L1*L2)); //second joint angle
//Drawing Triangle:
rotate(S1);
line(0,0,120,0);
translate(120,0);

rotate(-S2);
line(0,0,180,0);}

I hope my writing is clear and sorry for any confusion.

Answer 1

This is going to be much more involved than you're describing. If you know the start point, the lengths of the arm segments, and the target point, then you can use inverse kinematics to come up with the angles your segments need to hit the target point. It's not as simple as applying simple trigonometry.

That being said, if you already know the angles of one of the segments, then you can indeed use basic trig to figure out the last one:

float midX;
float midY;

float length1 = 100;
float length2 = 100;

float angle1;

void setup(){
  size(500, 500);
  midX = width/2;
  midY = height/2;
}

void keyPressed(){
  angle1 += .05;
}

void draw() { 

  background(255);

  //we already know the angle of the first segment
  //so we can get the end point of the first segment
  float endX1 = midX + cos(angle1)*length1;
  float endY1 = midX + sin(angle1)*length2;

  //we don't know the angle of the second segment
  //but we can point it towards a goal point
  float deltaY = mouseY - endY1;
  float deltaX = mouseX - endX1;
  float angle2 = atan2(deltaY, deltaX);

  //now we figured out the angle,
  //we can get the end point of the second segment
  float endX2 = endX1 + cos(angle2)*length2;
  float endY2 = endY1 + sin(angle2)*length2;

  //draw the segments
  line(midX, midY, endX1, endY1);
  line(endX1, endY1, endX2, endY2);
}

However, you can't figure out all of your angles just using basic trig. For that, you have to use something more involved like inverse kinematics.

You can google something like "Processing inverse kinematics" for a bunch of results, but here is a great example:

int sx,sy,ex,ey,hx,hy,hxo,hyo;
int armLength,ua,la;
float uad, lad;
void setup(){
  size(500,500);
  background(255, 224, 150);
  sx = width/2;
  sy = height/2;
  armLength = int(width/5);
}

void draw(){
  fill(255);
  rect(0,0,width,height);
  upperArm();
}

void upperArm(){
  int dx = mouseX - sx;
  int dy = mouseY - sy;
  float distance = sqrt(dx*dx+dy*dy);

  int a = armLength;
  int b = armLength;
  float c = min(distance, a + b);

  float B = acos((b*b-a*a-c*c)/(-2*a*c));
  float C = acos((c*c-a*a-b*b)/(-2*a*b));

  float D = atan2(dy,dx);
  float E = D + B + PI + C;

  ex = int((cos(E) * a)) + sx;
  ey = int((sin(E) * a)) + sy;
  print("UpperArm Angle=  "+degrees(E)+"    ");

  hx = int((cos(D+B) * b)) + ex;
  hy = int((sin(D+B) * b)) + ey;
 println("LowerArm Angle=  "+degrees((D+B)));

  stroke(255,0,0,100);
  fill(240,0,0,200);
  ellipse(sx,sy,10,10);
  ellipse(ex,ey,8,8);
  ellipse(hx,hy,6,6);
  stroke(0);
  line(sx,sy,ex,ey);
  line(ex,ey,hx,hy);
  //float angle = atan2(dy, dx);
  //println("angle = " + degrees(angle))
  //ex = int((cos(angle) * r)) + sx;
  //ey = int((sin(angle) * r)) + sy;
  //line(sx,sy,ex,ey);
}