lane_change.py

import numpy as np
import logging
from datetime import datetime

class SharedControl_LC:
    def __init__(self, log=False):
        self.L = 2.6        # Wheelbase (m)
        self.Lf = 0.9       # Distance from CoG to front axle (m)
        self.Lr = 1.7       # Distance from CoG to rear axle (m)
        self.m = 1600       # Vehicle mass (kg)
        self.v_init = 15    # Vehicle velocity (m/s)
        
        self.QA_max = np.diag([0, 5, 10, 0])   # max ADAS state weights [x, y, theta, v]
        self.QD_max = np.diag([0, 10, 10, 0])  # max driver state weights [x, y, theta, v]
        self.RA = np.diag([1.0, 0.1])          # ADAS control weights [delta, accel]
        self.RD = np.diag([2.0, 0.1])          # Driver control weights [delta, accel]
        self.T = 0.5        # Preview horizon (s)
        self.dt = 0.01      # Time step (s)
        
        self.log = log
        if log: self.setup_logger()
    
    def kinematic_model(self, x, delta, accel):
        x_dot = np.zeros(4)
        
        theta = x[2]
        v = x[3]
        
        x_dot[0] = v * np.cos(theta)           # x_dot
        x_dot[1] = v * np.sin(theta)           # y_dot
        x_dot[2] = v * np.tan(delta) / self.L  # theta_dot
        x_dot[3] = accel                       # v_dot
        
        return x_dot
    
    def discretize_model(self, x, delta, accel):
        k1 = self.kinematic_model(x, delta, accel)
        k2 = self.kinematic_model(x + self.dt/2 * k1, delta, accel)
        k3 = self.kinematic_model(x + self.dt/2 * k2, delta, accel)
        k4 = self.kinematic_model(x + self.dt * k3, delta, accel)
        
        x_next = x + self.dt/6 * (k1 + 2*k2 + 2*k3 + k4)
        return x_next
    
    def jacobian_state(self, x, delta):
        theta = x[2]
        v = x[3]
        
        A = np.zeros((4, 4))
        
        A[0, 2] = -v * np.sin(theta)
        A[0, 3] = np.cos(theta)
        A[1, 2] = v * np.cos(theta)
        A[1, 3] = np.sin(theta)
        A[2, 3] = np.tan(delta) / self.L
        
        A[0, 0] = 1
        A[1, 1] = 1
        A[2, 2] = 1
        A[3, 3] = 1
        
        return A
    
    def jacobian_input(self, x, delta):
        v = x[3]
        
        B = np.zeros((4, 2))
        
        B[2, 0] = v / (self.L * np.cos(delta)**2)
        B[3, 1] = 1
        
        return B
    
    def solve_riccati(self, A, BD, BA, QD, QA, RD, RA):
        n = A.shape[0]
        PD = np.zeros((n, n))
        PA = np.zeros((n, n))
        
        SD = BD @ np.linalg.inv(RD) @ BD.T
        SA = BA @ np.linalg.inv(RA) @ BA.T
        
        steps = int(self.T/self.dt)
        for _ in range(steps):
            PD_dot = (A - SA @ PA).T @ PD + PD @ (A - SA @ PA) - PD @ SD @ PD + QD
            PD += PD_dot * self.dt
            
            PA_dot = (A - SD @ PD).T @ PA + PA @ (A - SD @ PD) - PA @ SA @ PA + QA
            PA += PA_dot * self.dt
        
        return PD, PA
    
    def compute_control(self, x, t, alpha_D, alpha_A, path_ref):
        x_ref, y_ref, theta_ref, v_ref = self.get_reference_at_position(x, path_ref)
        
        # Create reference state vector
        state_ref = np.array([x_ref, y_ref, theta_ref, v_ref])
        
        # State error
        x_tilde = x - state_ref
        
        # Current steering
        current_delta = 0
        
        # Linearization of the model around current state
        A = self.jacobian_state(x, current_delta)
        B = self.jacobian_input(x, current_delta)
        
        # Scale input matrices for driver and ADAS
        BD = B 
        BA = B
        
        # Scale weight matrices by authority levels
        # QD = alpha_D * self.QD_max
        # QA = alpha_A * self.QA_max
        QD = self.QD_max
        QA = self.QA_max

        RD = self.RD  
        RA = self.RA
        
        # Solve Riccati equations
        PD, PA = self.solve_riccati(A, BD, BA, QD, QA, RD, RA)
        
        # Compute control inputs
        uD = -np.linalg.inv(RD) @ BD.T @ PD @ x_tilde
        uA = -np.linalg.inv(RA) @ BA.T @ PA @ x_tilde

        # shared control
        uD = alpha_D * uD
        uA = alpha_A * uA
            
        return uD, uA
    
    def get_reference_at_position(self, x, path_ref):
        current_x, current_y = x[0], x[1]
        
        x_path = path_ref[:, 0]
        y_path = path_ref[:, 1]
        theta_path = path_ref[:, 2]
        v_path = path_ref[:, 3]
        
        # Find closest point on reference path
        distances = np.sqrt((x_path - current_x)**2 + (y_path - current_y)**2)
        idx = np.argmin(distances)
        
        # Look ahead a few points for smoother control
        look_ahead = 5  # Look ahead by 5 points
        idx_ahead = min(idx + look_ahead, len(x_path) - 1)
        
        return x_path[idx_ahead], y_path[idx_ahead], theta_path[idx_ahead], v_path[idx_ahead]
    
    def bezier_curve(self, P0, P1, P2, P3, t):
        return (
            (1 - t)**3 * P0 +
            3 * (1 - t)**2 * t * P1 +
            3 * (1 - t) * t**2 * P2 +
            t**3 * P3
        )
    
    def bezier_derivative(self, P0, P1, P2, P3, t):
        return (
            3 * (1 - t)**2 * (P1 - P0) +
            6 * (1 - t) * t * (P2 - P1) +
            3 * t**2 * (P3 - P2)
        )
    
    def generate_trajectory(self, sim_length):
        """
        Generate a reference trajectory for a lane change maneuver
        """
        t = np.arange(0, sim_length, self.dt)
        x_ref = self.v_init * t
        
        t_start = 4.0       # Start of lane change
        t_duration = 2.5    # Duration of lane change
        lane_width = 3.75   # Target lateral position (m)
        
        # Initialize arrays
        y_ref = np.zeros_like(t)
        theta_ref = np.zeros_like(t)
        v_ref = np.ones_like(t) * self.v_init
        
        # Control points for Bezier curve
        P0 = 0.0
        P1 = 0.0
        P2 = lane_width
        P3 = lane_width
        
        # Create the lane change trajectory
        idx_start = int(t_start / self.dt)
        idx_end = int((t_start + t_duration) / self.dt)
        
        # Apply Bezier curve for the lane change portion
        t_normalized = np.linspace(0, 1, idx_end - idx_start)
        y_ref[idx_start:idx_end] = self.bezier_curve(P0, P1, P2, P3, t_normalized)
        y_ref[idx_end:] = lane_width
        
        # Calculate heading from the derivative of y position
        dy_dt = np.zeros_like(t)
        dy_dt[idx_start:idx_end] = self.bezier_derivative(P0, P1, P2, P3, t_normalized) / t_duration
        theta_ref = np.arctan2(dy_dt, self.v_init)
        
        path_ref = np.column_stack((x_ref, y_ref, theta_ref, v_ref))
        
        return path_ref, y_ref, theta_ref
    
    def transition_functions(self, t, ts, te, type='linear', x=None, path_ref=None):
        if t < ts:
            return 0.0, 1.0  # Full ADAS control
        elif t > te:
            return 1.0, 0.0  # Full driver control
        
        if type == 'step':
            alpha_A = 0.0
        elif type == 'linear':
            alpha_A = -(1/(te - ts)) * (t - ts) + 1
        elif type == 'cooperative':
            alpha_A = 0.5
        elif type == 'sigmoid':
            alpha_A = 1 - 1 / (1 + np.exp(-10 * (t - (ts + te) / 2) / (te - ts)))
        elif type == 'exponential':
            normalized_t = (t - ts) / (te - ts)
            alpha_A = np.exp(-5 * normalized_t)
        elif type == 'blend':
            normalized_t = (t - ts) / (te - ts)
            linear_A = 1 - normalized_t
            sigmoid_A = 1 - 1 / (1 + np.exp(-10 * (normalized_t - 0.5)))
            alpha_A = 0.5 * linear_A + 0.5 * sigmoid_A
        elif type == 'lagged':
            if t <= ts + 2:
                alpha_A = 1.0
            else:
                alpha_A = 0.5
        elif type == 'adaptive':
            if x is None or path_ref is None:
                alpha_A = 0.5
            else:
                x_ref, y_ref, theta_ref, v_ref = self.get_reference_at_position(x, path_ref)
                lateral_error = abs(x[1] - y_ref)
                heading_error = abs(x[2] - theta_ref)
                max_lateral_error = 0.3
                max_heading_error = 0.2
                norm_lateral_error = min(lateral_error / max_lateral_error, 1.0)
                norm_heading_error = min(heading_error / max_heading_error, 1.0)
                total_error = 0.7 * norm_lateral_error + 0.3 * norm_heading_error
                base_alpha_A = 0.5
                error_factor = total_error * 0.14
                
                alpha_A = min(max(base_alpha_A + error_factor, 0.0), 1.0)
        else:
            raise ValueError("Unknown transition type")
        
        alpha_D = 1.0 - alpha_A

        return alpha_D, alpha_A
    
    def setup_logger(self):
        timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
        logging.basicConfig(
            filename=f'logs/shared_control_log_{timestamp}.txt',
            level=logging.INFO,
            format='%(asctime)s - %(message)s',
            datefmt='%Y-%m-%d %H:%M:%S'
        )

        console_handler = logging.StreamHandler()
        console_handler.setLevel(logging.INFO)
        logging.getLogger().addHandler(console_handler)

    def log_control_state(self, t, alpha_D, alpha_A, x):
        phase = self.determine_control_phase(t)
        
        log_msg = (
            f"Time: {t:.2f}s | "
            f"Phase: {phase} | "
            f"Authority - Driver: {alpha_D:.2f}, AV: {alpha_A:.2f} | "
            f"Position: ({x[0]:.2f}, {x[1]:.2f}) | "
            f"Heading: {np.degrees(x[2]):.2f}° | "
            f"Velocity: {x[3]:.2f} m/s"
        )
        logging.info(log_msg)

    def determine_control_phase(self, t):
        if t < 3.0:
            return "AV Full Control"
        elif t > 10.0:
            return "Driver Full Control"
        else:
            return "Transition Phase"

    def simulate_lane_change(self, transition_type='linear'):
        # Simulation time
        sim_length = 15.0
        t_sim = np.arange(0, sim_length, self.dt)
        
        ts = 3.0      # Start of transition
        te = 10.0      # End of transition

        path_ref, y_ref, psi_ref = self.generate_trajectory(sim_length)
        
        x = np.array([0.0, 0.0, 0.0, self.v_init])      # [x, y, theta, v]
        
        states = [x.copy()]
        controls_D = []
        controls_A = []
        alphas_D = []
        alphas_A = []
        
        deltas = []
        accels = []
        
        for i, t in enumerate(t_sim[:-1]):
            alpha_D, alpha_A = self.transition_functions(t, ts, te, transition_type, x, path_ref)
            alphas_D.append(alpha_D)
            alphas_A.append(alpha_A)
            
            uD, uA = self.compute_control(x, t, alpha_D, alpha_A, path_ref)
            controls_D.append(uD)
            controls_A.append(uA)
            
            delta = alpha_D * uD[0] + alpha_A * uA[0]
            accel = alpha_D * uD[1] + alpha_A * uA[1]
            
            deltas.append(delta)
            accels.append(accel)
            
            x = self.discretize_model(x, delta, accel)
            states.append(x.copy())

            if self.log:
                self.log_control_state(t, alpha_D, alpha_A, x)
        
        states = np.array(states)
        controls_D = np.array(controls_D)
        controls_A = np.array(controls_A)
        deltas = np.array(deltas)
        accels = np.array(accels)
        
        return (states, controls_D, controls_A, 
                np.array(alphas_D), np.array(alphas_A), 
                t_sim, y_ref, psi_ref, deltas, accels)
    
    def do_lane_change(self, transition_types):
        results = {}

        for trans_type in transition_types:
            result = self.simulate_lane_change(trans_type)
            results[trans_type] = result
            print(f"Completed {trans_type} transition!")

        print("-"*50)

        return results

if __name__ == "__main__":
    controller = SharedControl_LC()
    transition_types = ['step', 'linear', 'cooperative', 'sigmoid', 'blend', 'lagged', 'adaptive']
    results = controller.do_lane_change(transition_types)