Forward Kinematics

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)

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    ])

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

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

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)

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

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)