Key Engineering Takeaways
- •Forward Kinematics computes the Cartesian pose (x, y, z, roll, pitch, yaw) of the end-effector given known joint angles (θ₁, θ₂, ..., θₙ).
- •The 4 DH parameters are: Link Length (a), Link Twist (α), Link Offset (d), and Joint Angle (θ).
- •The compounding transformation matrix is obtained by chained matrix multiplication: T₀ₙ = A₁ · A₂ · ... · Aₙ.
Prerequisites
- • Matrix multiplication
- • Trigonometry (sin/cos addition formulas)
Required Hardware / Tools
- • Python 3.10+ / NumPy / SciPy environment
The Forward Kinematics Problem Defined
Given a robotic serial manipulator with $n$ actuated joints, each joint variable is represented by a vector of generalized coordinates:
$$\mathbf{q} = [\theta_1, \theta_2, \dots, \theta_n]^T$$
The **Forward Kinematics (FK)** map $\mathbf{f}(\mathbf{q})$ transforms joint angles into the position $\mathbf{p} = [x, y, z]^T$ and orientation matrix $\mathbf{R} \in SO(3)$ of the tool frame with respect to the base frame:
$$\mathbf{T}_0^n(\mathbf{q}) = \begin{bmatrix} \mathbf{R}(\mathbf{q}) & \mathbf{p}(\mathbf{q}) \\ \mathbf{0}_{1 \times 3} & 1 \end{bmatrix} \in SE(3)$$
The 4 Denavit-Hartenberg Parameters & Coordinate Frame Rules
The Standard Denavit-Hartenberg convention reduces the 6 spatial parameters needed to relate two arbitrary coordinate frames down to just **4 parameters**:
1. **$a_i$ (Link Length)**: Distance along $x_i$ from the intersection of $x_i$ and $z_{i-1}$ to the origin of frame $i$.
2. **$\alpha_i$ (Link Twist)**: Angle from $z_{i-1}$ to $z_i$ measured about $x_i$.
3. **$d_i$ (Link Offset)**: Distance along $z_{i-1}$ from origin of frame $i-1$ to the intersection of $x_i$ and $z_{i-1}$.
4. **$\theta_i$ (Joint Angle)**: Angle from $x_{i-1}$ to $x_i$ measured about $z_{i-1}$.
### The Standard DH Transformation Matrix:
$$\mathbf{A}_i = \begin{bmatrix}
\cos\theta_i & -\sin\theta_i\cos\alpha_i & \sin\theta_i\sin\alpha_i & a_i\cos\theta_i \\
\sin\theta_i & \cos\theta_i\cos\alpha_i & -\cos\theta_i\sin\alpha_i & a_i\sin\theta_i \\
0 & \sin\alpha_i & \cos\alpha_i & d_i \\
0 & 0 & 0 & 1
\end{bmatrix}$$
forward_kinematics_solver.py
python
import numpy as np
def dh_matrix(a, alpha, d, theta):
"""Compute 4x4 Homogeneous Transformation Matrix for a single DH link."""
ct = np.cos(theta)
st = np.sin(theta)
ca = np.cos(alpha)
sa = 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 solve_forward_kinematics(joint_angles):
"""
3-DOF Planar Articulated Robot Arm
Link lengths: L1 = 0.3m, L2 = 0.25m, L3 = 0.15m
"""
t1, t2, t3 = joint_angles
L1, L2, L3 = 0.3, 0.25, 0.15
# DH Table: [a, alpha, d, theta]
A1 = dh_matrix(L1, 0, 0, t1)
A2 = dh_matrix(L2, 0, 0, t2)
A3 = dh_matrix(L3, 0, 0, t3)
# Chain multiplication: T_0_3 = A1 * A2 * A3
T_0_1 = A1
T_0_2 = np.dot(T_0_1, A2)
T_0_3 = np.dot(T_0_2, A3)
x = T_0_3[0, 3]
y = T_0_3[1, 3]
z = T_0_3[2, 3]
return x, y, z, T_0_3
# Example: calculate end-effector position at [30 deg, 45 deg, -20 deg]
angles_rad = np.radians([30, 45, -20])
x, y, z, T = solve_forward_kinematics(angles_rad)
print(f"End Effector Position -> X: {x:.4f} m, Y: {y:.4f} m, Z: {z:.4f} m")Tags:#Kinematics#DH Parameters#Robotic Arm#Forward Kinematics#Linear Algebra#Transformation Matrix