Denavit-Hartenberg (DH) Parameters

 1import numpy as np
 2
 3def dh_matrix(theta, d, a, alpha):
 4    """
 5    Compute DH transformation matrix (Classic convention)
 6    
 7    Args:
 8        theta: Joint angle (radians)
 9        d: Link offset
10        a: Link length
11        alpha: Link twist (radians)
12    
13    Returns:
14        4x4 homogeneous transformation matrix
15    """
16    ct = np.cos(theta)
17    st = np.sin(theta)
18    ca = np.cos(alpha)
19    sa = np.sin(alpha)
20    
21    T = np.array([
22        [ct, -st*ca,  st*sa, a*ct],
23        [st,  ct*ca, -ct*sa, a*st],
24        [0,   sa,     ca,    d   ],
25        [0,   0,      0,     1   ]
26    ])
27    
28    return T
29
30# Example: Single joint
31theta = np.pi/4  # 45 degrees
32d = 0.1
33a = 0.5
34alpha = np.pi/2  # 90 degrees
35
36T = dh_matrix(theta, d, a, alpha)
37print("Transformation matrix:")
38print(T)

 1Step 1: Identify joint axes
 2   - Mark the axis of rotation (revolute) or translation (prismatic)
 3   - These become the z-axes
 4
 5Step 2: Establish base frame
 6   - z₀ along joint 1 axis
 7   - x₀ and y₀ chosen for convenience (usually align with world frame)
 8
 9Step 3: For each joint i (i = 1 to n):
10   a. Draw z_i along joint i+1 axis
11   b. Find common normal between z_{i-1} and z_i
12   c. Place x_i along this common normal
13      - If z_{i-1} and z_i intersect: x_i perpendicular to both
14      - If z_{i-1} and z_i are parallel: x_i arbitrary (choose convenient)
15   d. Place origin at intersection of x_i and z_i
16   e. Complete frame with y_i (right-hand rule)
17
18Step 4: Measure DH parameters
19   a. a_i: distance along x_i from z_{i-1} to z_i
20   b. α_i: angle about x_i from z_{i-1} to z_i (right-hand rule)
21   c. d_i: distance along z_{i-1} from x_{i-1} to x_i
22   d. θ_i: angle about z_{i-1} from x_{i-1} to x_i (right-hand rule)

 1def modified_dh_matrix(theta, d, a, alpha):
 2    """
 3    Modified DH transformation (Khalil convention)
 4    
 5    Differences from classic:
 6    - Frame i attached to joint i (not i+1)
 7    - Different parameter interpretation
 8    """
 9    ct = np.cos(theta)
10    st = np.sin(theta)
11    ca = np.cos(alpha)
12    sa = np.sin(alpha)
13    
14    T = np.array([
15        [ct,    -st,    0,   a    ],
16        [st*ca, ct*ca, -sa, -d*sa],
17        [st*sa, ct*sa,  ca,  d*ca],
18        [0,     0,      0,   1    ]
19    ])
20    
21    return T

 1def forward_kinematics(dh_params):
 2    """
 3    Compute forward kinematics from DH parameters
 4    
 5    Args:
 6        dh_params: List of dicts with keys 'theta', 'd', 'a', 'alpha'
 7    
 8    Returns:
 9        4x4 transformation matrix from base to end-effector
10    """
11    T = np.eye(4)
12    
13    for params in dh_params:
14        Ti = dh_matrix(**params)
15        T = T @ Ti
16    
17    return T
18
19# Example: 2-link planar arm
20dh_params = [
21    {'theta': np.pi/4, 'd': 0, 'a': 1.0, 'alpha': 0},
22    {'theta': np.pi/3, 'd': 0, 'a': 0.8, 'alpha': 0}
23]
24
25T = forward_kinematics(dh_params)
26print("End-effector position:", T[:3, 3])
27print("End-effector orientation:")
28print(T[:3, :3])

 1def extract_pose(T):
 2    """Extract position and orientation from transformation matrix"""
 3    # Position
 4    position = T[:3, 3]
 5    
 6    # Orientation (rotation matrix)
 7    rotation = T[:3, :3]
 8    
 9    # Convert to Euler angles (ZYX convention)
10    import scipy.spatial.transform as tf
11    r = tf.Rotation.from_matrix(rotation)
12    euler_angles = r.as_euler('zyx', degrees=False)
13    
14    return {
15        'position': position,
16        'rotation_matrix': rotation,
17        'euler_angles': euler_angles
18    }
19
20pose = extract_pose(T)
21print("Position:", pose['position'])
22print("Euler angles (ZYX):", np.degrees(pose['euler_angles']), "degrees")

1❌ BAD: z-axis not along joint axis
2   - Arbitrary frame placement
3   - Doesn't follow DH convention
4
5✅ GOOD: z-axis strictly along joint rotation/translation
6   - Follow DH rules systematically
7   - Verify each frame placement

1# ❌ BAD: Incorrect order
2T = Trans(x, a) @ Rot(z, theta) @ Trans(z, d) @ Rot(x, alpha)
3
4# ✅ GOOD: Correct DH order (Classic)
5T = Rot(z, theta) @ Trans(z, d) @ Trans(x, a) @ Rot(x, alpha)

1# ❌ BAD: Mixing degrees and radians
2theta_deg = 45
3T = dh_matrix(theta_deg, d, a, alpha)  # Wrong!
4
5# ✅ GOOD: Always use radians
6theta_rad = np.radians(45)
7T = dh_matrix(theta_rad, d, a, alpha)

1❌ BAD: Inconsistent angle measurement
2   - Mixing left-hand and right-hand rules
3   - Arbitrary positive directions
4
5✅ GOOD: Consistent right-hand rule
6   - All rotations follow right-hand rule
7   - Document positive directions clearly

 1def validate_dh(robot, test_cases):
 2    """Validate DH parameters with known configurations"""
 3    
 4    for test in test_cases:
 5        joints = test['joints']
 6        expected = test['expected_position']
 7        
 8        # Compute FK
 9        T = robot.forward_kinematics(*joints)
10        actual = T[:3, 3]
11        
12        # Check error
13        error = np.linalg.norm(actual - expected)
14        
15        print(f"Joints: {joints}")
16        print(f"Expected: {expected}")
17        print(f"Actual: {actual}")
18        print(f"Error: {error:.6f} m")
19        
20        assert error < 1e-6, f"FK error too large: {error}"
21    
22    print("\n✓ All validation tests passed!")
23
24# Example test cases
25test_cases = [
26    {
27        'joints': [0, 0],
28        'expected_position': np.array([1.8, 0, 0])  # L1 + L2
29    },
30    {
31        'joints': [np.pi/2, 0],
32        'expected_position': np.array([0, 1.8, 0])
33    }
34]

 1✅ DO:
 2- Follow DH convention strictly
 3- Document frame assignments clearly
 4- Use consistent units (radians, meters)
 5- Validate with known configurations
 6- Draw diagrams before implementing
 7- Check for parallel/intersecting axes cases
 8
 9❌ DON'T:
10- Mix DH conventions (classic vs modified)
11- Skip frame assignment verification
12- Use degrees in calculations
13- Forget to handle special cases (parallel axes)
14- Assume arbitrary frame placement is valid