Front-Wheel Drive (FWD) Kinematics

1        Front (Steered + Driven)
2              ↑
3         ┌─────────┐
4         │ ╱     ╲ │  ← Steered wheels
5         │         │
6         │    •    │  ← Center of rotation
7         │         │
8         │ │     │ │  ← Rear wheels (passive)
9         └─────────┘

 1import numpy as np
 2
 3class FrontWheelDrive:
 4    def __init__(self, wheelbase=0.5, wheel_radius=0.1):
 5        """
 6        Args:
 7            wheelbase: Distance between front and rear axles (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, v, delta, dt):
17        """
18        Update robot pose from velocity and steering angle
19        
20        Args:
21            v: Linear velocity (m/s)
22            delta: Steering angle (rad)
23            dt: Time step (s)
24        
25        Returns:
26            Updated pose [x, y, theta]
27        """
28        x, y, theta = self.state
29        
30        # Calculate angular velocity
31        if abs(delta) < 1e-6:
32            # Straight line motion
33            omega = 0
34        else:
35            omega = v * np.tan(delta) / self.L
36        
37        # Update pose
38        x += v * np.cos(theta) * dt
39        y += v * np.sin(theta) * dt
40        theta += omega * dt
41        
42        # Normalize angle
43        theta = np.arctan2(np.sin(theta), np.cos(theta))
44        
45        self.state = np.array([x, y, theta])
46        return self.state.copy()
47    
48    def get_turning_radius(self, delta):
49        """Calculate turning radius for given steering angle"""
50        if abs(delta) < 1e-6:
51            return float('inf')
52        return self.L / np.tan(delta)
53    
54    def get_min_turning_radius(self, max_steering_angle=np.pi/3):
55        """Get minimum turning radius"""
56        return self.L / np.tan(max_steering_angle)
57
58# Example
59robot = FrontWheelDrive(wheelbase=0.5, wheel_radius=0.1)
60
61# Straight line
62for _ in range(100):
63    robot.forward_kinematics(v=1.0, delta=0, dt=0.01)
64
65print(f"After straight motion: {robot.state}")
66
67# Turn
68robot.state = np.array([0.0, 0.0, 0.0])
69for _ in range(100):
70    robot.forward_kinematics(v=1.0, delta=np.pi/6, dt=0.01)
71
72print(f"After turning: {robot.state}")

 1def inverse_kinematics(self, v_desired, R_desired):
 2    """
 3    Compute steering angle for desired turning radius
 4    
 5    Args:
 6        v_desired: Desired linear velocity (m/s)
 7        R_desired: Desired turning radius (m)
 8            - Positive: turn left
 9            - Negative: turn right
10            - Infinity: straight line
11    
12    Returns:
13        (v, delta): velocity and steering angle
14    """
15    if abs(R_desired) > 1000:  # Effectively straight
16        delta = 0
17    else:
18        delta = np.arctan(self.L / R_desired)
19    
20    return v_desired, delta
21
22# Add to FrontWheelDrive class
23FrontWheelDrive.inverse_kinematics = inverse_kinematics
24
25# Example: Turn with 2m radius
26robot = FrontWheelDrive(wheelbase=0.5)
27v, delta = robot.inverse_kinematics(v_desired=1.0, R_desired=2.0)
28print(f"For R=2m: v={v:.2f} m/s, δ={np.degrees(delta):.1f}°")

 1def ackermann_angles(self, delta_center, track_width=0.4):
 2    """
 3    Calculate individual wheel steering angles
 4    
 5    Args:
 6        delta_center: Center steering angle (rad)
 7        track_width: Distance between left and right wheels (m)
 8    
 9    Returns:
10        (delta_inner, delta_outer): Steering angles for inner and outer wheels
11    """
12    if abs(delta_center) < 1e-6:
13        return 0, 0
14    
15    # Turning radius at center
16    R = self.L / np.tan(delta_center)
17    
18    # Inner wheel (sharper angle)
19    R_inner = R - track_width / 2
20    delta_inner = np.arctan(self.L / R_inner)
21    
22    # Outer wheel (gentler angle)
23    R_outer = R + track_width / 2
24    delta_outer = np.arctan(self.L / R_outer)
25    
26    return delta_inner, delta_outer
27
28# Add to class
29FrontWheelDrive.ackermann_angles = ackermann_angles
30
31# Example
32robot = FrontWheelDrive(wheelbase=0.5)
33delta_inner, delta_outer = robot.ackermann_angles(delta_center=np.pi/6, track_width=0.4)
34print(f"Inner wheel: {np.degrees(delta_inner):.1f}°")
35print(f"Outer wheel: {np.degrees(delta_outer):.1f}°")

 1def pure_pursuit(self, target_x, target_y, lookahead_distance=1.0):
 2    """
 3    Pure pursuit path following algorithm
 4    
 5    Args:
 6        target_x, target_y: Target point coordinates
 7        lookahead_distance: Lookahead distance (m)
 8    
 9    Returns:
10        Steering angle to reach target
11    """
12    x, y, theta = self.state
13    
14    # Transform target to robot frame
15    dx = target_x - x
16    dy = target_y - y
17    
18    # Rotate to robot frame
19    dx_robot = dx * np.cos(theta) + dy * np.sin(theta)
20    dy_robot = -dx * np.sin(theta) + dy * np.cos(theta)
21    
22    # Calculate curvature
23    curvature = 2 * dy_robot / (lookahead_distance ** 2)
24    
25    # Calculate steering angle
26    delta = np.arctan(curvature * self.L)
27    
28    # Limit steering angle
29    max_delta = np.pi / 3
30    delta = np.clip(delta, -max_delta, max_delta)
31    
32    return delta
33
34# Add to class
35FrontWheelDrive.pure_pursuit = pure_pursuit