Two-Wheel Differential Drive Robot Kinematics

 1        Front
 2          ↑
 3    ┌─────┴─────┐
 4    │     •     │  ← Center of mass/rotation
 5    │           │
 6    ├───┐   ┌───┤
 7    │ L │   │ R │  ← Left and Right wheels
 8    └───┘   └───┘
 9         ↕
10         L (wheelbase)

 1import numpy as np
 2
 3class DifferentialDriveRobot:
 4    def __init__(self, wheelbase=0.3, wheel_radius=0.05):
 5        """
 6        Args:
 7            wheelbase: Distance between wheels (m)
 8            wheel_radius: Radius of wheels (m)
 9        """
10        self.L = wheelbase
11        self.r = wheel_radius
12        
13        # State: [x, y, theta]
14        self.state = np.array([0.0, 0.0, 0.0])
15    
16    def forward_kinematics(self, omega_left, omega_right, dt):
17        """
18        Update robot pose given wheel velocities
19        
20        Args:
21            omega_left: Left wheel angular velocity (rad/s)
22            omega_right: Right wheel angular velocity (rad/s)
23            dt: Time step (s)
24        
25        Returns:
26            Updated pose [x, y, theta]
27        """
28        x, y, theta = self.state
29        
30        # Robot velocities
31        v = self.r * (omega_right + omega_left) / 2
32        omega = self.r * (omega_right - omega_left) / self.L
33        
34        # Update pose
35        if abs(omega) < 1e-6:
36            # Straight line motion
37            x += v * np.cos(theta) * dt
38            y += v * np.sin(theta) * dt
39        else:
40            # Arc motion
41            R = v / omega  # Turning radius
42            
43            # Instantaneous center of curvature (ICC)
44            icc_x = x - R * np.sin(theta)
45            icc_y = y + R * np.cos(theta)
46            
47            # Rotate around ICC
48            dtheta = omega * dt
49            x = np.cos(dtheta) * (x - icc_x) - np.sin(dtheta) * (y - icc_y) + icc_x
50            y = np.sin(dtheta) * (x - icc_x) + np.cos(dtheta) * (y - icc_y) + icc_y
51            theta += dtheta
52        
53        # Normalize angle
54        theta = np.arctan2(np.sin(theta), np.cos(theta))
55        
56        self.state = np.array([x, y, theta])
57        return self.state.copy()
58    
59    def get_velocity(self, omega_left, omega_right):
60        """Get robot linear and angular velocity"""
61        v = self.r * (omega_right + omega_left) / 2
62        omega = self.r * (omega_right - omega_left) / self.L
63        return v, omega
64
65# Example: Straight line
66robot = DifferentialDriveRobot(wheelbase=0.3, wheel_radius=0.05)
67
68# Both wheels same speed → straight line
69for _ in range(100):
70    robot.forward_kinematics(omega_left=5.0, omega_right=5.0, dt=0.01)
71
72print(f"After straight motion: {robot.state}")
73
74# Reset and turn in place
75robot.state = np.array([0.0, 0.0, 0.0])
76
77# Opposite wheel speeds → rotation in place
78for _ in range(100):
79    robot.forward_kinematics(omega_left=-5.0, omega_right=5.0, dt=0.01)
80
81print(f"After rotation: {robot.state}")

 1def inverse_kinematics(self, v_desired, omega_desired):
 2    """
 3    Compute wheel velocities for desired robot motion
 4    
 5    Args:
 6        v_desired: Desired linear velocity (m/s)
 7        omega_desired: Desired angular velocity (rad/s)
 8    
 9    Returns:
10        (omega_left, omega_right) in rad/s
11    """
12    omega_left = (v_desired - omega_desired * self.L / 2) / self.r
13    omega_right = (v_desired + omega_desired * self.L / 2) / self.r
14    
15    return omega_left, omega_right
16
17# Add to DifferentialDriveRobot class
18DifferentialDriveRobot.inverse_kinematics = inverse_kinematics
19
20# Example: Move forward at 0.5 m/s
21robot = DifferentialDriveRobot()
22omega_l, omega_r = robot.inverse_kinematics(v_desired=0.5, omega_desired=0)
23print(f"Straight motion: ωL={omega_l:.2f}, ωR={omega_r:.2f} rad/s")
24
25# Turn in place at 1 rad/s
26omega_l, omega_r = robot.inverse_kinematics(v_desired=0, omega_desired=1.0)
27print(f"Rotation: ωL={omega_l:.2f}, ωR={omega_r:.2f} rad/s")
28
29# Arc motion
30omega_l, omega_r = robot.inverse_kinematics(v_desired=0.5, omega_desired=0.5)
31print(f"Arc motion: ωL={omega_l:.2f}, ωR={omega_r:.2f} rad/s")

 1def navigate_to_point(self, target_x, target_y, kp_linear=1.0, kp_angular=2.0):
 2    """
 3    Simple proportional controller to reach target point
 4    
 5    Args:
 6        target_x, target_y: Target position
 7        kp_linear: Linear velocity gain
 8        kp_angular: Angular velocity gain
 9    
10    Returns:
11        (omega_left, omega_right) wheel velocities
12    """
13    x, y, theta = self.state
14    
15    # Error in position
16    dx = target_x - x
17    dy = target_y - y
18    distance = np.sqrt(dx**2 + dy**2)
19    
20    # Desired heading
21    target_theta = np.arctan2(dy, dx)
22    
23    # Heading error
24    theta_error = np.arctan2(np.sin(target_theta - theta),
25                             np.cos(target_theta - theta))
26    
27    # Control law
28    v_desired = kp_linear * distance
29    omega_desired = kp_angular * theta_error
30    
31    # Limit velocities
32    v_max = 1.0  # m/s
33    omega_max = 2.0  # rad/s
34    v_desired = np.clip(v_desired, -v_max, v_max)
35    omega_desired = np.clip(omega_desired, -omega_max, omega_max)
36    
37    # Inverse kinematics
38    omega_left, omega_right = self.inverse_kinematics(v_desired, omega_desired)
39    
40    return omega_left, omega_right, distance
41
42# Add to class
43DifferentialDriveRobot.navigate_to_point = navigate_to_point
44
45# Example: Navigate to (2, 3)
46robot = DifferentialDriveRobot()
47target = (2.0, 3.0)
48
49dt = 0.05
50trajectory = [robot.state.copy()]
51
52for _ in range(500):
53    omega_l, omega_r, dist = robot.navigate_to_point(*target)
54    
55    if dist < 0.05:  # Reached target
56        break
57    
58    robot.forward_kinematics(omega_l, omega_r, dt)
59    trajectory.append(robot.state.copy())
60
61print(f"Final position: {robot.state}")
62print(f"Distance to target: {dist:.4f} m")
63
64# Plot trajectory
65import matplotlib.pyplot as plt
66trajectory = np.array(trajectory)
67plt.figure(figsize=(8, 8))
68plt.plot(trajectory[:, 0], trajectory[:, 1], 'b-', linewidth=2, label='Path')
69plt.plot(0, 0, 'go', markersize=10, label='Start')
70plt.plot(target[0], target[1], 'ro', markersize=10, label='Target')
71plt.grid(True)
72plt.axis('equal')
73plt.xlabel('X (m)')
74plt.ylabel('Y (m)')
75plt.legend()
76plt.title('Differential Drive Navigation')
77plt.show()

 1def apply_velocity_limits(self, omega_left, omega_right, omega_max=10.0):
 2    """
 3    Apply wheel velocity limits while preserving motion direction
 4    
 5    Args:
 6        omega_left, omega_right: Desired wheel velocities
 7        omega_max: Maximum wheel velocity (rad/s)
 8    
 9    Returns:
10        Limited wheel velocities
11    """
12    # Find scaling factor
13    max_omega = max(abs(omega_left), abs(omega_right))
14    
15    if max_omega > omega_max:
16        scale = omega_max / max_omega
17        omega_left *= scale
18        omega_right *= scale
19    
20    return omega_left, omega_right
21
22# Add to class
23DifferentialDriveRobot.apply_velocity_limits = apply_velocity_limits

 1class DifferentialDriveWithOdometry(DifferentialDriveRobot):
 2    def __init__(self, wheelbase=0.3, wheel_radius=0.05, 
 3                 encoder_resolution=2048):
 4        super().__init__(wheelbase, wheel_radius)
 5        self.encoder_resolution = encoder_resolution
 6        self.encoder_left = 0
 7        self.encoder_right = 0
 8    
 9    def update_from_encoders(self, encoder_left, encoder_right, dt):
10        """
11        Update pose from encoder readings
12        
13        Args:
14            encoder_left, encoder_right: Encoder tick counts
15            dt: Time since last update
16        """
17        # Encoder deltas
18        delta_left = encoder_left - self.encoder_left
19        delta_right = encoder_right - self.encoder_right
20        
21        # Update stored values
22        self.encoder_left = encoder_left
23        self.encoder_right = encoder_right
24        
25        # Convert to angular displacement
26        angle_left = 2 * np.pi * delta_left / self.encoder_resolution
27        angle_right = 2 * np.pi * delta_right / self.encoder_resolution
28        
29        # Wheel velocities
30        omega_left = angle_left / dt
31        omega_right = angle_right / dt
32        
33        # Update pose
34        return self.forward_kinematics(omega_left, omega_right, dt)
35
36# Example
37robot = DifferentialDriveWithOdometry()
38
39# Simulate encoder readings
40encoder_left = 0
41encoder_right = 0
42
43for i in range(100):
44    # Simulate forward motion
45    encoder_left += 10  # 10 ticks per iteration
46    encoder_right += 10
47    
48    robot.update_from_encoders(encoder_left, encoder_right, dt=0.01)
49
50print(f"Odometry position: {robot.state}")

 1class MotionPrimitives:
 2    @staticmethod
 3    def straight(distance, speed=0.5):
 4        """Move straight for given distance"""
 5        duration = distance / speed
 6        return {
 7            'v': speed,
 8            'omega': 0,
 9            'duration': duration
10        }
11    
12    @staticmethod
13    def rotate(angle, angular_speed=1.0):
14        """Rotate in place by given angle"""
15        duration = abs(angle) / angular_speed
16        omega = angular_speed if angle > 0 else -angular_speed
17        return {
18            'v': 0,
19            'omega': omega,
20            'duration': duration
21        }
22    
23    @staticmethod
24    def arc(radius, angle, speed=0.5):
25        """Follow circular arc"""
26        arc_length = radius * abs(angle)
27        duration = arc_length / speed
28        omega = speed / radius
29        if angle < 0:
30            omega = -omega
31        return {
32            'v': speed,
33            'omega': omega,
34            'duration': duration
35        }
36
37# Example: Execute motion sequence
38robot = DifferentialDriveRobot()
39
40sequence = [
41    MotionPrimitives.straight(distance=1.0, speed=0.5),
42    MotionPrimitives.rotate(angle=np.pi/2, angular_speed=1.0),
43    MotionPrimitives.straight(distance=0.5, speed=0.5),
44    MotionPrimitives.arc(radius=0.5, angle=np.pi, speed=0.3)
45]
46
47dt = 0.01
48for primitive in sequence:
49    t = 0
50    while t < primitive['duration']:
51        omega_l, omega_r = robot.inverse_kinematics(
52            primitive['v'], primitive['omega']
53        )
54        robot.forward_kinematics(omega_l, omega_r, dt)
55        t += dt
56
57print(f"Final pose after sequence: {robot.state}")

 1def simulate_and_plot(robot, control_function, duration=10.0, dt=0.01):
 2    """
 3    Simulate robot motion and plot trajectory
 4    
 5    Args:
 6        robot: DifferentialDriveRobot instance
 7        control_function: Function(robot, t) -> (omega_left, omega_right)
 8        duration: Simulation duration (s)
 9        dt: Time step (s)
10    """
11    import matplotlib.pyplot as plt
12    from matplotlib.animation import FuncAnimation
13    
14    trajectory = []
15    t = 0
16    
17    while t < duration:
18        # Get control inputs
19        omega_l, omega_r = control_function(robot, t)
20        
21        # Update robot
22        robot.forward_kinematics(omega_l, omega_r, dt)
23        trajectory.append(robot.state.copy())
24        
25        t += dt
26    
27    trajectory = np.array(trajectory)
28    
29    # Plot
30    fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(14, 6))
31    
32    # Trajectory
33    ax1.plot(trajectory[:, 0], trajectory[:, 1], 'b-', linewidth=2)
34    ax1.plot(trajectory[0, 0], trajectory[0, 1], 'go', markersize=10, label='Start')
35    ax1.plot(trajectory[-1, 0], trajectory[-1, 1], 'ro', markersize=10, label='End')
36    
37    # Draw robot orientation at intervals
38    for i in range(0, len(trajectory), len(trajectory)//10):
39        x, y, theta = trajectory[i]
40        dx = 0.1 * np.cos(theta)
41        dy = 0.1 * np.sin(theta)
42        ax1.arrow(x, y, dx, dy, head_width=0.05, head_length=0.05, fc='r', ec='r')
43    
44    ax1.grid(True)
45    ax1.axis('equal')
46    ax1.set_xlabel('X (m)')
47    ax1.set_ylabel('Y (m)')
48    ax1.legend()
49    ax1.set_title('Robot Trajectory')
50    
51    # Heading over time
52    time = np.arange(len(trajectory)) * dt
53    ax2.plot(time, np.degrees(trajectory[:, 2]), 'b-', linewidth=2)
54    ax2.grid(True)
55    ax2.set_xlabel('Time (s)')
56    ax2.set_ylabel('Heading (degrees)')
57    ax2.set_title('Robot Heading vs Time')
58    
59    plt.tight_layout()
60    plt.show()
61
62# Example: Figure-8 pattern
63def figure_eight_control(robot, t):
64    """Control for figure-8 trajectory"""
65    v = 0.5  # m/s
66    omega = 0.5 * np.sin(t)  # rad/s
67    return robot.inverse_kinematics(v, omega)
68
69robot = DifferentialDriveRobot()
70simulate_and_plot(robot, figure_eight_control, duration=20.0)