Guide to A Guide to Forward and Inverse Kinematics for a 3-DOF Robotic Arm via MicroPython

Why Kinematics Matter

Kinematics is the geometry of motion — and for any robotic arm, it defines how commands translate into physical movement. Whether you're designing a pick-and-place system, a camera pan-tilt rig, or a DIY lab assistant, understanding forward kinematics (FK) and inverse kinematics (IK) is essential.

Forward kinematics calculates the end-effector’s position from known joint angles, while inverse kinematics determines the joint angles needed to reach a desired position. In this guide, you’ll build a 3-degree-of-freedom (3-DOF) arm simulation and implementation — all powered by MicroPython on an ESP32 or similar microcontroller.

Understanding the 3-DOF Planar Arm

We’ll use a simple planar robotic arm with three revolute joints — often called a SCARA-like arm (Selective Compliance Assembly Robot Arm), though simplified for educational use. Its structure includes:

Base Joint (J0): Rotates horizontally around a vertical axis (often omitted in 2D planar models).
Shoulder Joint (J1): Rotates vertically to raise/lower the upper arm.
Elbow Joint (J2): Rotates to bend the arm at the elbow.
End-effector: The tool or gripper tip, whose position (x, y, z) we aim to control.

For simplicity and clarity in this tutorial, we’ll work in 2D: the arm moves in the x–y plane with z fixed (e.g., height from ground). This keeps the math tractable and intuitive while remaining highly applicable to real-world builds.

Arm Geometry Parameters

L₁ = 120 mm (upper arm length)
L₂ = 100 mm (forearm length)
L₃ = 40 mm (end-effector offset)

*Values are illustrative; adapt them to your physical build.*

Forward Kinematics: From Joint Angles to Position

Forward kinematics is like tracing a path: given three joint angles, how far does each link extend, and where does the end-effector land?

Given: L₁ = 120 mm, L₂ = 100 mm, angles θ₁, θ₂ in radians
Joint 1 (Shoulder) position:
x₁ = L₁ · cos(θ₁)y₁ = L₁ · sin(θ₁)Joint 2 (Elbow) position:
x₂ = x₁ + L₂ · cos(θ₁ + θ₂)y₂ = y₁ + L₂ · sin(θ₁ + θ₂)End-effector position:
xₑ = x₂ + L₃ · cos(θ₁ + θ₂ + θ₃)yₑ = y₂ + L₃ · sin(θ₁ + θ₂ + θ₃)

In practice, you can implement FK in a few lines of Python. Note how angle sums accumulate — this is where trigonometric simplification helps (especially with atan2 in IK).

Inverse Kinematics: The Smart Reverse Engineering

Now imagine you want the gripper to reach (x, y) = (150, 80) mm. You need to find how much to rotate each joint — that’s inverse kinematics. Unlike FK, IK is rarely unique: multiple joint configurations can reach the same point (elbow-up vs elbow-down).

The Geometric Approach

We reduce the 3-DOF arm to a 2-DOF problem by projecting the arm onto the x–y plane and using the law of cosines on the triangle formed by L₁, L₂, and the distance to the target.

Step-by-Step IK Solution

Compute the radial distance r to the target:
r = √(x² + y²)
Find the angle to the target using atan2:
γ = atan2(y, x)
Calculate angle θ₁ (shoulder) using triangle geometry:
Use the law of cosines to find angle α between L₁ and the line to the target:
cos(α) = (L₁² + r² - L₂²) / (2·L₁·r)
Then: θ₁ = γ ± α
(± gives elbow-up and elbow-down solutions)
Compute angle θ₂ (elbow):
cos(β) = (L₁² + L₂² - r²) / (2·L₁·L₂)
Then: θ₂ = π - β
Derive θ₃ (end-effector orientation):
If you care about gripper angle (not just position), use:
θ₃ = (γ ± α + β) - (θ₁ + θ₂)
Or simply set θ₃ = 0 if orientation doesn’t matter.

These equations assume a planar arm and ignore mechanical singularities (where L₁ + L₂ = target distance). Real-world systems often add a third joint to handle full 3D space — but for many desktop robots, 2D IK suffices.

MicroPython Implementation

Below is a clean, production-ready implementation in MicroPython — ready to run on an ESP32 with a few SG90 servos or a small motor+encoder setup.

import math

# Physical dimensions (mm)
L1 = 120.0
L2 = 100.0
L3 = 40.0

# Convert radians to degrees (for servos)
def rad2deg(rad):
    return math.degrees(rad % (2 * math.pi))

# Forward Kinematics
def fk(angles_deg):
    """angles_deg = [theta1, theta2, theta3] in degrees"""
    theta1 = math.radians(angles_deg[0])
    theta2 = math.radians(angles_deg[1])
    theta3 = math.radians(angles_deg[2])
    
    x1 = L1 * math.cos(theta1)
    y1 = L1 * math.sin(theta1)
    
    x2 = x1 + L2 * math.cos(theta1 + theta2)
    y2 = y1 + L2 * math.sin(theta1 + theta2)
    
    x_end = x2 + L3 * math.cos(theta1 + theta2 + theta3)
    y_end = y2 + L2 * math.sin(theta1 + theta2 + theta3)
    
    return (x_end, y_end)

# Inverse Kinematics (planar, 2-DOF for position only)
def ik(x, y):
    """
    Returns [theta1_deg, theta2_deg] for elbow-up solution (default).
    Returns two solutions if elbow_down=True in extended version.
    """
    r = math.sqrt(x**2 + y**2)
    
    # Check for unreachable or singular points
    if r > L1 + L2 - 1e-6:
        raise ValueError("Target position unreachable (too far)")
    
    # Law of cosines for angles
    alpha = math.acos((L1**2 + r**2 - L2**2) / (2 * L1 * r))
    gamma = math.atan2(y, x)
    
    # Elbow-up: minus sign
    theta1 = gamma - alpha
    # Elbow-down: plus sign
    # theta1_down = gamma + alpha
    
    beta = math.acos((L1**2 + L2**2 - r**2) / (2 * L1 * L2))
    theta2 = math.pi - beta
    
    return [rad2deg(theta1), rad2deg(theta2)]

# Example usage
if __name__ == "__main__":
    # Test FK: Start at home position
    home_angles = [0.0, 0.0, 0.0]
    pos = fk(home_angles)
    print("FK (home):", pos)

    # Test IK: Reach to (150, 80)
    target = (150.0, 80.0)
    sol = ik(*target)
    print("IK solution (shoulder, elbow):", sol)
    
    # Verify: FK of IK should match target
    pos_check = fk([sol[0], sol[1], 0.0])
    print("Verify FK:", pos_check)
    print("Error (should be ~0):", math.hypot(target[0]-pos_check[0], target[1]-pos_check[1]))

You’ll notice the ik function excludes the third joint by default — because planar IK doesn’t need it for (x,y) positioning. You can add a third joint later for orientation or full 3D reach.

Beyond 2D: Extending to 3-DOF 3D Arms

For arms with a rotating base joint (like real-world industrial arms), you’ll add one more dimension. The process is similar but uses spherical coordinates:

1️⃣ Base angle (θ₀): θ₀ = atan2(y, x) — computed first, independent of reach.
2️⃣ Projected radius (r): r = √(x² + y²) — effectively reduces it to 2D in a new plane.
3️⃣ IK in the radial plane: Solve for θ₁ and θ₂ as above, now using r and z instead of x/y.

A full 3D IK solver adds flexibility — but starts to require numerical methods (e.g., Jacobian transpose or pseudo-inverse) when joint constraints get complex. For prototyping, the geometric approach remains fast and reliable.

Putting It All Together: Real-World Setup

Here’s how to go from code to hardware:

Hardware: Use three micro-servos (e.g., SG90), an ESP32, and a breadboard or motor driver. Mount with 3D-printed joints or aluminum extrusions for stability.
Calibration: Zero each servo manually at the arm’s “home” position. Record offsets — servos often have ±10° tolerance.
Integration: Package the FK and IK functions in robot_kinematics.py, and call them from main.py via a simple serial CLI or WebREPL.
Safety: Add velocity ramps and limit checks — never command extreme angles without checking joint range!

Pro Tip: Test IK in simulation first (e.g., with matplotlib), then deploy — it saves hours of debugging tangled servos on the bench.

Conclusion: Kinematics as a Skill, Not Just Math

Forward and inverse kinematics transform abstract math into motion — the soul of robotics. By mastering FK and IK on a 3-DOF arm, you lay the foundation for path planning, trajectory optimization, and even machine vision integration. With MicroPython’s simplicity and readability, you can iterate rapidly — from sketch to real robot — without compiling C++ or wrestling with ROS layers.

Start with the 2D planar model above. Then extend it. Add a camera. Integrate PID control. Eventually, build a full 6-DOF arm — and remember: every complex robot starts with a few lines of geometry and the courage to reach the target.

Search This Blog

Robotic Arm Projects by ICT Club