Initializing 3D Canvas...

Forward Kinematics

2 min read•1 page

Denavit-Hartenberg parameters provide a standard way to map coordinate frames between robot links. Only 4 parameters (theta, d, a, alpha) are needed to describe the transformation from one joint to the next.

DH Params:

1. Theta (θ): Rotation about Z axis to align X axes. 2. d: Translation along Z axis to align X axes. 3. a: Translation along X axis to reach next Z axis. 4. Alpha (α): Rotation about X axis to align Z axes.
python
1# Denavit-Hartenberg (DH) Parameters
2# 4 parameters describe the transformation from link i-1 to link i.
3

4class DH_Param:
5    def __init__(self, theta, d, a, alpha):
6        self.theta = theta  # Joint angle (rotation about Z)
7        self.d = d          # Link offset (translation along Z)
8        self.a = a          # Link length (translation along X)
9        self.alpha = alpha  # Link twist (rotation about X)
Theta (Rotation Z)
0.00
d (Offset Z)
1.00
a (Length X)
2.00
1 min read•1 page

The 4 DH parameters are plugged into a standard 4x4 homogenous transformation matrix. This single matrix encapsulates all the necessary rotations and translations to move from link i-1 to link i.

DH Matrix:

1. Compute sines and cosines of theta and alpha. 2. Populate the 4x4 matrix following the DH convention. 3. The right column holds the translational vector (x, y, z).
python
1# Constructing the DH Transformation Matrix
2import numpy as np
3

4def dh_matrix(theta, d, a, alpha):
5    """Returns 4x4 homogenous transform matrix."""
6    c_t, s_t = np.cos(theta), np.sin(theta)
7    c_a, s_a = np.cos(alpha), np.sin(alpha)
8    
9    return np.array([
10        [c_t, -s_t*c_a,  s_t*s_a, a*c_t],
11        [s_t,  c_t*c_a, -c_t*s_a, a*s_t],
12        [0,    s_a,      c_a,     d],
13        [0,    0,        0,       1]
14    ])
Theta
0.50
Alpha
0.50
2 min read•1 page

To find the position of any link in the world space, we multiply the local DH matrices sequentially from the base. This process is called Forward Kinematics (FK).

Matrix Multiplication:

1. T_0_to_n = T_0_to_1 * T_1_to_2 * ... * T_n-1_to_n 2. Matrix multiplication is not commutative! Order matters. 3. The resulting matrix gives world position and orientation.
python
1# Forward Kinematics Transform Concatenation
2def calculate_forward_kinematics(dh_params, joint_angles):
3    """Computes poses for all links in the chain."""
4    T_curr = np.eye(4)
5    poses = [T_curr]
6    
7    for i, p in enumerate(dh_params):
8        # Update theta with current joint angle
9        theta_i = p.theta + joint_angles[i]
10        
11        # Get local transform for this link
12        T_local = dh_matrix(theta_i, p.d, p.a, p.alpha)
13        
14        # Multiply with accumulated transform
15        T_curr = np.dot(T_curr, T_local)
16        poses.append(T_curr)
17        
18    return poses
Joint 1
0.50
Joint 2
0.50
Joint 3
0.50
2 min read•1 page

In the Denavit-Hartenberg convention, the Z-axis of the local coordinate frame is always aligned with the axis of rotation (or translation for prismatic joints). Extracting the Z-column from the transform matrix gives the axis direction in world space.

Axis Extraction:

1. Compute the world transform matrix for the joint. 2. Extract the translation vector (column 3). 3. Extract the Z-axis vector (column 2 of the rotation submatrix).
python
1# Visualizing Joint Axes
2def get_joint_axis(T_matrix):
3    """Extract the Z-axis vector from the transform matrix."""
4    # In DH convention, the Z-axis is the axis of rotation
5    # It is stored in the 3rd column of the rotation matrix (index 2)
6    z_dir = T_matrix[0:3, 2]
7    pos = T_matrix[0:3, 3]
8    return pos, z_dir
Joint 1 (Base)
0.50
Joint 2 (Pitch)
0.50
2 min read•1 page

The Tool Center Point (TCP) is the functional tip of the robot (e.g., the nozzle of an extruder or the jaws of a gripper). It is defined as a fixed offset from the last joint's coordinate frame.

TCP Offset:

1. Calculate transform to the final joint. 2. Apply a static local translation/rotation for the tool. 3. The resulting matrix defines the TCP in world space.
python
1# TCP (Tool Center Point) Configuration
2# The end effector has its own local offset from the final joint.
3

4def get_tcp_pose(final_joint_matrix, tcp_offset_vector):
5    """Calculate the pose of the tool center point."""
6    import numpy as np
7    
8    # Create translation matrix for the TCP offset
9    T_tcp = np.eye(4)
10    T_tcp[0:3, 3] = tcp_offset_vector
11    
12    # Final pose is joint matrix multiplied by tool offset matrix
13    return np.dot(final_joint_matrix, T_tcp)
Joint 1
0.50
Tool Z Offset
1.00
2 min read•1 page

By sweeping through all possible joint angle combinations within their limits, we can map out the entire reachable workspace of the robot arm. This is a brute-force application of Forward Kinematics.

Workspace Generation:

1. Discretize joint ranges based on a resolution. 2. Iterate over all combinations of joint angles. 3. Compute FK for each combination and store the TCP position.
python
1# Configuration Space Sweep
2def generate_workspace(kinematics_fn, joint_limits, resolution):
3    """Samples the reachable workspace of the robot."""
4    workspace_points = []
5    
6    # Nested loops sweep through all possible joint angles
7    for j1 in range(joint_limits[0].min, joint_limits[0].max, resolution):
8        for j2 in range(joint_limits[1].min, joint_limits[1].max, resolution):
9            # Calculate forward kinematics for this configuration
10            tcp_pose = kinematics_fn([j1, j2])
11            workspace_points.append(tcp_pose.position)
12            
13    return workspace_points
2 min read•1 page

In a dynamic simulation, joint angles are updated continuously over time. The forward kinematics algorithm runs every frame, recomputing the entire pose chain to animate the robot smoothly.

Control Loop:

1. Read or generate new joint angles for time `t`. 2. Re-evaluate the Forward Kinematics chain. 3. Apply resulting transformation matrices to 3D meshes.
python
1# Real-time FK Control Loop
2def update_robot(time_t):
3    """Animate joint angles over time."""
4    import math
5    
6    # Sine wave driven joints
7    j1 = math.sin(time_t * 1.5) * math.pi/2
8    j2 = math.cos(time_t * 2.0) * math.pi/3
9    j3 = math.sin(time_t * 0.5) * math.pi/4
10    
11    # Compute FK and update renderer
12    poses = calculate_forward_kinematics(dh_params, [j1, j2, j3])
13    render_robot(poses)