simulation_side.py

import os

import numpy as np
import torch

from src.utils.simulation import initialize_generative_model, initialize_generative_process, run_simulation, find_parameters
from src.utils.saving import check_config_availability, save_results

from src.intersection.dynamics_true import Dynamics_true
from src.intersection.decoder_true import Decoder_true
from src.intersection.reward import IndependentReward


def set_config(initial_state, model_params, ttc_trigger):

    # Scenario descriptin
    # Copy initial state
    Config = {

        # Bicycle model descriptions
        "dt": 0.2,
        
        # Perception noise
        "w_sd_process": 0., #0.001,
        "x_sd_perc": 0.02 * model_params['perc_noise_factor'],
        "y_sd_perc": 0.002 * model_params['perc_noise_factor'],
        "theta_sd_perc": 0.02 * model_params['perc_noise_factor'],
        "delta_sd_perc": 0.2 * model_params['perc_noise_factor'],
        "v_sd_perc": 0.02 * model_params['perc_noise_factor'],
        "a_sd_perc": 0.002 * model_params['perc_noise_factor'],
        "w_sd_perc": 0.2 * model_params['perc_noise_factor'],
        "LA_sd_perc": 0.001 * model_params['perc_noise_factor'], # Looming angles perception
        "d_LA_sd_perc": 0.001 * model_params['perc_noise_factor'],
        "dd_LA_sd_perc": 0.0001 * model_params['perc_noise_factor'],

        # Generative model update noise
        "a_sd_model": model_params['a_sd_model'], # (0: Assume constant acceleration, inf: Make no assumption about future acceleration)
        "w_sd_model": int(10000 * model_params['a_sd_model'] * initial_state['w_max'] / initial_state['a_max']) / 10000, 
        "noise_pred_fac": model_params['noise_pred_fac'], # Additional factor applied to sd_model only during EFE calculation, not during belief update
        "use_pedals": model_params['use_pedals'],

        # Belief update method
        "uncorrolated_belief_update": True,
        "use_looming_perception": model_params['Loom_perc'],
        "looming_threshold": model_params['d_phi_thres'],
        "looming_change_observable": model_params['Loom_change_obs'],

        # Current scenario description
        "a_tar_min_intensity": model_params['a_tar_min_intensity'], # The intensity of the minimum target acceleration
        "ttc_trigger": ttc_trigger, # At what project ttc does the maneuver trigger

        # Reward function parameters
        "v_ego_sd_des": model_params['pref_v_sd'],
        "a_ego_sd_des": model_params['pref_a_sd'],
        "w_ego_sd_des": model_params['pref_w_sd'],
        "lane_change_cost": model_params['lane_change_cost'],
        "road_leave_cost": model_params['lane_cost'],
        "road_gaze_preference": model_params['road_pref'],
        "collision_cost": model_params['coll_cost'],
        "collision_cost_adjusted": model_params['collision_cost_adjusted'],
        "use_looming_reward": model_params['Loom_reward'],
        "weigh_particles": model_params['weigh_particles'],
        "full_violation_factor": model_params['full_violation_factor'],
        "unpunished_heading": model_params['unpunished_heading'],
        "N_norm": model_params['N_norm'],
        "H_norm": model_params['H_norm'],
        "alpha": model_params['alpha'],

        # Planner parameters
        "num_plans": model_params['num_plan'],
        "a_sd_plan": model_params['a_sd_plan'],
        "EA_mode": model_params['EA_mode'],
        "EA_fac": model_params['EA_fac'],
        "EA_init": model_params['EA_init'],
        "Sample_steering_rate": model_params['sample_steering_rate'],
        "H": model_params['H'], 
        "plan_ignore_w": model_params['plan_ignore_w'],
        "plan_smooth_delta": model_params['plan_smooth_delta'],
    }
    
    # add initial state
    Config.update(initial_state)

    # Update v_ego
    Config['v_ego_des'] = Config['v_ego'] + model_params['v_diff']


    config = {
        "dynamics": {
            "lane_width": Config["lane_width"],
            "lane_ratio": 1.,
            "p": 1.,
            "lf": Config["lf"],
            "lr": Config["lr"],
            "d": Config["d"],
            "a_max": Config["a_max"],
            "w_max": Config["w_max"],
            "x_sd": 0.,
            "y_sd": 0.,
            "theta_sd": 0.,
            "delta_sd": 0.,
            "v_sd": 0.,
            "a_sd": Config["a_sd_model"],
            "w_sd": Config["w_sd_model"],
            "noise_pred_fac": Config["noise_pred_fac"], # The factor that the model noise is multiplied with during the EFE calculation
            "a_tar_min": - Config["a_tar_min_intensity"] * Config["a_max"],
            "dt": Config["dt"],
            "N_norm": Config["N_norm"],
            "H_norm": Config["H_norm"],
        },

        "env": {
            "x_sd": 0.,
            "y_sd": 0.,
            "theta_sd": 0.,
            "delta_sd": 0.,
            "v_sd": 0.,
            "a_sd": 0.,
            "w_sd": Config["w_sd_process"],
            "ttc_trigger": Config["ttc_trigger"],
        },
        "decoder": {
            "x_sd": Config["x_sd_perc"],
            "y_sd": Config["y_sd_perc"],
            "theta_sd": Config["theta_sd_perc"],
            "delta_sd": Config["delta_sd_perc"],
            "v_sd": Config["v_sd_perc"],
            "a_sd": Config["a_sd_perc"],
            "w_sd": Config["w_sd_perc"],
            "LA_sd": Config["LA_sd_perc"],
            "d_LA_sd": Config["d_LA_sd_perc"],
            "dd_LA_sd": Config["dd_LA_sd_perc"],
            "dist_type": "norm",
        },
        "encoder": {
            "resample_method": "systematic",
            "num_particles": 75,
            "planner_particles": 75,
            "uncorrolated_belief_update": Config["uncorrolated_belief_update"],
            "use_looming_perception": Config["use_looming_perception"],
            "looming_threshold": Config["looming_threshold"],
            "looming_change_observable": Config["looming_change_observable"],
        },
        "reward": {
            "lane_width": Config["lane_width"],
            "d": Config["d"],
            "l": Config["lf"] + Config["lr"],
            "v_mu": Config["v_ego_des"],
            "a_mu": 0.,
            "w_mu": 0.,
            "v_sd": Config["v_ego_sd_des"],
            "a_sd": Config["a_ego_sd_des"],
            "w_sd": Config["w_ego_sd_des"],
            "theta_unpunished": Config["unpunished_heading"], # The heading angle that is not punished
            "theta_max": 90., # Maximum heading angle allowed (in degrees)
            "road_pref": Config["road_gaze_preference"], # Preference for looking at the road
            "lane_cost": Config["road_leave_cost"], # Cost of leaving lane
            "lane_change_cost": Config["lane_change_cost"], # Cost of changing lane
            "coll_cost": Config["collision_cost"], # Cost of collision
            "collision_cost_adjusted": Config["collision_cost_adjusted"], # If True, the collision cost is adjusted according to the relative impact speed
            "use_looming_reward": Config["use_looming_reward"],
            "weigh_particles": Config["weigh_particles"],
            "full_violation_factor": Config["full_violation_factor"],
            "alpha": Config["alpha"],
        },
        "planner": {
            "mu_a": [0., 0.],
            "sd_a": [Config['a_sd_plan'], 0.02 * Config["a_sd_plan"]],
            "H": Config["H"],
            "num_samples": Config["num_plans"], # Number of plans evaluated
            "top_percent": 0.1,
            "iters": 10,
            "EA_mode": Config["EA_mode"],
            "EA_fac": Config["EA_fac"],
            "EA_init": Config["EA_init"], # If True, the initial planm is optimized instead of using zero controls
            "use_pedals": Config["use_pedals"],
            "sample_steering_rate": Config["Sample_steering_rate"],
            "verbose": False,
            "plan_ignore_w": Config["plan_ignore_w"],
            "plan_smooth_delta": Config["plan_smooth_delta"],
        },
        "init_belief": {
            "I": [1, 0], # The agent has the gaze on the road
            
            "x_ego": Config["x_ego"],
            "y_ego": Config["y_ego"],
            "theta_ego": Config["theta_ego"],
            "delta_ego": Config["delta_ego"],
            "v_ego": Config["v_ego"],

            "x_tar": Config["x_tar"],
            "y_tar": Config["y_tar"],
            "theta_tar": Config["theta_tar"],
            "delta_tar": Config["delta_tar"],
            "v_tar": Config["v_tar"],
        },
        "init_state": {
            "x_ego": Config["x_ego"],
            "y_ego": Config["y_ego"],
            "theta_ego": Config["theta_ego"],
            "delta_ego": Config["delta_ego"],
            "v_ego": Config["v_ego"],

            "x_tar": Config["x_tar"],
            "y_tar": Config["y_tar"],
            "theta_tar": Config["theta_tar"],
            "delta_tar": Config["delta_tar"],
            "v_tar": Config["v_tar"],
            "t_acc": Config["ttc_trigger"],
        },
        # sim
        "rollout_batch_size": 12,
        "T": 60
    }
    return Config, config


def simulate(config, device=torch.device("cpu")):
    # Set up the configuration
    b, w, decoder, encoder, belief_dynamics = initialize_generative_model(config, device=device)
    
    reward = IndependentReward(
        decoder            = decoder,
        lane_width         = config["reward"]["lane_width"],
        d                  = config["reward"]["d"],
        l                  = config["reward"]["l"],
        a_max              = config["dynamics"]["a_max"],
        a_tar_min          = config["dynamics"]["a_tar_min"],
        dt                 = config["dynamics"]["dt"],
        v_mu               = config["reward"]["v_mu"],
        a_mu               = config["reward"]["a_mu"],
        w_mu               = config["reward"]["w_mu"],
        v_sd               = config["reward"]["v_sd"],
        a_sd               = config["reward"]["a_sd"],
        w_sd               = config["reward"]["w_sd"],
        theta_unpunished   = config["reward"]["theta_unpunished"],
        theta_max          = config["reward"]["theta_max"],
        road_pref          = config["reward"]["road_pref"],
        lane_cost          = config["reward"]["lane_cost"],
        lane_change_cost   = config["reward"]["lane_change_cost"],
        coll_cost          = config["reward"]["coll_cost"],
        coll_cost_adjusted = config["reward"]["collision_cost_adjusted"],
        use_looming_reward = config["reward"]["use_looming_reward"],
        weigh_particles    = config["reward"]["weigh_particles"],
        full_violation_factor = config["reward"]["full_violation_factor"],
        device             = device)
    
    # Add reward to belief dynamics
    belief_dynamics.add_reward_function(reward)

    dynamics_true = Dynamics_true(
        lane_width      = config["dynamics"]["lane_width"],
        lane_ratio      = config["dynamics"]["lane_ratio"],
        p               = config["dynamics"]["p"],
        lf              = config["dynamics"]["lr"],
        lr              = config["dynamics"]["lr"],
        d               = config["dynamics"]["d"],
        a_max           = config["dynamics"]["a_max"],
        w_max           = config["dynamics"]["w_max"],
        x_sd            = config["env"]["x_sd"],
        y_sd            = config["env"]["y_sd"],
        theta_sd        = config["env"]["theta_sd"],
        delta_sd        = config["env"]["delta_sd"],
        v_sd            = config["env"]["v_sd"],
        a_sd            = config["env"]["a_sd"],
        w_sd            = config["env"]["w_sd"],
        ttc_trigger     = config["env"]["ttc_trigger"],
        dt              = config["dynamics"]["dt"],
        device          = device)
    
    decoder_true = Decoder_true(
        d         = config["dynamics"]["d"],
        lr        = config["dynamics"]["lr"],
        lf        = config["dynamics"]["lf"],
        x_sd      = config["decoder"]["x_sd"] * 0.001,
        y_sd      = config["decoder"]["y_sd"] * 0.001,
        theta_sd  = config["decoder"]["theta_sd"] * 0.001,
        delta_sd  = config["decoder"]["delta_sd"] * 0.001,
        v_sd      = config["decoder"]["v_sd"] * 0.001,
        a_sd      = config["decoder"]["a_sd"] * 0.001,
        w_sd      = config["decoder"]["w_sd"] * 0.001,
        dist_type = config["decoder"]["dist_type"],
        device    = device)
        
        
    agent, env, eta = initialize_generative_process(config, encoder, decoder, belief_dynamics,
                                                    dynamics_true, decoder_true, reward, device)

    # run actual simulation
    data = run_simulation(config, agent, env, eta, b, w)
    return data


if __name__ == "__main__":
    # Set the fixed varaibles
    # Define the decoder parameters
    # The uncertainty about future target agent control actions
    a_sd_model = 3.0 # m/s^2
    # The use of looming based perception
    Loom_perc = True
    # The looming observation threshold
    d_phi_thres = 0.00215 # In rad/s
    # The method for observing looming changes
    Loom_change_obs = -1
    # The factor applied to perception noise
    perc_noise_factor = 0.01 

    # Set the planner varaibles
    # The factor applied to the noise during planning
    noise_pred_fac = 0.2
    # The number of plans to consider
    num_plan = 100
    # The initial std for sampling acceleration during planing
    a_sd_plan = 5 # m/s^2
    # Do we sample plans over steering rate instead of its derivative
    sample_steering_rate = True
    # The use of realistic pedal use
    use_pedals = True
    # The number of planning steps
    H = 30
    # The requirement to assume w = 0 during kinematic predictions of other agents
    plan_ignore_w = True
    # The smoothing of the steering angle over past timesteps
    plan_smooth_delta = True


    # Reward function parameters
    # std of desired velocity of the ego vehicle
    pref_v_sd = 0.5 # m/s
    # std of desired acceleration of the ego vehicle
    pref_a_sd = 0.1 # m/s^2
    # std of desired angular velocity of the ego vehicle
    pref_w_sd = 0.02 # rad/s
    # Punishment for leaving road
    lane_cost = -15000
    # Punishment for leaving desired lane
    lane_change_cost = -1000
    # Reward for observing collision
    coll_cost = -10000
    # Preference for looking at the road
    road_pref = 0 # 0: Always look at road, -inf: Never look at road
    # Specific reward function used
    Loom_reward = 'V7' # Use loom reward, with strictes tau_inv preference
    # which heading values are not punished
    unpunished_heading = 85 # In Degrees
    # Decide whether collision cost is adjusted according to the relative impact speed
    collision_cost_adjusted = True
    # Use the epistemic value
    alpha = 1.0
    # The number of sampled control inputs used for applying norm bias
    N_norm = 32
    # The number of timesteps we use for the long time horizon
    H_norm = 20
    # The weight used for moderate norm infractions
    weigh_particles = 0.001
    # The factor applied to moderate weight for severe infractions
    full_violation_factor = 0.01

    # Evidence accumulation parameters
    # What evidence should be accumulated
    EA_mode = 'Surprise'
    # Should first iteration be
    EA_init = False # Do not use initial evidence accumulation
    # The evidence accumulation factor
    EA_fac = -5.95

    model_params = {
        # Perception parameters
        'a_sd_model': a_sd_model,
        'Loom_perc': Loom_perc,
        'd_phi_thres': d_phi_thres,
        'Loom_change_obs': Loom_change_obs,
        'perc_noise_factor': perc_noise_factor,

        # Planning parameters
        'noise_pred_fac': noise_pred_fac,
        'num_plan': num_plan,
        'a_sd_plan': a_sd_plan,
        'sample_steering_rate': sample_steering_rate,
        'use_pedals': use_pedals,
        'H': H,
        'plan_ignore_w': plan_ignore_w,
        'plan_smooth_delta': plan_smooth_delta,

        # Reward function parameters
        'pref_v_sd': pref_v_sd,
        'pref_a_sd': pref_a_sd,
        'pref_w_sd': pref_w_sd,
        'lane_cost': lane_cost,
        'lane_change_cost': lane_change_cost,
        'coll_cost': coll_cost,
        'road_pref': road_pref,
        'Loom_reward': Loom_reward,
        'weigh_particles': weigh_particles,
        'full_violation_factor': full_violation_factor,
        'unpunished_heading': unpunished_heading,
        'collision_cost_adjusted': collision_cost_adjusted,
        'N_norm': N_norm,
        'H_norm': H_norm,
        'alpha': alpha,

        # Evidence accumulation parameters
        'EA_mode': EA_mode,
        'EA_fac': EA_fac,
        'EA_init': EA_init,
    }


    
    ################################################################################
    #                      Define the current scenario                             #
    ################################################################################

    # Set name for saving files:
    name = 'intersection'

    # Get the oncoming specific values
    # The initial buffer of time before tar agent starts accelerating
    ttc_trigger = 0.25 # s

    # Define intial state of the target agent
    initial_state = {
        # Environment parameters
        'lane_width': 3.5,
        'd': 1.72,
        'lf': 2.1,
        'lr': 2.1,
        'a_max': 8,
        'w_max': 1.22,

        # The initial kinematic state
        'x_ego': -50, # Scenario dependent
        'y_ego': 0.0,
        'theta_ego': 0.0,
        'delta_ego': 0.0,
        'v_ego': 16.67, # ~60 km/h

        'x_tar': 0.0,
        'y_tar': -10, # Scenario dependent
        'theta_tar': 0.5 * np.pi,
        'delta_tar': 0.0,
        'v_tar': 0.0 # Scenario dependent
    }


    ################################################################################
    #                      Run the simulations                                     #
    ################################################################################

    # print torch version and cuda device
    print('Torch version:', torch.__version__)
    if torch.cuda.is_available():
        print('Cuda available')
        device = torch.device('cuda', index=0) 
        torch.cuda.set_device(0)

    else:
        print('GPU not available')
        device = torch.device('cpu')

    for EA_mode in ['Surprise', 'None']:
        for Loom_perc in [True, False]:
            for d_phi_thres in [0.00215, 0.0]:
                for noise_pred_fac in [0.2, 0.002]:
                    for N_norm in [32, 1]:
                        for use_pedals in [True, False]:
                            for alpha in [1.0, 0.0]:

                                num_ablations = 0
                                if not Loom_perc:
                                    num_ablations += 1
                                if not d_phi_thres == 0.00215:
                                    num_ablations += 1
                                if not noise_pred_fac == 0.2:
                                    num_ablations += 1
                                if not N_norm == 32:
                                    num_ablations += 1
                                if not EA_mode == 'Surprise':
                                    num_ablations += 1
                                if not use_pedals:
                                    num_ablations += 1
                                if not num_plan == 100:
                                    num_ablations += 1
                                if not alpha == 1.0:
                                    num_ablations += 1

                                
                                if num_ablations > 1:
                                    continue

                                # overwrite model params
                                model_params['Loom_perc'] = Loom_perc
                                model_params['d_phi_thres'] = d_phi_thres
                                model_params['noise_pred_fac'] = noise_pred_fac
                                model_params['N_norm'] = N_norm
                                model_params['EA_mode'] = EA_mode
                                model_params['use_pedals'] = use_pedals
                                model_params['num_plan'] = num_plan
                                model_params['alpha'] = alpha


                                # Interpolate the required a_tar_min and v_diff
                                thw_des = 1.5 # s
                                _, a_tar_min = find_parameters(initial_state['v_ego'], EA_fac, noise_pred_fac, H, d_phi_thres, thw_des)
                                a_tar_min_intensity = - a_tar_min / initial_state['a_max'] 

                                # Add the tar agent assumptions to model params
                                model_params['v_diff'] = 0.0
                                model_params['a_tar_min_intensity'] = a_tar_min_intensity

                                # Some playroom for setting up scenario
                                for v_ego in [14.66, 15.5, 16.33]:
                                    initial_state['v_ego'] = v_ego
                                    # Get traveld distance until ttc
                                    y_radius = 10 + 0.5 * initial_state['lane_width'] + initial_state['lf']
                                    acceleration = 1.4715 # m/s^2, 0.15 g
                                    ttc_acc = 4.3
                                    d_traveled = 0.5 * acceleration * ttc_acc ** 2
                                    angle_travelled = d_traveled / y_radius
                                    x_travelled = y_radius * (1 - np.cos(angle_travelled))

                                    # Go through scenarios
                                    for starting_condition in ['RS', 'RNS']:
                                        if starting_condition == 'RS':
                                            # Tar vehicle start stopped at the intersection
                                            initial_state['y_tar'] = - y_radius
                                            initial_state['v_tar'] = 0.0

                                            # We know time to collision:
                                            ttc = 4.3

                                            # Get corresponding angle
                                            initial_state['x_ego'] = x_travelled - initial_state['v_ego'] * (ttc + ttc_trigger) - initial_state['lf']
                                        else:
                                            # Target vehicle starts at 25 km/h
                                            initial_state['v_tar'] = 6.802 # m/s

                                            ttc = 2.0
                                            reasonable_stop_acceleration = 7.5 # m/s^2
                                            d_stop = (initial_state['v_tar'] ** 2) / (2 * reasonable_stop_acceleration)
                                            t_added = d_stop / initial_state['v_tar'] + ttc_trigger

                                            y_tar_initial = - y_radius - t_added * initial_state['v_tar']
                                            initial_state['y_tar'] = y_tar_initial

                                            # No right of way scenario
                                            initial_state['x_ego'] = x_travelled - initial_state['v_ego'] * (ttc + t_added) - initial_state['lf']

                                        torch.manual_seed(0)
                                        Config, config = set_config(initial_state, model_params, ttc_trigger)

                                        # Set prediction timeframe
                                        config["T"] = 50
                                        Config["T"] = config["T"]
                                        
                                        config_allowed, exp_index = check_config_availability(Config, name)

                                        if not config_allowed:
                                            print('')
                                            continue
                                        else:
                                            print("Config " + str(exp_index) + " is unique and will be run.")



                                        config["rollout_batch_size"] = 32

                                        # Cuda clear empty cache
                                        if torch.cuda.is_available():
                                            torch.cuda.empty_cache()
                                            gpu_total         = torch.cuda.get_device_properties(device = device).total_memory  / 2 ** 30
                                            gpu_reserved      = torch.cuda.memory_reserved(device = device) / 2 ** 30
                                            torch.cuda.reset_peak_memory_stats()
                                            print('GPU: {:5.2f}/{:5.2f} GB are available'.format(gpu_total - gpu_reserved, gpu_total))

                                        data = simulate(config, device)

                                        # Save results
                                        save_results(data, Config, name)

                                        print('')
     
import Analysis_side
# import visualization_side