simulation.py

from config import *
from controller import *


# Main simulation function 
def run_simulation(L1, L2,pos_data, vel_data, h_data):
    # Initialize the CBF-QP controller
    # Dimension = drone number * 6 constraints * 2 L shape obstacles
    cbf_controller, u1, u1_ref, _, Lgh_list, b = initialize_controller()

    # Main simulation Loop
    for t in range(1, len(TIME_DATA)):

        # Current positions
        current_positions = np.array([pos_data[i][t - 1, :2] for i in range(N)])

        # Compute the desired control input u_des: 
        u_des = compute_u_des(KAPPA_J, A_J, OMEGA_J, current_positions, t)
        u1_ref.value = u_des.reshape(N*DIM,1)

        # If the Safety filter is on,
        if FILTER_ON:
            
            # Update the constraint matrices 
            update_Lgh(L1, L2, Lgh_list)
            update_b(L1, L2,b, ALPHAS,t, h_data, pos_data)
            
            # Compute the control input from CBF-QP        
            cbf_controller.solve(solver='CLARABEL',canon_backend=cp.SCIPY_CANON_BACKEND)
            u = u1.value.reshape(N, DIM)

        # If no safety filter
        else:
            u = u_des

        # Save the position and control input of agents
        for i in range(N):
            vel_data[i][t, :2] = u[i,:]
            pos_data[i][t, :2] = pos_data[i][t - 1, :2] + DT * u[i,:]
            
    if SAVE_DATA:
        np.save("pos_data.npy", pos_data, allow_pickle=True)