Inverse-Kinematics: How to calculate angles for servos of a robotic arm to reach all possible points in a canvas?

Question

I have a robotic arm composed of 2 servo motors. I am trying to calculate inverse kinematics such that the arm is positioned in the middle of a canvas and can move to all possible points in both directions (left and right). This is an image of the system Image. The first servo moves 0-180 (Anti-clockwise). The second servo moves 0-180 (clockwise).

Here is my code:

    int L1 = 170;
    int L2 = 230;
    Vector shoulderV;
    Vector targetV;
    shoulderV = new Vector(0,0);
    targetV = new Vector(0,400);


    Vector difference = Vector.Subtract(targetV, shoulderV);
    double L3 = difference.Length;
    if (L3 > 400) { L3 = 400; }
    if (L3 < 170) { L3 = 170; }

    // a + b is the equivelant of the shoulder angle
    double a = Math.Acos((L1 * L1 + L3 * L3 - L2 * L2) / (2 * L1 * L3));  
    double b = Math.Atan(difference.Y / difference.X);

   // S1 is the shoulder angle
   double S1 = a + b;
  // S2 is the elbow angle
  double S2 = Math.Acos((L1 * L1 + L2 * L2 - L3 * L3) / (2 * L1 * L2));

  int shoulderAngle = Convert.ToInt16(Math.Round(S1 * 180 / Math.PI));
  if (shoulderAngle < 0) { shoulderAngle = 180 - shoulderAngle; }
  if (shoulderAngle > 180) { shoulderAngle = 180; }
  int elbowAngle = Convert.ToInt16(Math.Round(S2 * 180 / Math.PI));

  elbowAngle = 180 - elbowAngle;

Initially, when the system is first started, the arm is straightened with shoulder=90, elbow =0. When I give positive x values I get correct results in the left side of the canvas. However, I want the arm to move in the right side as well. I do not get correct values when I enter negatives. What am I doing wrong? Do I need an extra servo to reach points in the right side?

Sorry if the explanation is not good. English is not my first language.

Have you tried using Denavit hartenberg convention to do the inverse kinematics calculation? — Altronicx, Jun 10 '16 at 06:45

score 0 · Answer 1 · answered Jul 26 '16 at 08:11

I suspect that you are losing a sign when you are using Math.Atan(). I don't know what programming language or environment this is, but try and see if you have something like this:

Instead of this line:

double b = Math.Atan(difference.Y / difference.X);

Use something like this:

double b = Math.Atan2(difference.Y, difference.X);

When difference.Y and difference.X have the same sign, dividing them results in a positive value. That prevents you from differentiating between the cases when they are both positive and both negative. In that case, you cannot differentiate between 30 and 210 degrees, for example.

Inverse-Kinematics: How to calculate angles for servos of a robotic arm to reach all possible points in a canvas?

1 Answers1