Introduction to Forward Kinematics

Forward kinematics is one of the fundamental concepts in robotics that allows us to determine the position and orientation of a robot’s end-effector given its joint angles. This week, we’ll dive deep into forward kinematics, exploring both the mathematical foundations and practical implementations.

What is Forward Kinematics?

Forward kinematics is the process of calculating the position and orientation of the end-effector (the “hand” or “tool” at the end of a robot arm) based on the known joint angles. Unlike inverse kinematics (which we covered last week), forward kinematics has a unique solution - given joint angles, there’s only one possible end-effector position.

The mathematical relationship between joint angles and end-effector position is defined by the robot’s kinematic chain, which represents the sequence of links and joints that connect the base to the end-effector.

Mathematical Foundations

Denavit-Hartenberg Parameters

The Denavit-Hartenberg (D-H) convention provides a systematic method for assigning coordinate frames to each link of a robot. This allows us to define the relationship between consecutive links using four parameters:

• a: Link length (distance between joint axes)
• α: Link twist (angle between joint axes)
• d: Link offset (distance along joint axis to common normal)
• θ: Joint angle (angle around joint axis from common normal)

Transformation Matrices

Each joint-link pair can be represented by a 4×4 homogeneous transformation matrix:

import numpy as np

def dh_transform(theta, d, a, alpha):
    """
    Create a Denavit-Hartenberg transformation matrix
    
    Args:
        theta: Joint angle (radians)
        d: Link offset
        a: Link length
        alpha: Link twist (radians)
    
    Returns:
        4x4 transformation matrix
    """
    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]
    ])

Practical Implementation

Let’s implement a simple 2D forward kinematics example:

import numpy as np
import matplotlib.pyplot as plt

class Robot2D:
    def __init__(self, link_lengths):
        """
        Initialize a 2D robot arm
        
        Args:
            link_lengths: List of link lengths
        """
        self.link_lengths = link_lengths
        self.num_joints = len(link_lengths)
    
    def forward_kinematics(self, joint_angles):
        """
        Calculate end-effector position from joint angles
        
        Args:
            joint_angles: List of joint angles in radians
        
        Returns:
            List of joint positions including base
        """
        positions = [(0, 0)]  # Base position
        x, y = 0, 0
        angle_sum = 0
        
        for i, (length, angle) in enumerate(zip(self.link_lengths, joint_angles)):
            angle_sum += angle
            x += length * np.cos(angle_sum)
            y += length * np.sin(angle_sum)
            positions.append((x, y))
        
        return positions
    
    def plot_robot(self, joint_angles):
        """
        Visualize the robot arm
        
        Args:
            joint_angles: List of joint angles in radians
        """
        positions = self.forward_kinematics(joint_angles)
        
        plt.figure(figsize=(10, 8))
        x_coords = [pos[0] for pos in positions]
        y_coords = [pos[1] for pos in positions]
        
        plt.plot(x_coords, y_coords, 'b-o', linewidth=3, markersize=8)
        plt.plot(x_coords[0], y_coords[0], 'go', markersize=10, label='Base')
        plt.plot(x_coords[-1], y_coords[-1], 'ro', markersize=10, label='End-effector')
        
        plt.xlabel('X Position')
        plt.ylabel('Y Position')
        plt.title('2D Robot Arm - Forward Kinematics')
        plt.grid(True, alpha=0.3)
        plt.axis('equal')
        plt.legend()
        plt.show()

# Example usage
robot = Robot2D([2.0, 1.5])  # Two links with lengths 2.0 and 1.5
joint_angles = [np.pi/4, np.pi/6]  # 45° and 30°

# Calculate forward kinematics
positions = robot.forward_kinematics(joint_angles)
print("Joint positions:")
for i, pos in enumerate(positions):
    print(f"Joint {i}: ({pos[0]:.3f}, {pos[1]:.3f})")

# Visualize
robot.plot_robot(joint_angles)

3D Forward Kinematics

For 3D robots, we need to work with rotation matrices and homogeneous coordinates:

def rotation_x(angle):
    """Rotation matrix around X-axis"""
    c, s = np.cos(angle), np.sin(angle)
    return np.array([
        [1, 0, 0],
        [0, c, -s],
        [0, s, c]
    ])

def rotation_y(angle):
    """Rotation matrix around Y-axis"""
    c, s = np.cos(angle), np.sin(angle)
    return np.array([
        [c, 0, s],
        [0, 1, 0],
        [-s, 0, c]
    ])

def rotation_z(angle):
    """Rotation matrix around Z-axis"""
    c, s = np.cos(angle), np.sin(angle)
    return np.array([
        [c, -s, 0],
        [s, c, 0],
        [0, 0, 1]
    ])

def forward_kinematics_3d(joint_angles, link_lengths):
    """
    3D forward kinematics for a simple robot arm
    
    Args:
        joint_angles: List of joint angles [theta1, theta2, theta3]
        link_lengths: List of link lengths [L1, L2, L3]
    
    Returns:
        4x4 transformation matrix for end-effector
    """
    # Base to first joint
    T01 = np.eye(4)
    T01[2, 3] = link_lengths[0]
    
    # First to second joint
    T12 = rotation_z(joint_angles[0])
    T12[2, 3] = link_lengths[1]
    
    # Second to third joint (end-effector)
    T23 = rotation_y(joint_angles[1])
    T23[0, 3] = link_lengths[2]
    
    # End-effector orientation
    T34 = rotation_x(joint_angles[2])
    
    # Final transformation
    T04 = T01 @ T12 @ T23 @ T34
    
    return T04

# Example 3D forward kinematics
joint_angles = [np.pi/4, np.pi/3, np.pi/6]
link_lengths = [1.0, 0.8, 0.5]

T_end_effector = forward_kinematics_3d(joint_angles, link_lengths)
print("End-effector transformation matrix:")
print(T_end_effector)
print(f"End-effector position: [{T_end_effector[0,3]:.3f}, {T_end_effector[1,3]:.3f}, {T_end_effector[2,3]:.3f}]")

Applications of Forward Kinematics

Forward kinematics is used in numerous robotics applications:

1. Robot Control: Determining where the robot will move given joint commands
2. Simulation: Creating accurate robot models for virtual testing
3. Path Planning: Understanding robot workspace and reachability
4. Collision Detection: Checking if robot links will collide with obstacles
5. Teleoperation: Mapping human input to robot movements

Common Challenges

1. Singularities: Certain configurations where the robot loses degrees of freedom
2. Joint Limits: Real robots have physical constraints on joint angles
3. Precision Errors: Small errors in joint angles can lead to larger position errors
4. Computational Efficiency: Real-time applications require fast calculations

Key Takeaways

• Forward kinematics calculates end-effector position from joint angles
• Denavit-Hartenberg convention provides a systematic approach to robot kinematics
• Transformation matrices represent the relationship between coordinate frames
• 2D and 3D robots require different mathematical approaches
• Forward kinematics has unique solutions, unlike inverse kinematics

Next Steps

In tomorrow’s lesson, we’ll explore joint space and Cartesian space, and understand how to map between them using Jacobian matrices. We’ll also implement a complete forward kinematics solver for a 6-DOF robot arm.

Practice Exercise: Implement forward kinematics for your own robot arm configuration and visualize its workspace. Experiment with different joint angle combinations to understand how the end-effector moves through space.