Robot leg inverse kinematics and Generic Gait with ESP32

Intro

I have been experimenting with a simple robot leg built using SG series hobby servos and an ESP32, with the goal of developing my own gait sequences for quadruped robots. This project started as a way to better understand inverse kinematics on a small scale and gradually grew into an exploration of how multiple legs can be coordinated to create stable and repeatable walking patterns.

The work is inspired by the excellent “Simple Robot Leg Inverse Kinematics with ESP32” project, which demonstrates how a two-joint leg can be controlled using basic trigonometry. I used that idea as a foundation and expanded it toward gait generation rather than focusing only on a single leg.

Project Goal

The main goal of this project is to move from controlling one leg accurately to coordinating four legs in a predictable and reusable way. Instead of hard-coding servo angles, the leg is controlled by defining a desired foot position and letting inverse kinematics calculate the joint angles automatically.

Once this worked reliably, the next step was to design a generic gait algorithm that could be reused and modified for different walking styles such as crawl, wave, or diagonal stepping.

Leg Design and Control

Each leg uses two SG servos:

• One servo for the hip joint
• One servo for the knee joint

The leg is modeled as a simple two-link planar mechanism. By specifying an X and Y position for the foot relative to the hip joint, inverse kinematics is used to calculate the angles needed for both servos.

This approach makes the movement smoother and easier to tune, since changing step length or height only requires adjusting coordinates rather than servo angles.

Generic Quadruped Gait Algorithm

The quadruped gait is based on dividing movement into repeating phases. Each leg alternates between two main states:

• Stance phase: the foot stays on the ground and supports the body
• Swing phase: the foot is lifted, moved forward, and placed back down

A simple crawl gait is implemented by moving one leg at a time while the other three remain in stance. The order of leg movement determines the gait pattern.

Below is a simplified example showing how a generic gait loop and inverse kinematics can be implemented on an ESP32 using Arduino-style code.

// Servo pins for each leg
int hipPins[4]  = {14, 27, 26, 25};
int kneePins[4] = {12, 13, 33, 32};

// Leg geometry
const float L1 = 50.0; // hip to knee length
const float L2 = 50.0; // knee to foot length

// Gait parameters
float stepLength = 30;
float stepHeight = 20;
int stepDelay = 100;

// Crawl gait order
int gaitOrder[4] = {0, 1, 2, 3};

void setup() {
  for (int i = 0; i < 4; i++) {
    pinMode(hipPins[i], OUTPUT);
    pinMode(kneePins[i], OUTPUT);
  }
}

void loop() {
  for (int i = 0; i < 4; i++) {
    moveLeg(gaitOrder[i], stepLength, stepHeight);
    delay(stepDelay);
  }
}

void moveLeg(int leg, float forward, float lift) {
  // Lift and swing forward
  applyIK(leg, forward, -lift);
  delay(stepDelay);

  // Place foot down
  applyIK(leg, forward, 0);
  delay(stepDelay);
}

void applyIK(int leg, float x, float y) {
  float distance = sqrt(x * x + y * y);
  distance = constrain(distance, 10, L1 + L2 - 5);

  float angleA = atan2(y, x);
  float angleB = acos((L1 * L1 + distance * distance - L2 * L2) / (2 * L1 * distance));
  float angleC = acos((L1 * L1 + L2 * L2 - distance * distance) / (2 * L1 * L2));

  float hipAngle  = (angleA + angleB) * 180.0 / PI;
  float kneeAngle = angleC * 180.0 / PI;

  analogWrite(hipPins[leg], hipAngle);
  analogWrite(kneePins[leg], kneeAngle);
}

How the Gait Works

The gait loop cycles through each leg in a predefined order. For each leg:

• The foot is lifted by increasing its Y coordinate.
• The foot is moved forward by increasing its X coordinate.
• The foot is placed back on the ground.
• The body effectively moves forward relative to the grounded legs.

By adjusting parameters like step length, step height, delay time, and gait order, different walking styles can be tested without changing the core logic.

What This Enables

This structure allows:

• Easy tuning of walking speed and smoothness
• Reusable gait logic for different robot sizes
• Cleaner code compared to hard-coded servo angles
• A foundation for more advanced gaits

Because everything is based on coordinates and timing, it also becomes easier to later add features like body tilt, turning, or terrain adaptation.

Inverse Kinematics

Inverse kinematics (IK) is the process of calculating joint angles when the desired position of the foot is known. Instead of telling each servo what angle to move to, we tell the leg where the foot should be in space, and the math figures out how the joints must rotate to reach that point.

For a simple robot leg with two servos, this problem can be solved using basic trigonometry.

Leg Geometry Model

The leg is modeled as a two-link planar mechanism:

• Link 1 (L1): Hip joint to knee joint
• Link 2 (L2): Knee joint to foot

The foot position is defined by an X and Y coordinate relative to the hip joint.

Coordinate System

Y
          |
          |
          |      Foot (x, y)
          |        *
          |       /
          |      /
          |     /
          |    /
          |   /
          |  /
          | /
          |/
Hip (0,0) +-------------------- X

X controls forward and backward motion

• X controls forward and backward motion
• Y controls lifting and lowering the foot

Negative Y values lift the leg upward, which works well for walking gaits.

Triangle Representation

Once the foot position is known, the leg forms a triangle:

Foot
          *
         / \
        /   \
     L2/     \ Distance (D)
      /       \
     *---------*
   Knee        Hip
         L1

Where:

• L1 is the hip-to-knee length
• L2 is the knee-to-foot length
• D is the distance from hip to foot

Step 1: Distance to the Foot

First, compute the distance from the hip joint to the foot position using the Pythagorean theorem:

D = sqrt(x² + y²)

This value must be within the physical limits of the leg:

|L1 - L2| ≤ D ≤ (L1 + L2)

If D is outside this range, the leg cannot physically reach the target point.

Step 2: Hip Angle Calculation

The hip angle is made up of two parts:

• The angle from the X-axis to the foot position
• The internal angle of the triangle formed by L1, L2, and D

Angle to the Foot

α = atan2(y, x)

This angle points directly toward the foot position.

Triangle Angle at the Hip

Using the law of cosines:

β = acos((L1² + D² - L2²) / (2 * L1 * D))

Final Hip Angle

θ₁ = α + β

This is the angle sent to the hip servo.

Step 3: Knee Angle Calculation

The knee angle is also found using the law of cosines:

θ₂ = acos((L1² + L2² - D²) / (2 * L1 * L2))

This gives the internal knee angle between the two leg segments.

Converting to Servo Angles

Most servo libraries expect angles in degrees, not radians, so the final step is conversion:

degrees = radians * (180 / π)

Offsets may need to be added depending on how the servos are mounted mechanically.

Next Steps

Future improvements will focus on:

Adding IMU feedback for balance

• Adding IMU feedback for balanceImplementing diagonal and trot gaits
• Implementing diagonal and trot gaitsSmoothing transitions between stance and swing
• Smoothing transitions between stance and swingIntegrating sensors for obstacle detection
• Integrating sensors for obstacle detection
• Scaling the design to higher-torque servos

This project is intentionally simple, but it provides a solid foundation for experimenting with quadruped locomotion using inexpensive hardware and clear math.