Robotic Arm with Servos: Inverse Kinematics for a 2-Link Planar Arm

Most beginner robotic arms are controlled joint by joint: one potentiometer or slider per servo, moved by hand until the gripper reaches the object. That is fine for play but useless for automation, where you want to say "move the tip to x = 12 cm, y = 8 cm" and let the program work out the joint angles. That calculation is called inverse kinematics (IK).

This tutorial builds the IK for the simplest useful arm, two links moving in a vertical plane, like a shoulder and an elbow. You will derive the equations with the law of cosines, check that your servos have enough torque, and write an Arduino sketch that moves the arm tip along a straight line using the equations.

Forward Kinematics First

Let link 1 (shoulder to elbow) have length L1 and link 2 (elbow to tip) length L2. The shoulder angle theta1 is measured from the horizontal x axis, and the elbow angle theta2 is measured relative to link 1, so theta2 = 0 means the arm is straight. Forward kinematics gives the tip position from the angles.

x = L1*cos(theta1) + L2*cos(theta1 + theta2)
y = L1*sin(theta1) + L2*sin(theta1 + theta2)

Deriving the Inverse Kinematics

The shoulder, elbow and tip form a triangle with sides L1, L2 and the distance from the shoulder to the target, sqrt(x squared + y squared). The law of cosines gives the elbow angle directly. The shoulder angle is the direction to the target minus the angle that link 2 adds.

cos(theta2) = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2)
theta2      = +acos(...)   elbow-down    or    -acos(...)   elbow-up

theta1 = atan2(y, x) - atan2(L2*sin(theta2), L1 + L2*cos(theta2))

Reachable only if |cos(theta2)| <= 1, i.e. |L1 - L2| <= sqrt(x^2+y^2) <= L1 + L2

Worked example

L1 = 10 cm, L2 = 8 cm, target x = 12 cm, y = 6 cm
x^2 + y^2 = 144 + 36 = 180
cos(theta2) = (180 - 100 - 64) / (2*10*8) = 16 / 160 = 0.100
theta2 = -acos(0.100) = -84.3 deg   (elbow-up solution)

atan2(6, 12) = 26.6 deg
atan2(8*sin(-84.3), 10 + 8*cos(-84.3)) = atan2(-7.96, 10.80) = -36.4 deg
theta1 = 26.6 - (-36.4) = 63.0 deg

Check: x = 10cos63.0 + 8cos(-21.3) = 4.54 + 7.45 = 12.0 cm
       y = 10sin63.0 + 8sin(-21.3) = 8.91 - 2.91 = 6.0 cm

Always verify an IK solution by putting the angles back into forward kinematics, as in the last two lines. It catches sign mistakes immediately.

Hardware and Torque Check

PartSuggestedNotes
Shoulder servoMG996R (metal gear)Carries the whole arm, so it needs the most torque
Elbow servoMG996R or MG90SCarries link 2 and the gripper
LinksAluminium servo brackets or 3D printedKeep link 2 light
Servo supply5 V to 6 V, at least 3 ANever from the Arduino 5 V pin
ControllerArduino UnoShoulder signal on D9, elbow on D10

Worst-case torque on the shoulder occurs when the arm is stretched horizontally. Add up each mass multiplied by its horizontal distance from the shoulder axis.

Example: link 1 mass 40 g at 5 cm, elbow servo 55 g at 10 cm,
         link 2 + gripper 50 g at 14 cm, payload 30 g at 18 cm
Torque = 40*5 + 55*10 + 50*14 + 30*18 = 200 + 550 + 700 + 540
       = 1990 g*cm = about 2.0 kg*cm
With a safety factor of 2 to 3 -> choose a servo rated 4 to 6 kg*cm or more

Arduino IK Code

Servo angles rarely line up with the mathematical angles, so the code maps them with an offset and direction for each joint. Find these by commanding 90 degrees and measuring where each link actually points.

Two-link inverse kinematics driving two servos along a straight line
#include <Servo.h>
#include <math.h>

const float L1 = 10.0;   // cm, shoulder to elbow
const float L2 = 8.0;    // cm, elbow to tip

// Calibration: servoAngle = offset + dir * jointAngleDeg
const float SH_OFFSET = 0.0,  SH_DIR = 1.0;   // shoulder: 0 deg joint = horizontal
const float EL_OFFSET = 90.0, EL_DIR = 1.0;   // elbow: 0 deg joint = arm straight

Servo shoulder, elbow;

// Compute joint angles (degrees) for target (x, y). Returns false if unreachable.
bool solveIK(float x, float y, float &t1, float &t2, bool elbowUp) {
  float c2 = (x * x + y * y - L1 * L1 - L2 * L2) / (2.0 * L1 * L2);
  if (c2 < -1.0 || c2 > 1.0) return false;      // outside the workspace
  float th2 = acos(c2);
  if (elbowUp) th2 = -th2;
  float th1 = atan2(y, x) - atan2(L2 * sin(th2), L1 + L2 * cos(th2));
  t1 = th1 * 180.0 / PI;
  t2 = th2 * 180.0 / PI;
  return true;
}

bool moveTo(float x, float y) {
  float t1, t2;
  if (!solveIK(x, y, t1, t2, true)) return false;
  float s1 = SH_OFFSET + SH_DIR * t1;
  float s2 = EL_OFFSET + EL_DIR * t2;
  if (s1 < 0 || s1 > 180 || s2 < 0 || s2 > 180) return false; // servo cannot reach
  shoulder.write((int)(s1 + 0.5));
  elbow.write((int)(s2 + 0.5));
  return true;
}

// Move the tip in a straight line by solving IK at many small steps
void lineTo(float x0, float y0, float x1, float y1, int steps, int msPerStep) {
  for (int i = 0; i <= steps; i++) {
    float f = (float)i / steps;
    float x = x0 + (x1 - x0) * f;
    float y = y0 + (y1 - y0) * f;
    if (!moveTo(x, y)) {
      Serial.print("Unreachable: "); Serial.print(x); Serial.print(", "); Serial.println(y);
    }
    delay(msPerStep);
  }
}

void setup() {
  Serial.begin(9600);
  shoulder.attach(9);
  elbow.attach(10);
  moveTo(12.0, 6.0);   // start position from the worked example
  delay(1500);
}

void loop() {
  lineTo(12.0, 6.0, 12.0, 12.0, 60, 20);   // straight up
  lineTo(12.0, 12.0, 6.0, 12.0, 60, 20);   // straight left
  lineTo(6.0, 12.0, 12.0, 6.0, 60, 20);    // diagonal back
  delay(1000);
}

Because servos move on their own internal schedule, sending a large angle jump makes the tip sweep along a curve. Breaking the path into 60 small IK steps makes the tip follow an approximately straight line, which is how industrial arms do linear moves too, only much faster.

Troubleshooting

  • Arm moves to a mirror-image position: the elbow solution or the joint direction is flipped. Try the other elbowUp value or change EL_DIR to -1.
  • Many targets reported unreachable: either they are outside the circle of radius L1 + L2, or the servo's 0 to 180 degree range does not cover the required joint angle. Change the offsets or mount angle.
  • Arm drifts from the target by a constant amount: measure L1 and L2 between the actual rotation axes, not the bracket ends.
  • Servos jitter and the Arduino resets: the servo supply is too weak. Use a separate 5 V to 6 V supply with common ground.

Summary

Forward kinematics gives the tip position from joint angles; inverse kinematics reverses it using the law of cosines for the elbow and two atan2 terms for the shoulder. Check reachability, choose elbow-up or elbow-down, calibrate servo offsets, verify with forward kinematics, and size the shoulder servo from the worst-case horizontal torque.