Week 02

Python Kinematics Practice: Building Forward Kinematics from Scratch

Published: 2026-04-11 | Author: Smartotics Learning Journey | Reading Time: 10 min

Figure 1: Python Forward Kinematics: Workspace visualization with code snippet

Quick Summary

Today we write Python code that computes forward kinematics from scratch — no robotics libraries, just NumPy. You’ll build a rotation matrix function, a homogeneous transform function, and a 2-DOF arm forward kinematics solver. By the end, you’ll have a working kinematics library you can extend to 3-DOF and 6-DOF arms.

Setup: What You Need

pip install numpy matplotlib

That’s it. NumPy for matrix math, Matplotlib for visualization.

Step 1: Build Rotation Matrices

These are the three basic rotations around X, Y, and Z axes:

import numpy as np

def rot_x(theta): """Rotation matrix about X axis by theta radians""" c, s = np.cos(theta), np.sin(theta) return np.array([ [1, 0, 0], [0, c, -s], [0, s, c] ])

def rot_y(theta): """Rotation matrix about Y axis by theta radians""" c, s = np.cos(theta), np.sin(theta) return np.array([ [ c, 0, s], [ 0, 1, 0], [-s, 0, c] ])

def rot_z(theta): """Rotation matrix about Z axis by theta radians""" c, s = np.cos(theta), np.sin(theta) return np.array([ [c, -s, 0], [s, c, 0], [0, 0, 1] ])

Test: rotate point [1, 0, 0] by 90 degrees around Z

p = np.array([1.0, 0.0, 0.0]) R_90 = rot_z(np.pi / 2) p_rotated = R_90 @ p print(f”Rotated [1, 0, 0] by 90 deg around Z: {p_rotated}“)

Expected: [0, 1, 0]

@ means matrix multiplication

In Python/NumPy, the @ operator is matrix multiplication. Don’t use * — that does element-wise multiplication. A @ B is the same as np.matmul(A, B).

Step 2: Build Homogeneous Transform

def homogeneous_transform(R, d): """ Create 4x4 homogeneous transform from 3x3 rotation R and 3x1 translation d.

T = [ R  | d ]
    [ 0  | 1 ]
"""
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = d
return T

Example: rotate 30 degrees around Z, then translate [0.5, 0, 0]

R = rot_z(np.radians(30)) d = np.array([0.5, 0.0, 0.0]) T = homogeneous_transform(R, d) print(“Transform matrix:”) print(T.round(3))

We can extract position and orientation back from any transform:

def extract_pose(T): """Extract (position, rotation_matrix) from 4x4 transform""" position = T[:3, 3] rotation = T[:3, :3] return position, rotation

def invert_transform(T): """Inverse of homogeneous transform (fast: transpose instead of inversion)""" T_inv = np.eye(4) R = T[:3, :3] d = T[:3, 3] T_inv[:3, :3] = R.T T_inv[:3, 3] = -R.T @ d return T_inv

Step 3: 2-DOF Planar Arm Forward Kinematics

Now combine everything. The 2-joint planar arm has two links with lengths L1 and L2:

def fk_2dof(q1, q2, L1, L2): """ Forward kinematics for 2-DOF planar arm.

Args:
    q1, q2: joint angles in radians
    L1, L2: link lengths in meters

Returns:
    x, y: end-effector position
"""
# Transform from base to joint 1
T_0_1 = homogeneous_transform(rot_z(q1), np.array([L1 * np.cos(q1), L1 * np.sin(q1), 0]))

# Transform from joint 1 to end-effector
T_1_2 = homogeneous_transform(rot_z(q2), np.array([L2 * np.cos(q2), L2 * np.sin(q2), 0]))

# Chain: base to end-effector
T_0_2 = T_0_1 @ T_1_2

# Extract position
x, y, z = T_0_2[:3, 3]
return x, y

Test: q1=30 deg, q2=45 deg, L1=0.5m, L2=0.3m

x, y = fk_2dof(np.radians(30), np.radians(45), 0.5, 0.3) print(f”End-effector position: x={x:.4f}m, y={y:.4f}m”)

Expected: x ≈ 0.642m, y ≈ 0.462m

Step 4: Visualize the Arm

Seeing the robot move makes everything click:

def plot_2dof_arm(q1, q2, L1, L2): """Visualize the 2-DOF planar arm using matplotlib""" import matplotlib.pyplot as plt

# Compute joint positions
origin = np.array([0.0, 0.0])
joint1 = np.array([L1 * np.cos(q1), L1 * np.sin(q1)])
ee_x, ee_y = fk_2dof(q1, q2, L1, L2)
end_effector = np.array([ee_x, ee_y])

# Plot
fig, ax = plt.subplots(1, 1, figsize=(8, 8))

# Draw links
ax.plot([origin[0], joint1[0]], [origin[1], joint1[1]], 'b-', linewidth=3, label='Link 1')
ax.plot([joint1[0], end_effector[0]], [joint1[1], end_effector[1]], 'r-', linewidth=3, label='Link 2')

# Draw joints
ax.plot(*origin, 'ko', markersize=10, label='Base')
ax.plot(*joint1, 'go', markersize=8, label='Joint 1 (Elbow)')
ax.plot(*end_effector, 'ro', markersize=8, label='End-Effector')

ax.set_xlim(-1.5, 1.5)
ax.set_ylim(-1.5, 1.5)
ax.set_aspect('equal')
ax.axhline(y=0, color='gray', linewidth=0.5)
ax.axvline(x=0, color='gray', linewidth=0.5)
ax.grid(True, alpha=0.3)
ax.legend()
ax.set_title(f'2-DOF Planar Arm: q1={np.degrees(q1):.0f}, q2={np.degrees(q2):.0f}')

plt.show()

Visualize

q1 = np.radians(30) q2 = np.radians(45) plot_2dof_arm(q1, q2, 0.5, 0.3)

Step 5: General D-H Forward Kinematics

Instead of hardcoding for 2-DOF, let’s use the D-H parameters properly. This function works for any serial robot:

def dh_transform(theta, d, a, alpha): """ Standard Denavit-Hartenberg transform.

Args:
    theta: joint angle (radians) - variable for revolute joints
    d: link offset - variable for prismatic joints
    a: link length
    alpha: link twist

Returns:
    4x4 homogeneous transformation matrix
"""
ct, st = np.cos(theta), np.sin(theta)
ca, sa = np.cos(alpha), np.sin(alpha)

return np.array([
    [ct, -st*ca,  st*sa,  a*ct],
    [st,  ct*ca, -ct*sa,  a*st],
    [0,   sa,     ca,     d   ],
    [0,   0,      0,      1    ]
])

def fk_serial(dh_params, joint_angles): """ Forward kinematics for any serial chain robot.

Args:
    dh_params: list of [a, alpha, d] for each joint (theta varies)
    joint_angles: list of theta values for each joint

Returns:
    4x4 transform from base to end-effector
"""
T = np.eye(4)
for i, (a, alpha, d) in enumerate(dh_params):
    T = T @ dh_transform(joint_angles[i], d, a, alpha)
return T

Example: 2-DOF planar arm using D-H

dh_2dof = [ [0.5, 0, 0], # Joint 1: a=0.5, alpha=0, d=0 [0.3, 0, 0], # Joint 2: a=0.3, alpha=0, d=0 ] T_ee = fk_serial(dh_2dof, [np.radians(30), np.radians(45)]) print(f”End-effector position: [{T_ee[0,3]:.4f}, {T_ee[1,3]:.4f}, {T_ee[2,3]:.4f}]“)

From Here to 6-DOF

The fk_serial() function above works for any number of joints. For a 6-axis industrial robot, just provide 6 D-H parameter sets and 6 joint angles. The math is exactly the same — only the input data changes.

Exercises: Try These

Vary the angles: Try q1=0, q2=0 (fully extended) and q1=90, q2=-90 (folded). Verify the positions match your mental model.
Add a third joint: Extend the DH parameters list to 3 joints. Add a vertical offset (d != 0) to the third joint.
Workspace analysis: Sweep q1 and q2 from -180 to 180 degrees and plot all reachable (x, y) positions. The shape you see is the robot’s workspace.

Exercise 3: Plot workspace

import matplotlib.pyplot.pyplot as plt

workspace_x, workspace_y = [], [] for q1_deg in range(-180, 181, 5): for q2_deg in range(-180, 181, 5): x, y = fk_2dof(np.radians(q1_deg), np.radians(q2_deg), 0.5, 0.3) workspace_x.append(x) workspace_y.append(y)

plt.figure(figsize=(8, 8)) plt.scatter(workspace_x, workspace_y, s=1, alpha=0.5) plt.gca().set_aspect(‘equal’) plt.title(‘2-DOF Planar Arm Workspace’) plt.xlabel(‘X (m)’) plt.ylabel(‘Y (m)’) plt.grid(True, alpha=0.3) plt.show()

FAQ

Q: I don’t have Python or NumPy set up. What do I do?

Install Python from python.org, then run pip install numpy matplotlib. If you need a free environment, use Google Colab — it has NumPy pre-installed and runs in your browser.

Q: Why write from scratch instead of using a robotics library?

You write it once to understand. Then you use a library for production. Understanding is non-negotiable — otherwise you’re just a library user, not an engineer.

Q: How do I extend this to 3D?

Add Z rotations to each DH parameter row. Set alpha to non-zero values and d to non-zero values. The fk_serial() function above already handles 3D — you just need 3D D-H parameters.

Key Takeaways

Disclaimer

For educational purposes only. This article is part of a structured learning curriculum.