DH Parameter Examples and Common Configurations

 1import numpy as np
 2
 3def dh_matrix(theta, d, a, alpha):
 4    """DH transformation matrix"""
 5    ct, st = np.cos(theta), np.sin(theta)
 6    ca, sa = np.cos(alpha), np.sin(alpha)
 7    
 8    return np.array([
 9        [ct, -st*ca,  st*sa, a*ct],
10        [st,  ct*ca, -ct*sa, a*st],
11        [0,   sa,     ca,    d   ],
12        [0,   0,      0,     1   ]
13    ])
14
15class PlanarArm2DOF:
16    def __init__(self, L1=1.0, L2=0.8):
17        self.L1 = L1
18        self.L2 = L2
19    
20    def forward_kinematics(self, theta1, theta2):
21        """Compute end-effector pose"""
22        T1 = dh_matrix(theta1, 0, self.L1, 0)
23        T2 = dh_matrix(theta2, 0, self.L2, 0)
24        
25        T = T1 @ T2
26        
27        x = T[0, 3]
28        y = T[1, 3]
29        phi = np.arctan2(T[1, 0], T[0, 0])
30        
31        return x, y, phi
32    
33    def plot_arm(self, theta1, theta2):
34        """Visualize arm configuration"""
35        import matplotlib.pyplot as plt
36        
37        # Joint positions
38        p0 = np.array([0, 0])
39        p1 = np.array([self.L1 * np.cos(theta1),
40                       self.L1 * np.sin(theta1)])
41        p2 = np.array([p1[0] + self.L2 * np.cos(theta1 + theta2),
42                       p1[1] + self.L2 * np.sin(theta1 + theta2)])
43        
44        plt.figure(figsize=(8, 8))
45        plt.plot([p0[0], p1[0], p2[0]], [p0[1], p1[1], p2[2]], 'o-', linewidth=2)
46        plt.plot(p0[0], p0[1], 'ro', markersize=10, label='Base')
47        plt.plot(p2[0], p2[1], 'go', markersize=10, label='End-effector')
48        plt.grid(True)
49        plt.axis('equal')
50        plt.legend()
51        plt.title(f'θ₁={np.degrees(theta1):.1f}°, θ₂={np.degrees(theta2):.1f}°')
52        plt.show()
53
54# Example
55arm = PlanarArm2DOF(L1=1.0, L2=0.8)
56x, y, phi = arm.forward_kinematics(np.pi/4, np.pi/3)
57print(f"End-effector: x={x:.3f}, y={y:.3f}, φ={np.degrees(phi):.1f}°")

 1class RRRManipulator:
 2    def __init__(self, L1=0.5, L2=0.4, L3=0.3):
 3        self.L1 = L1
 4        self.L2 = L2
 5        self.L3 = L3
 6    
 7    def forward_kinematics(self, theta1, theta2, theta3):
 8        """Compute end-effector pose"""
 9        T1 = dh_matrix(theta1, self.L1, 0, np.pi/2)
10        T2 = dh_matrix(theta2, 0, self.L2, 0)
11        T3 = dh_matrix(theta3, 0, self.L3, 0)
12        
13        T = T1 @ T2 @ T3
14        return T
15    
16    def get_joint_positions(self, theta1, theta2, theta3):
17        """Get position of each joint"""
18        T0 = np.eye(4)
19        T1 = dh_matrix(theta1, self.L1, 0, np.pi/2)
20        T2 = dh_matrix(theta2, 0, self.L2, 0)
21        T3 = dh_matrix(theta3, 0, self.L3, 0)
22        
23        positions = [
24            T0[:3, 3],
25            T1[:3, 3],
26            (T1 @ T2)[:3, 3],
27            (T1 @ T2 @ T3)[:3, 3]
28        ]
29        
30        return positions
31
32# Example
33robot = RRRManipulator()
34T = robot.forward_kinematics(0, np.pi/4, np.pi/6)
35print("End-effector position:", T[:3, 3])

 1class SCARArobot:
 2    def __init__(self, L1=0.3, L2=0.25, d3_min=0.0, d3_max=0.2):
 3        self.L1 = L1
 4        self.L2 = L2
 5        self.d3_min = d3_min
 6        self.d3_max = d3_max
 7    
 8    def forward_kinematics(self, theta1, theta2, d3, theta4):
 9        """Compute end-effector pose"""
10        # Clamp prismatic joint
11        d3 = np.clip(d3, self.d3_min, self.d3_max)
12        
13        T1 = dh_matrix(theta1, 0, self.L1, 0)
14        T2 = dh_matrix(theta2, 0, self.L2, np.pi)
15        T3 = dh_matrix(0, d3, 0, 0)
16        T4 = dh_matrix(theta4, 0, 0, 0)
17        
18        T = T1 @ T2 @ T3 @ T4
19        return T
20    
21    def workspace_limits(self):
22        """Calculate workspace dimensions"""
23        r_max = self.L1 + self.L2
24        r_min = abs(self.L1 - self.L2)
25        z_min = self.d3_min
26        z_max = self.d3_max
27        
28        return {
29            'r_min': r_min,
30            'r_max': r_max,
31            'z_min': z_min,
32            'z_max': z_max
33        }
34
35# Example
36scara = SCARArobot()
37T = scara.forward_kinematics(np.pi/6, np.pi/4, 0.1, 0)
38print(f"SCARA end-effector: [{T[0,3]:.3f}, {T[1,3]:.3f}, {T[2,3]:.3f}]")
39print("Workspace:", scara.workspace_limits())

 1class StanfordArm:
 2    def __init__(self, d2=0.154, d3_min=0.0, d3_max=0.5):
 3        self.d2 = d2
 4        self.d3_min = d3_min
 5        self.d3_max = d3_max
 6    
 7    def forward_kinematics(self, theta1, theta2, d3, theta4, theta5):
 8        """Compute end-effector pose"""
 9        d3 = np.clip(d3, self.d3_min, self.d3_max)
10        
11        T1 = dh_matrix(theta1, 0, 0, -np.pi/2)
12        T2 = dh_matrix(theta2, self.d2, 0, np.pi/2)
13        T3 = dh_matrix(0, d3, 0, 0)
14        T4 = dh_matrix(theta4, 0, 0, -np.pi/2)
15        T5 = dh_matrix(theta5, 0, 0, 0)
16        
17        T = T1 @ T2 @ T3 @ T4 @ T5
18        return T
19
20# Example
21stanford = StanfordArm()
22T = stanford.forward_kinematics(0, np.pi/4, 0.3, 0, 0)
23print("Stanford arm end-effector:", T[:3, 3])

 1class PUMA560:
 2    def __init__(self):
 3        # PUMA 560 dimensions (meters)
 4        self.a2 = 0.4318
 5        self.a3 = 0.0203
 6        self.d3 = 0.15005
 7        self.d4 = 0.4318
 8    
 9    def forward_kinematics(self, joints):
10        """
11        Compute forward kinematics
12        
13        Args:
14            joints: List of 6 joint angles [θ1, θ2, θ3, θ4, θ5, θ6]
15        """
16        theta1, theta2, theta3, theta4, theta5, theta6 = joints
17        
18        T1 = dh_matrix(theta1, 0, 0, np.pi/2)
19        T2 = dh_matrix(theta2, 0, self.a2, 0)
20        T3 = dh_matrix(theta3, self.d3, self.a3, -np.pi/2)
21        T4 = dh_matrix(theta4, self.d4, 0, np.pi/2)
22        T5 = dh_matrix(theta5, 0, 0, -np.pi/2)
23        T6 = dh_matrix(theta6, 0, 0, 0)
24        
25        T = T1 @ T2 @ T3 @ T4 @ T5 @ T6
26        return T
27    
28    def get_all_transforms(self, joints):
29        """Get transformation to each joint frame"""
30        theta1, theta2, theta3, theta4, theta5, theta6 = joints
31        
32        T1 = dh_matrix(theta1, 0, 0, np.pi/2)
33        T2 = dh_matrix(theta2, 0, self.a2, 0)
34        T3 = dh_matrix(theta3, self.d3, self.a3, -np.pi/2)
35        T4 = dh_matrix(theta4, self.d4, 0, np.pi/2)
36        T5 = dh_matrix(theta5, 0, 0, -np.pi/2)
37        T6 = dh_matrix(theta6, 0, 0, 0)
38        
39        transforms = [
40            np.eye(4),
41            T1,
42            T1 @ T2,
43            T1 @ T2 @ T3,
44            T1 @ T2 @ T3 @ T4,
45            T1 @ T2 @ T3 @ T4 @ T5,
46            T1 @ T2 @ T3 @ T4 @ T5 @ T6
47        ]
48        
49        return transforms
50
51# Example
52puma = PUMA560()
53joints = [0, np.pi/4, 0, 0, 0, 0]
54T = puma.forward_kinematics(joints)
55print("PUMA 560 end-effector:")
56print(T)

 1class GenericManipulator:
 2    def __init__(self, dh_table):
 3        """
 4        Args:
 5            dh_table: List of dicts with keys:
 6                - 'a': link length
 7                - 'alpha': link twist
 8                - 'd_const': constant offset (for revolute)
 9                - 'theta_const': constant angle (for prismatic)
10                - 'joint_type': 'revolute' or 'prismatic'
11        """
12        self.dh_table = dh_table
13        self.n_joints = len(dh_table)
14    
15    def forward_kinematics(self, joint_values):
16        """
17        Compute forward kinematics
18        
19        Args:
20            joint_values: List of joint values (angles or distances)
21        """
22        T = np.eye(4)
23        
24        for i, (value, dh) in enumerate(zip(joint_values, self.dh_table)):
25            if dh['joint_type'] == 'prismatic':
26                d = value
27                theta = dh.get('theta_const', 0)
28            else:  # revolute
29                d = dh.get('d_const', 0)
30                theta = value
31            
32            Ti = dh_matrix(theta, d, dh['a'], dh['alpha'])
33            T = T @ Ti
34        
35        return T
36    
37    def check_joint_limits(self, joint_values):
38        """Check if joints are within limits"""
39        for i, (value, dh) in enumerate(zip(joint_values, self.dh_table)):
40            limits = dh.get('limits', {'min': -np.inf, 'max': np.inf})
41            if not (limits['min'] <= value <= limits['max']):
42                return False, f"Joint {i+1} out of range"
43        return True, "OK"
44
45# Example: Custom 4-DOF robot
46custom_dh = [
47    {'a': 0, 'alpha': np.pi/2, 'd_const': 0.5, 'joint_type': 'revolute',
48     'limits': {'min': -np.pi, 'max': np.pi}},
49    {'a': 0.4, 'alpha': 0, 'd_const': 0, 'joint_type': 'revolute',
50     'limits': {'min': -np.pi/2, 'max': np.pi/2}},
51    {'a': 0, 'alpha': 0, 'theta_const': 0, 'joint_type': 'prismatic',
52     'limits': {'min': 0, 'max': 0.3}},
53    {'a': 0, 'alpha': 0, 'd_const': 0.2, 'joint_type': 'revolute',
54     'limits': {'min': -np.pi, 'max': np.pi}}
55]
56
57robot = GenericManipulator(custom_dh)
58joints = [np.pi/6, np.pi/4, 0.15, 0]
59valid, msg = robot.check_joint_limits(joints)
60if valid:
61    T = robot.forward_kinematics(joints)
62    print("End-effector:", T[:3, 3])

 1def plot_workspace_3d(robot, n_samples=5000):
 2    """Plot 3D workspace by sampling joint space"""
 3    import matplotlib.pyplot as plt
 4    from mpl_toolkits.mplot3d import Axes3D
 5    
 6    points = []
 7    
 8    for _ in range(n_samples):
 9        # Random valid joint configuration
10        joints = []
11        for dh in robot.dh_table:
12            limits = dh.get('limits', {'min': -np.pi, 'max': np.pi})
13            value = np.random.uniform(limits['min'], limits['max'])
14            joints.append(value)
15        
16        # Forward kinematics
17        T = robot.forward_kinematics(joints)
18        points.append(T[:3, 3])
19    
20    points = np.array(points)
21    
22    # Plot
23    fig = plt.figure(figsize=(10, 8))
24    ax = fig.add_subplot(111, projection='3d')
25    ax.scatter(points[:, 0], points[:, 1], points[:, 2], 
26               alpha=0.1, s=1, c=points[:, 2], cmap='viridis')
27    ax.set_xlabel('X (m)')
28    ax.set_ylabel('Y (m)')
29    ax.set_zlabel('Z (m)')
30    ax.set_title('Robot Workspace')
31    plt.colorbar(ax.scatter(points[:, 0], points[:, 1], points[:, 2], 
32                            c=points[:, 2], cmap='viridis'), ax=ax)
33    plt.show()
34
35# Example
36plot_workspace_3d(robot, n_samples=3000)

 1class RobotWithLimits:
 2    def __init__(self, dh_table, joint_limits):
 3        self.dh_table = dh_table
 4        self.joint_limits = joint_limits
 5    
 6    def clamp_joints(self, joints):
 7        """Clamp joints to valid range"""
 8        clamped = []
 9        for joint, limits in zip(joints, self.joint_limits):
10            clamped.append(np.clip(joint, limits['min'], limits['max']))
11        return np.array(clamped)
12    
13    def safe_forward_kinematics(self, joints):
14        """FK with automatic joint clamping"""
15        joints_safe = self.clamp_joints(joints)
16        
17        # Warn if clamping occurred
18        if not np.allclose(joints, joints_safe):
19            print("Warning: Joints clamped to limits")
20            for i, (orig, safe) in enumerate(zip(joints, joints_safe)):
21                if not np.isclose(orig, safe):
22                    print(f"  Joint {i+1}: {orig:.3f} → {safe:.3f}")
23        
24        # Compute FK
25        T = np.eye(4)
26        for value, dh in zip(joints_safe, self.dh_table):
27            if dh['joint_type'] == 'prismatic':
28                Ti = dh_matrix(dh.get('theta_const', 0), value, 
29                              dh['a'], dh['alpha'])
30            else:
31                Ti = dh_matrix(value, dh.get('d_const', 0), 
32                              dh['a'], dh['alpha'])
33            T = T @ Ti
34        
35        return T, joints_safe