Beispiel #1
0
    def take_action(self, action, num_actions, phase):
        # Set Paramaters
        fov_v = 45 * np.pi / 180
        fov_h = 80 * np.pi / 180
        r = 0.4

        ignore_collision = False
        sqrt_num_actions = np.sqrt(num_actions)

        posit = self.client.simGetVehiclePose()
        pos = posit.position
        orientation = posit.orientation

        quat = (orientation.w_val, orientation.x_val, orientation.y_val,
                orientation.z_val)
        eulers = euler_from_quaternion(quat)
        alpha = eulers[2]

        theta_ind = int(action[0] / sqrt_num_actions)
        psi_ind = action[0] % sqrt_num_actions

        theta = fov_v / sqrt_num_actions * (theta_ind -
                                            (sqrt_num_actions - 1) / 2)
        psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2)

        # print('Theta: ', theta * 180 / np.pi, end='')
        # print(' Psi: ', psi * 180 / np.pi)

        if phase == 'train':
            noise_theta = (fov_v / sqrt_num_actions) / 6
            noise_psi = (fov_h / sqrt_num_actions) / 6

            psi = psi + random.uniform(-1, 1) * noise_psi
            theta = theta + random.uniform(-1, 1) * noise_theta

            x = pos.x_val + r * np.cos(alpha + psi)
            y = pos.y_val + r * np.sin(alpha + psi)
            z = pos.z_val + r * np.sin(
                theta)  # -ve because Unreal has -ve z direction going upwards

            self.client.simSetVehiclePose(airsim.Pose(
                airsim.Vector3r(x, y, z),
                airsim.to_quaternion(0, 0, alpha + psi)),
                                          ignore_collison=ignore_collision)
        elif phase == 'infer':
            r_infer = 0.5
            vx = r_infer * np.cos(alpha + psi)
            vy = r_infer * np.sin(alpha + psi)
            vz = r_infer * np.sin(theta)
            # TODO
            # Take average of previous velocities and current to smoothen out drone movement.
            self.client.moveByVelocityAsync(
                vx=vx,
                vy=vy,
                vz=vz,
                duration=1,
                drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
                yaw_mode=airsim.YawMode(is_rate=False,
                                        yaw_or_rate=180 * (alpha + psi) /
                                        np.pi))
Beispiel #2
0
    def take_action(self, action, num_actions):
        # Set Paramaters
        fov_v = 45 * np.pi / 180
        fov_h = 80 * np.pi / 180
        r = 0.4

        ignore_collision = False
        sqrt_num_actions = np.sqrt(num_actions)

        posit = self.client.simGetVehiclePose()
        pos = posit.position
        orientation = posit.orientation

        quat = (orientation.w_val, orientation.x_val, orientation.y_val,
                orientation.z_val)
        eulers = euler_from_quaternion(quat)
        alpha = eulers[2]

        theta_ind = int(action[0] / sqrt_num_actions)
        psi_ind = action[0] % sqrt_num_actions

        theta = fov_v / sqrt_num_actions * (theta_ind -
                                            (sqrt_num_actions - 1) / 2)
        psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2)

        noise_theta = (fov_v / sqrt_num_actions) / 6
        noise_psi = (fov_h / sqrt_num_actions) / 6

        psi = psi + random.uniform(-1, 1) * noise_psi
        theta = theta + random.uniform(-1, 1) * noise_theta

        # print('Theta: ', theta * 180 / np.pi, end='')
        # print(' Psi: ', psi * 180 / np.pi)

        x = pos.x_val + r * np.cos(alpha + psi)
        y = pos.y_val + r * np.sin(alpha + psi)
        z = pos.z_val + r * np.sin(
            theta)  # -ve because Unreal has -ve z direction going upwards

        self.client.simSetVehiclePose(airsim.Pose(
            airsim.Vector3r(x, y, z), airsim.to_quaternion(0, 0, alpha + psi)),
                                      ignore_collison=ignore_collision)
Beispiel #3
0
    def Estimate(self, data):

        result = []
        for i in range(len(data)):
            result.append([])
            quaternion = (data[i][0], data[i][1], data[i][2], data[i][3])
            euler = transformations.euler_from_quaternion(quaternion)
            L = list(euler)
            L[0] = 0
            L[2] = 0
            newQuat = transformations.quaternion_from_euler(L[0], L[1], L[2])

            result[i].append(newQuat[0])
            result[i].append(newQuat[1])
            result[i].append(newQuat[2])
            result[i].append(newQuat[3])
            result[i].append(data[i][4])
            result[i].append((data[i][5]) - 5.2)
            result[i].append(data[i][6])
        return result
Beispiel #4
0
def DeepQLearning(cfg, env_process, env_folder):
    algorithm_cfg = read_cfg(config_filename='configs/DeepQLearning.cfg',
                             verbose=True)
    algorithm_cfg.algorithm = cfg.algorithm

    if 'GlobalLearningGlobalUpdate-SA' in algorithm_cfg.distributed_algo:
        # algorithm_cfg = update_algorithm_cfg(algorithm_cfg, cfg)
        cfg.num_agents = 1

    # # Start the environment
    # env_process, env_folder = start_environment(env_name=cfg.env_name)
    # Connect to Unreal Engine and get the drone handle: client
    client, old_posit, initZ = connect_drone(ip_address=cfg.ip_address,
                                             phase=cfg.mode,
                                             num_agents=cfg.num_agents)
    print(old_posit)
    initial_pos = old_posit.copy()
    # Load the initial positions for the environment
    # if Target:
    if cfg.Target:
        print("Traget Iniitial")
        reset_array, reset_array_raw, level_name, crash_threshold = initial_positions(
            cfg.env_name, initZ, cfg.num_agents)
    else:
        print("Source Iniitial")
        reset_array, reset_array_raw, level_name, crash_threshold = initial_positions2(
            cfg.env_name + "2", initZ, cfg.num_agents)
        # ""

    print("reset_array", reset_array)
    print("reset_array_raw", reset_array_raw)
    print("level_name", level_name)
    print("crash_threshold", crash_threshold)

    # Initialize System Handlers
    process = psutil.Process(getpid())
    # nvidia_smi.nvmlInit()

    # Load PyGame Screen
    screen = pygame_connect(phase=cfg.mode)

    fig_z = []
    fig_nav = []
    debug = False
    # Generate path where the weights will be saved
    cfg, algorithm_cfg = save_network_path(cfg=cfg,
                                           algorithm_cfg=algorithm_cfg)
    current_state = {}
    new_state = {}
    posit = {}
    name_agent_list = []
    agent = {}
    # Replay Memory for RL
    if cfg.mode == 'train':
        ReplayMemory = {}
        target_agent = {}

        if algorithm_cfg.distributed_algo == 'GlobalLearningGlobalUpdate-MA':
            print_orderly('global', 40)
            # Multiple agent acts as data collecter and one global learner
            global_agent = PedraAgent(algorithm_cfg,
                                      client,
                                      name='DQN',
                                      vehicle_name='global',
                                      Target=cfg.Target,
                                      data_collect=cfg.data_collect)
            ReplayMemory = Memory(algorithm_cfg.buffer_len)
            target_agent = PedraAgent(algorithm_cfg,
                                      client,
                                      name='Target',
                                      vehicle_name='global',
                                      Target=cfg.Target)

        for drone in range(cfg.num_agents):
            name_agent = "drone" + str(drone)
            name_agent_list.append(name_agent)
            print_orderly(name_agent, 40)
            # TODO: turn the neural network off if global agent is present
            agent[name_agent] = PedraAgent(algorithm_cfg,
                                           client,
                                           name='DQN',
                                           vehicle_name=name_agent,
                                           Target=cfg.Target,
                                           data_collect=cfg.data_collect)

            if algorithm_cfg.distributed_algo != 'GlobalLearningGlobalUpdate-MA':
                ReplayMemory[name_agent] = Memory(algorithm_cfg.buffer_len)
                target_agent[name_agent] = PedraAgent(algorithm_cfg,
                                                      client,
                                                      name='Target',
                                                      vehicle_name=name_agent,
                                                      Target=cfg.Target)
            current_state[name_agent] = agent[name_agent].get_state()

    elif cfg.mode == 'infer':
        name_agent = 'drone0'
        name_agent_list.append(name_agent)
        agent[name_agent] = PedraAgent(algorithm_cfg,
                                       client,
                                       name=name_agent + 'DQN',
                                       vehicle_name=name_agent,
                                       Target=cfg.Target)

        env_cfg = read_cfg(config_filename=env_folder + 'config_' +
                           ("Target" if cfg.Target else "Source") + '.cfg')
        nav_x = []
        nav_y = []
        altitude = {}
        altitude[name_agent] = []
        p_z, f_z, fig_z, ax_z, line_z, fig_nav, ax_nav, nav = initialize_infer(
            env_cfg=env_cfg, client=client, env_folder=env_folder)
        nav_text = ax_nav.text(0, 0, '')

        # Select initial position
        reset_to_initial(0, reset_array, client, vehicle_name=name_agent)
        old_posit[name_agent] = client.simGetVehiclePose(
            vehicle_name=name_agent)

    # Initialize variables
    iter = 1
    # num_collisions = 0
    episode = {}
    active = True

    print_interval = 1
    automate = True
    choose = False
    print_qval = False
    last_crash = {}
    ret = {}
    distance = {}
    num_collisions = {}
    level = {}
    level_state = {}
    level_posit = {}
    times_switch = {}
    last_crash_array = {}
    ret_array = {}
    distance_array = {}
    epi_env_array = {}
    log_files = {}

    # If the phase is inference force the num_agents to 1
    hyphens = '-' * int((80 - len('Log files')) / 2)
    print(hyphens + ' ' + 'Log files' + ' ' + hyphens)
    for name_agent in name_agent_list:
        ret[name_agent] = 0
        num_collisions[name_agent] = 0
        last_crash[name_agent] = 0
        level[name_agent] = 0
        episode[name_agent] = 0
        level_state[name_agent] = [None] * len(reset_array[name_agent])
        level_posit[name_agent] = [None] * len(reset_array[name_agent])
        times_switch[name_agent] = 0
        last_crash_array[name_agent] = np.zeros(shape=len(
            reset_array[name_agent]),
                                                dtype=np.int32)
        ret_array[name_agent] = np.zeros(shape=len(reset_array[name_agent]))
        distance_array[name_agent] = np.zeros(
            shape=len(reset_array[name_agent]))
        epi_env_array[name_agent] = np.zeros(shape=len(
            reset_array[name_agent]),
                                             dtype=np.int32)
        distance[name_agent] = 0
        # Log file
        log_path = algorithm_cfg.network_path + '/' + name_agent + '/' + cfg.mode + 'log.txt'
        print("Log path: ", log_path)
        log_files[name_agent] = open(log_path, 'w')

    print_orderly('Simulation begins', 80)

    while active:
        try:
            active, automate, algorithm_cfg, client = check_user_input(
                active, automate, agent[name_agent], client,
                old_posit[name_agent], initZ, fig_z, fig_nav, env_folder, cfg,
                algorithm_cfg)

            if automate:

                if cfg.mode == 'train':

                    if iter % algorithm_cfg.switch_env_steps == 0:
                        switch_env = True
                    else:
                        switch_env = False

                    for name_agent in name_agent_list:

                        start_time = time.time()
                        if switch_env:
                            posit1_old = client.simGetVehiclePose(
                                vehicle_name=name_agent)
                            times_switch[
                                name_agent] = times_switch[name_agent] + 1
                            level_state[name_agent][
                                level[name_agent]] = current_state[name_agent]
                            level_posit[name_agent][
                                level[name_agent]] = posit1_old
                            last_crash_array[name_agent][
                                level[name_agent]] = last_crash[name_agent]
                            ret_array[name_agent][
                                level[name_agent]] = ret[name_agent]
                            distance_array[name_agent][
                                level[name_agent]] = distance[name_agent]
                            epi_env_array[name_agent][
                                level[name_agent]] = episode[name_agent]

                            level[name_agent] = (level[name_agent] + 1) % len(
                                reset_array[name_agent])

                            print(name_agent + ' :Transferring to level: ',
                                  level[name_agent], ' - ',
                                  level_name[name_agent][level[name_agent]])

                            if times_switch[name_agent] < len(
                                    reset_array[name_agent]):
                                reset_to_initial(level[name_agent],
                                                 reset_array,
                                                 client,
                                                 vehicle_name=name_agent)
                            else:
                                current_state[name_agent] = level_state[
                                    name_agent][level[name_agent]]
                                posit1_old = level_posit[name_agent][
                                    level[name_agent]]
                                reset_to_initial(level[name_agent],
                                                 reset_array,
                                                 client,
                                                 vehicle_name=name_agent)
                                client.simSetVehiclePose(
                                    posit1_old,
                                    ignore_collison=True,
                                    vehicle_name=name_agent)
                                time.sleep(0.1)

                            last_crash[name_agent] = last_crash_array[
                                name_agent][level[name_agent]]
                            ret[name_agent] = ret_array[name_agent][
                                level[name_agent]]
                            distance[name_agent] = distance_array[name_agent][
                                level[name_agent]]
                            episode[name_agent] = epi_env_array[name_agent][
                                int(level[name_agent] / 3)]
                            # environ = environ^True
                        else:
                            # TODO: policy from one global agent: DONE
                            if algorithm_cfg.distributed_algo == 'GlobalLearningGlobalUpdate-MA':
                                agent_this_drone = global_agent
                                ReplayMemory_this_drone = ReplayMemory
                                target_agent_this_drone = target_agent
                            else:
                                agent_this_drone = agent[name_agent]
                                ReplayMemory_this_drone = ReplayMemory[
                                    name_agent]
                                target_agent_this_drone = target_agent[
                                    name_agent]

                            action, action_type, algorithm_cfg.epsilon, qvals = policy(
                                algorithm_cfg.epsilon,
                                current_state[name_agent], iter,
                                algorithm_cfg.epsilon_saturation,
                                algorithm_cfg.epsilon_model,
                                algorithm_cfg.wait_before_train,
                                algorithm_cfg.num_actions, agent_this_drone)

                            action_word = translate_action(
                                action, algorithm_cfg.num_actions)
                            # Take the action
                            agent[name_agent].take_action(
                                action,
                                algorithm_cfg.num_actions,
                                Mode='static')
                            # time.sleep(0.05)
                            new_state[name_agent] = agent[
                                name_agent].get_state()
                            new_depth1, thresh = agent[
                                name_agent].get_CustomDepth(cfg)

                            # Get GPS information
                            posit[name_agent] = client.simGetVehiclePose(
                                vehicle_name=name_agent)
                            position = posit[name_agent].position
                            old_p = np.array([
                                old_posit[name_agent].position.x_val,
                                old_posit[name_agent].position.y_val
                            ])
                            new_p = np.array([position.x_val, position.y_val])

                            # calculate distance
                            distance[name_agent] = distance[
                                name_agent] + np.linalg.norm(new_p - old_p)
                            old_posit[name_agent] = posit[name_agent]

                            reward, crash = agent[name_agent].reward_gen(
                                new_depth1, action, crash_threshold, thresh,
                                debug, cfg)

                            ret[name_agent] = ret[name_agent] + reward
                            agent_state = agent[name_agent].GetAgentState()

                            if agent_state.has_collided or distance[
                                    name_agent] < 0.1:
                                num_collisions[name_agent] = num_collisions[
                                    name_agent] + 1
                                print('crash')
                                crash = True
                                reward = -1
                            data_tuple = []
                            data_tuple.append([
                                current_state[name_agent], action,
                                new_state[name_agent], reward, crash
                            ])
                            # TODO: one replay memory global, target_agent, agent: DONE
                            err = get_errors(
                                data_tuple, choose, ReplayMemory_this_drone,
                                algorithm_cfg.input_size, agent_this_drone,
                                target_agent_this_drone, algorithm_cfg.gamma,
                                algorithm_cfg.Q_clip)
                            ReplayMemory_this_drone.add(err, data_tuple)

                            # Train if sufficient frames have been stored
                            if iter > algorithm_cfg.wait_before_train:
                                if iter % algorithm_cfg.train_interval == 0:
                                    # Train the RL network
                                    old_states, Qvals, actions, err, idx = minibatch_double(
                                        data_tuple, algorithm_cfg.batch_size,
                                        choose, ReplayMemory_this_drone,
                                        algorithm_cfg.input_size,
                                        agent_this_drone,
                                        target_agent_this_drone,
                                        algorithm_cfg.gamma,
                                        algorithm_cfg.Q_clip)
                                    # TODO global replay memory: DONE
                                    for i in range(algorithm_cfg.batch_size):
                                        ReplayMemory_this_drone.update(
                                            idx[i], err[i])

                                    if print_qval:
                                        print(Qvals)
                                    # TODO global agent, target_agent: DONE
                                    if choose:
                                        # Double-DQN
                                        target_agent_this_drone.network_model.train_n(
                                            old_states, Qvals, actions,
                                            algorithm_cfg.batch_size,
                                            algorithm_cfg.dropout_rate,
                                            algorithm_cfg.learning_rate,
                                            algorithm_cfg.epsilon, iter)
                                    else:
                                        agent_this_drone.network_model.train_n(
                                            old_states, Qvals, actions,
                                            algorithm_cfg.batch_size,
                                            algorithm_cfg.dropout_rate,
                                            algorithm_cfg.learning_rate,
                                            algorithm_cfg.epsilon, iter)

                            time_exec = time.time() - start_time
                            gpu_memory, gpu_utilization, sys_memory = get_SystemStats(
                                process, cfg.NVIDIA_GPU)

                            for i in range(0, len(gpu_memory)):
                                tag_mem = 'GPU' + str(i) + '-Memory-GB'
                                tag_util = 'GPU' + str(i) + 'Utilization-%'
                                agent[
                                    name_agent].network_model.log_to_tensorboard(
                                        tag=tag_mem,
                                        group='SystemStats',
                                        value=gpu_memory[i],
                                        index=iter)
                                agent[
                                    name_agent].network_model.log_to_tensorboard(
                                        tag=tag_util,
                                        group='SystemStats',
                                        value=gpu_utilization[i],
                                        index=iter)
                            agent[name_agent].network_model.log_to_tensorboard(
                                tag='Memory-GB',
                                group='SystemStats',
                                value=sys_memory,
                                index=iter)

                            s_log = '{:<6s} - Level {:>2d} - Iter: {:>6d}/{:<5d} {:<8s}-{:>5s} Eps: {:<1.4f} lr: {:>1.8f} Ret = {:<+6.4f} Last Crash = {:<5d} t={:<1.3f} SF = {:<5.4f}  Reward: {:<+1.4f}  '.format(
                                name_agent, int(level[name_agent]), iter,
                                episode[name_agent], action_word, action_type,
                                algorithm_cfg.epsilon,
                                algorithm_cfg.learning_rate, ret[name_agent],
                                last_crash[name_agent], time_exec,
                                distance[name_agent], reward)

                            if iter % print_interval == 0:
                                print(s_log)
                            log_files[name_agent].write(s_log + '\n')

                            last_crash[name_agent] = last_crash[name_agent] + 1
                            if debug:
                                cv2.imshow(
                                    name_agent,
                                    np.hstack(
                                        (np.squeeze(current_state[name_agent],
                                                    axis=0),
                                         np.squeeze(new_state[name_agent],
                                                    axis=0))))
                                cv2.waitKey(1)

                            if crash:
                                if distance[name_agent] < 0.01:
                                    # Drone won't move, reconnect
                                    print(
                                        'Recovering from drone mobility issue')

                                    agent[
                                        name_agent].client, old_posit, initZ = connect_drone(
                                            ip_address=cfg.ip_address,
                                            phase=cfg.mode,
                                            num_agents=cfg.num_agents,
                                            client=client)
                                else:

                                    agent[
                                        name_agent].network_model.log_to_tensorboard(
                                            tag='Return',
                                            group=name_agent,
                                            value=ret[name_agent],
                                            index=episode[name_agent])
                                    agent[
                                        name_agent].network_model.log_to_tensorboard(
                                            tag='Safe Flight',
                                            group=name_agent,
                                            value=distance[name_agent],
                                            index=episode[name_agent])

                                    ret[name_agent] = 0
                                    distance[name_agent] = 0
                                    episode[
                                        name_agent] = episode[name_agent] + 1
                                    last_crash[name_agent] = 0

                                    reset_to_initial(level[name_agent],
                                                     reset_array,
                                                     client,
                                                     vehicle_name=name_agent)
                                    # time.sleep(0.2)
                                    current_state[name_agent] = agent[
                                        name_agent].get_state()
                                    old_posit[
                                        name_agent] = client.simGetVehiclePose(
                                            vehicle_name=name_agent)
                            else:
                                current_state[name_agent] = new_state[
                                    name_agent]

                            if iter % algorithm_cfg.max_iters == 0:
                                automate = False

                    # TODO define and state agents
                    if iter % algorithm_cfg.update_target_interval == 0 and iter > algorithm_cfg.wait_before_train:

                        if algorithm_cfg.distributed_algo == 'GlobalLearningGlobalUpdate-MA':
                            print('global' + ' - Switching Target Network')
                            global_agent.network_model.save_network(
                                algorithm_cfg.network_path,
                                episode[name_agent])
                        else:
                            for name_agent in name_agent_list:
                                agent[name_agent].take_action(
                                    [-1],
                                    algorithm_cfg.num_actions,
                                    Mode='static')
                                print(name_agent +
                                      ' - Switching Target Network')
                                agent[name_agent].network_model.save_network(
                                    algorithm_cfg.network_path,
                                    episode[name_agent])

                        choose = not choose

                    # if iter % algorithm_cfg.communication_interval == 0 and iter > algorithm_cfg.wait_before_train:
                    #     print('Communicating the weights and averaging them')
                    #     communicate_across_agents(agent, name_agent_list, algorithm_cfg)
                    #     communicate_across_agents(target_agent, name_agent_list, algorithm_cfg)

                    iter += 1

                elif cfg.mode == 'infer':
                    # Inference phase
                    agent_state = agent[name_agent].GetAgentState()
                    if agent_state.has_collided:
                        print('Drone collided')
                        print("Total distance traveled: ",
                              np.round(distance[name_agent], 2))
                        active = False
                        client.moveByVelocityAsync(
                            vx=0,
                            vy=0,
                            vz=0,
                            duration=1,
                            vehicle_name=name_agent).join()

                        if nav_x:  # Nav_x is empty if the drone collides in first iteration
                            ax_nav.plot(nav_x.pop(),
                                        nav_y.pop(),
                                        'r*',
                                        linewidth=20)
                        file_path = env_folder + 'results/'
                        fig_z.savefig(file_path + 'altitude_variation.png',
                                      dpi=500)
                        fig_nav.savefig(file_path + 'navigation.png', dpi=500)
                        close_env(env_process)
                        print('Figures saved')
                    else:
                        posit[name_agent] = client.simGetVehiclePose(
                            vehicle_name=name_agent)
                        distance[name_agent] = distance[
                            name_agent] + np.linalg.norm(
                                np.array([
                                    old_posit[name_agent].position.x_val -
                                    posit[name_agent].position.x_val,
                                    old_posit[name_agent].position.y_val -
                                    posit[name_agent].position.y_val
                                ]))
                        # altitude[name_agent].append(-posit[name_agent].position.z_val+p_z)
                        altitude[name_agent].append(
                            -posit[name_agent].position.z_val - f_z)

                        quat = (posit[name_agent].orientation.w_val,
                                posit[name_agent].orientation.x_val,
                                posit[name_agent].orientation.y_val,
                                posit[name_agent].orientation.z_val)
                        yaw = euler_from_quaternion(quat)[2]

                        x_val = posit[name_agent].position.x_val
                        y_val = posit[name_agent].position.y_val
                        z_val = posit[name_agent].position.z_val

                        nav_x.append(env_cfg.alpha * x_val + env_cfg.o_x)
                        nav_y.append(env_cfg.alpha * y_val + env_cfg.o_y)
                        nav.set_data(nav_x, nav_y)
                        nav_text.remove()
                        nav_text = ax_nav.text(
                            25,
                            55,
                            'Distance: ' +
                            str(np.round(distance[name_agent], 2)),
                            style='italic',
                            bbox={
                                'facecolor': 'white',
                                'alpha': 0.5
                            })

                        line_z.set_data(np.arange(len(altitude[name_agent])),
                                        altitude[name_agent])
                        ax_z.set_xlim(0, len(altitude[name_agent]))
                        fig_z.canvas.draw()
                        fig_z.canvas.flush_events()

                        current_state[name_agent] = agent[
                            name_agent].get_state()
                        action, action_type, algorithm_cfg.epsilon, qvals = policy(
                            1, current_state[name_agent], iter,
                            algorithm_cfg.epsilon_saturation, 'inference',
                            algorithm_cfg.wait_before_train,
                            algorithm_cfg.num_actions, agent[name_agent])
                        action_word = translate_action(
                            action, algorithm_cfg.num_actions)
                        # Take continuous action
                        agent[name_agent].take_action(
                            action, algorithm_cfg.num_actions, Mode='static')
                        old_posit[name_agent] = posit[name_agent]

                        # Verbose and log making
                        s_log = 'Position = ({:<3.2f},{:<3.2f}, {:<3.2f}) Orientation={:<1.3f} Predicted Action: {:<8s}  '.format(
                            x_val, y_val, z_val, yaw, action_word)

                        print(s_log)
                        log_files[name_agent].write(s_log + '\n')

        except Exception as e:
            if str(e) == 'cannot reshape array of size 1 into shape (0,0,3)':
                print('Recovering from AirSim error')
                client, old_posit, initZ = connect_drone(
                    ip_address=cfg.ip_address,
                    phase=cfg.mode,
                    num_agents=cfg.num_agents)

                agent[name_agent].client = client
            else:
                print('------------- Error -------------')
                exc_type, exc_obj, exc_tb = sys.exc_info()
                fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
                print(exc_type, fname, exc_tb.tb_lineno)
                print(exc_obj)
                automate = False
                print('Hit r and then backspace to start from this point')
def DeepQLearning(cfg):

    f_x_val = 5.442914962768555
    f_y_val = 1.8743505477905273

    if cfg.mode == 'move_around':
        env_process, env_folder = start_environment(env_name=cfg.env_name)
    else:
        algorithm_cfg = read_cfg(config_filename='configs/DeepQLearning.cfg',
                                 verbose=True)

        if algorithm_cfg.distributed_algo == 'GlobalLearningGlobalUpdate' or cfg.mode == 'infer':
            cfg.num_agents = 1

        # Start the environment
        env_process, env_folder = start_environment(env_name=cfg.env_name)
        # Connect to Unreal Engine and get the drone handle: client
        client, old_posit, initZ = connect_drone(ip_address=cfg.ip_address,
                                                 phase=cfg.mode,
                                                 num_agents=cfg.num_agents)
        initial_pos = old_posit.copy()
        # Load the initial positions for the environment
        reset_array, reset_array_raw, level_name, crash_threshold = initial_positions(
            cfg.env_name, initZ, cfg.num_agents)
        # Get memory handler
        process = psutil.Process(getpid())
        # Load PyGame Screen
        screen = pygame_connect(phase=cfg.mode)

        fig_z = []
        fig_nav = []
        debug = False

        # Generate path where the weights will be saved
        cfg, algorithm_cfg = save_network_path(cfg=cfg,
                                               algorithm_cfg=algorithm_cfg)
        current_state = {}
        new_state = {}
        posit = {}
        name_agent_list = []
        agent = {}
        # Replay Memory for RL
        if cfg.mode == 'train':
            ReplayMemory = {}
            target_agent = {}
            for drone in range(cfg.num_agents):
                name_agent = "drone" + str(drone)
                name_agent_list.append(name_agent)
                print_orderly(name_agent, 40)
                ReplayMemory[name_agent] = Memory(algorithm_cfg.buffer_len)
                agent[name_agent] = PedraAgent(algorithm_cfg,
                                               client,
                                               name='DQN',
                                               vehicle_name=name_agent)
                target_agent[name_agent] = PedraAgent(algorithm_cfg,
                                                      client,
                                                      name='Target',
                                                      vehicle_name=name_agent)
                current_state[name_agent] = agent[name_agent].get_state()

        elif cfg.mode == 'infer':
            name_agent = 'drone0'
            name_agent_list.append(name_agent)
            agent[name_agent] = PedraAgent(algorithm_cfg,
                                           client,
                                           name=name_agent + 'DQN',
                                           vehicle_name=name_agent)

            env_cfg = read_cfg(config_filename=env_folder + 'config.cfg')
            nav_x = []
            nav_y = []
            altitude = {}
            altitude[name_agent] = []
            p_z, f_z, fig_z, ax_z, line_z, fig_nav, ax_nav, nav = initialize_infer(
                env_cfg=env_cfg, client=client, env_folder=env_folder)
            nav_text = ax_nav.text(0, 0, '')

        # Initialize variables
        iter = 1
        # num_collisions = 0
        episode = {}
        active = True

        print_interval = 1
        automate = True
        choose = False
        print_qval = False
        last_crash = {}
        ret = {}
        num_collisions = {}
        num_arrive = {}
        level = {}
        level_state = {}
        level_posit = {}
        times_switch = {}
        last_crash_array = {}
        ret_array = {}
        distance_array = {}
        epi_env_array = {}
        log_files = {}

        # If the phase is inference force the num_agents to 1
        hyphens = '-' * int((80 - len('Log files')) / 2)
        print(hyphens + ' ' + 'Log files' + ' ' + hyphens)
        for name_agent in name_agent_list:
            ret[name_agent] = 0
            num_collisions[name_agent] = 0
            last_crash[name_agent] = 0
            level[name_agent] = 0
            episode[name_agent] = 0
            level_state[name_agent] = [None] * len(reset_array[name_agent])
            level_posit[name_agent] = [None] * len(reset_array[name_agent])
            times_switch[name_agent] = 0
            last_crash_array[name_agent] = np.zeros(shape=len(
                reset_array[name_agent]),
                                                    dtype=np.int32)
            ret_array[name_agent] = np.zeros(
                shape=len(reset_array[name_agent]))
            distance_array[name_agent] = np.zeros(
                shape=len(reset_array[name_agent]))
            epi_env_array[name_agent] = np.zeros(shape=len(
                reset_array[name_agent]),
                                                 dtype=np.int32)

            # Log file
            log_path = algorithm_cfg.network_path + '/' + name_agent + '/' + cfg.mode + 'log.txt'
            print("Log path: ", log_path)
            log_files[name_agent] = open(log_path, 'w')

        distance = 0
        switch_env = False

        print_orderly('Simulation begins', 80)

        # save_posit = old_posit

        # last_crash_array = np.zeros(shape=len(level_name), dtype=np.int32)
        # ret_array = np.zeros(shape=len(level_name))
        # distance_array = np.zeros(shape=len(level_name))
        # epi_env_array = np.zeros(shape=len(level_name), dtype=np.int32)

        while active:
            try:
                active, automate, algorithm_cfg, client = check_user_input(
                    active, automate, agent[name_agent], client,
                    old_posit[name_agent], initZ, fig_z, fig_nav, env_folder,
                    cfg, algorithm_cfg)

                if automate:

                    if cfg.mode == 'train':

                        if iter % algorithm_cfg.switch_env_steps == 0:
                            switch_env = True
                        else:
                            switch_env = False

                        for name_agent in name_agent_list:

                            start_time = time.time()
                            if switch_env:
                                posit1_old = client.simGetVehiclePose(
                                    vehicle_name=name_agent)
                                times_switch[
                                    name_agent] = times_switch[name_agent] + 1
                                level_state[name_agent][level[
                                    name_agent]] = current_state[name_agent]
                                level_posit[name_agent][
                                    level[name_agent]] = posit1_old
                                last_crash_array[name_agent][
                                    level[name_agent]] = last_crash[name_agent]
                                ret_array[name_agent][
                                    level[name_agent]] = ret[name_agent]
                                distance_array[name_agent][
                                    level[name_agent]] = distance
                                epi_env_array[name_agent][
                                    level[name_agent]] = episode[name_agent]

                                level[name_agent] = (
                                    level[name_agent] + 1) % len(
                                        reset_array[name_agent])

                                print(
                                    name_agent + ' :Transferring to level: ',
                                    level[name_agent], ' - ',
                                    level_name[name_agent][level[name_agent]])

                                if times_switch[name_agent] < len(
                                        reset_array[name_agent]):
                                    reset_to_initial(level[name_agent],
                                                     reset_array,
                                                     client,
                                                     vehicle_name=name_agent)
                                else:
                                    current_state[name_agent] = level_state[
                                        name_agent][level[name_agent]]
                                    posit1_old = level_posit[name_agent][
                                        level[name_agent]]
                                    reset_to_initial(level[name_agent],
                                                     reset_array,
                                                     client,
                                                     vehicle_name=name_agent)
                                    client.simSetVehiclePose(
                                        posit1_old,
                                        ignore_collison=True,
                                        vehicle_name=name_agent)
                                    time.sleep(0.1)

                                last_crash[name_agent] = last_crash_array[
                                    name_agent][level[name_agent]]
                                ret[name_agent] = ret_array[name_agent][
                                    level[name_agent]]
                                distance = distance_array[name_agent][
                                    level[name_agent]]
                                episode[name_agent] = epi_env_array[
                                    name_agent][int(level[name_agent] / 3)]
                                # environ = environ^True
                            else:

                                action, action_type, algorithm_cfg.epsilon, qvals = policy(
                                    algorithm_cfg.epsilon,
                                    current_state[name_agent], iter,
                                    algorithm_cfg.epsilon_saturation,
                                    algorithm_cfg.epsilon_model,
                                    algorithm_cfg.wait_before_train,
                                    algorithm_cfg.num_actions,
                                    agent[name_agent])

                                action_word = translate_action(
                                    action, algorithm_cfg.num_actions)
                                # Take the action
                                agent[name_agent].take_action(
                                    action,
                                    algorithm_cfg.num_actions,
                                    SimMode=cfg.SimMode)
                                # time.sleep(0.05)
                                new_state[name_agent] = agent[
                                    name_agent].get_state()
                                new_depth1, thresh = agent[
                                    name_agent].get_depth(cfg)

                                # Get GPS information
                                posit[name_agent] = client.simGetVehiclePose(
                                    vehicle_name=name_agent)
                                position = posit[name_agent].position
                                old_p = np.array([
                                    old_posit[name_agent].position.x_val,
                                    old_posit[name_agent].position.y_val
                                ])
                                new_p = np.array(
                                    [position.x_val, position.y_val])

                                # calculate distance
                                distance = distance + np.linalg.norm(new_p -
                                                                     old_p)
                                old_posit[name_agent] = posit[name_agent]

                                #reward, crash = agent[name_agent].reward_gen(new_depth1, action, crash_threshold, thresh, debug, cfg)
                                #reward2, arrive = agent[name_agent].reward_gen2(start_time)
                                reward, crash, arrive = agent[
                                    name_agent].reward_gen(
                                        new_depth1, action, crash_threshold,
                                        thresh, debug,
                                        posit[name_agent].position.x_val,
                                        posit[name_agent].position.y_val,
                                        f_x_val, f_y_val, start_time, cfg)
                                ret[name_agent] = ret[name_agent] + reward
                                #ret[name_agent] = ret[name_agent] + reward2
                                agent_state = agent[name_agent].GetAgentState()
                                if agent_state.has_collided:
                                    num_collisions[
                                        name_agent] = num_collisions[
                                            name_agent] + 1
                                    print('crash')
                                    crash = True
                                    reward = -1000

                                #if (posit[name_agent].position.x_val == f_x_val) and (posit[name_agent].position.y_val == f_y_val):
                                #num_arrive[name_agent] = num_arrive[name_agent] + 1
                                #print("Arrived")
                                #arrive = True
                                #reward = 1000000

                                data_tuple = []
                                data_tuple.append([
                                    current_state[name_agent], action,
                                    new_state[name_agent], reward, crash
                                ])
                                err = get_errors(data_tuple, choose,
                                                 ReplayMemory[name_agent],
                                                 algorithm_cfg.input_size,
                                                 agent[name_agent],
                                                 target_agent[name_agent],
                                                 algorithm_cfg.gamma,
                                                 algorithm_cfg.Q_clip)
                                ReplayMemory[name_agent].add(err, data_tuple)
                                # Train if sufficient frames have been stored
                                if iter > algorithm_cfg.wait_before_train:
                                    if iter % algorithm_cfg.train_interval == 0:
                                        # Train the RL network
                                        old_states, Qvals, actions, err, idx = minibatch_double(
                                            data_tuple,
                                            algorithm_cfg.batch_size, choose,
                                            ReplayMemory[name_agent],
                                            algorithm_cfg.input_size,
                                            agent[name_agent],
                                            target_agent[name_agent],
                                            algorithm_cfg.gamma,
                                            algorithm_cfg.Q_clip)

                                        for i in range(
                                                algorithm_cfg.batch_size):
                                            ReplayMemory[name_agent].update(
                                                idx[i], err[i])

                                        if print_qval:
                                            print(Qvals)

                                        if choose:
                                            # Double-DQN
                                            target_agent[name_agent].train_n(
                                                old_states, Qvals, actions,
                                                algorithm_cfg.batch_size,
                                                algorithm_cfg.dropout_rate,
                                                algorithm_cfg.learning_rate,
                                                algorithm_cfg.epsilon, iter)
                                        else:
                                            agent[name_agent].train_n(
                                                old_states, Qvals, actions,
                                                algorithm_cfg.batch_size,
                                                algorithm_cfg.dropout_rate,
                                                algorithm_cfg.learning_rate,
                                                algorithm_cfg.epsilon, iter)

                                # iter += 1

                                time_exec = time.time() - start_time

                                mem_percent = process.memory_info()[0] / 2.**30

                                s_log = '{:<6s} - Level {:>2d} - Iter: {:>6d}/{:<5d} {:<8s}-{:>5s} Eps: {:<1.4f} lr: {:>1.8f} Ret = {:<+6.4f} Last Crash = {:<5d} t={:<1.3f} Mem = {:<5.4f}  Reward: {:<+1.4f}  '.format(
                                    name_agent, int(level[name_agent]), iter,
                                    episode[name_agent], action_word,
                                    action_type, algorithm_cfg.epsilon,
                                    algorithm_cfg.learning_rate,
                                    ret[name_agent], last_crash[name_agent],
                                    time_exec, mem_percent, reward)

                                if iter % print_interval == 0:
                                    print(s_log)
                                log_files[name_agent].write(s_log + '\n')

                                last_crash[
                                    name_agent] = last_crash[name_agent] + 1
                                if debug:
                                    cv2.imshow(
                                        name_agent,
                                        np.hstack(
                                            (np.squeeze(
                                                current_state[name_agent],
                                                axis=0),
                                             np.squeeze(new_state[name_agent],
                                                        axis=0))))
                                    cv2.waitKey(1)

                                if crash:
                                    agent[name_agent].return_plot(
                                        ret[name_agent], episode[name_agent],
                                        int(level[name_agent] / 3),
                                        mem_percent, iter, distance)
                                    ret[name_agent] = 0
                                    distance = 0
                                    episode[
                                        name_agent] = episode[name_agent] + 1
                                    last_crash[name_agent] = 0

                                    reset_to_initial(level[name_agent],
                                                     reset_array,
                                                     client,
                                                     vehicle_name=name_agent)
                                    # time.sleep(0.2)
                                    current_state[name_agent] = agent[
                                        name_agent].get_state()
                                else:
                                    #current_state[name_agent] = new_state[name_agent]
                                    if arrive:
                                        episode[name_agent] = episode[
                                            name_agent] + 1
                                        ret[name_agent] = 0
                                        distance = 0
                                        last_crash[name_agent] = 0
                                        reset_to_initial(
                                            level[name_agent],
                                            reset_array,
                                            client,
                                            vehicle_name=name_agent)
                                        # time.sleep(0.2)
                                        current_state[name_agent] = agent[
                                            name_agent].get_state()
                                    else:
                                        current_state[name_agent] = new_state[
                                            name_agent]

                                if iter % algorithm_cfg.max_iters == 0:
                                    automate = False

                                # if iter >140:
                                #     active=False
                        if iter % algorithm_cfg.communication_interval == 0 and iter > algorithm_cfg.wait_before_train:
                            print(
                                'Communicating the weights and averaging them')
                            communicate_across_agents(agent, name_agent_list,
                                                      algorithm_cfg)
                            communicate_across_agents(target_agent,
                                                      name_agent_list,
                                                      algorithm_cfg)

                        if iter % algorithm_cfg.update_target_interval == 0 and iter > algorithm_cfg.wait_before_train:
                            for name_agent in name_agent_list:
                                agent[name_agent].take_action(
                                    [-1],
                                    algorithm_cfg.num_actions,
                                    SimMode=cfg.SimMode)
                                print(name_agent +
                                      ' - Switching Target Network')
                                agent[name_agent].save_network(
                                    algorithm_cfg.network_path)

                            choose = not choose

                        iter += 1

                    elif cfg.mode == 'infer':
                        # Inference phase
                        agent_state = agent[name_agent].GetAgentState()
                        if agent_state.has_collided:
                            print('Drone collided')
                            print("Total distance traveled: ",
                                  np.round(distance, 2))
                            active = False
                            client.moveByVelocityAsync(
                                vx=0,
                                vy=0,
                                vz=0,
                                duration=1,
                                vehicle_name=name_agent).join()

                            if nav_x:  # Nav_x is empty if the drone collides in first iteration
                                ax_nav.plot(nav_x.pop(),
                                            nav_y.pop(),
                                            'r*',
                                            linewidth=20)
                            file_path = env_folder + 'results/'
                            fig_z.savefig(file_path + 'altitude_variation.png',
                                          dpi=500)
                            fig_nav.savefig(file_path + 'navigation.png',
                                            dpi=500)
                            close_env(env_process)
                            print('Figures saved')
                        else:
                            posit[name_agent] = client.simGetVehiclePose(
                                vehicle_name=name_agent)
                            distance = distance + np.linalg.norm(
                                np.array([
                                    old_posit[name_agent].position.x_val -
                                    posit[name_agent].position.x_val,
                                    old_posit[name_agent].position.y_val -
                                    posit[name_agent].position.y_val
                                ]))
                            # altitude[name_agent].append(-posit[name_agent].position.z_val+p_z)
                            altitude[name_agent].append(
                                -posit[name_agent].position.z_val - f_z)

                            quat = (posit[name_agent].orientation.w_val,
                                    posit[name_agent].orientation.x_val,
                                    posit[name_agent].orientation.y_val,
                                    posit[name_agent].orientation.z_val)
                            yaw = euler_from_quaternion(quat)[2]

                            x_val = posit[name_agent].position.x_val
                            y_val = posit[name_agent].position.y_val
                            z_val = posit[name_agent].position.z_val

                            nav_x.append(env_cfg.alpha * x_val + env_cfg.o_x)
                            nav_y.append(env_cfg.alpha * y_val + env_cfg.o_y)
                            nav.set_data(nav_x, nav_y)
                            nav_text.remove()
                            nav_text = ax_nav.text(25,
                                                   55,
                                                   'Distance: ' +
                                                   str(np.round(distance, 2)),
                                                   style='italic',
                                                   bbox={
                                                       'facecolor': 'white',
                                                       'alpha': 0.5
                                                   })

                            line_z.set_data(
                                np.arange(len(altitude[name_agent])),
                                altitude[name_agent])
                            ax_z.set_xlim(0, len(altitude[name_agent]))
                            fig_z.canvas.draw()
                            fig_z.canvas.flush_events()

                            current_state[name_agent] = agent[
                                name_agent].get_state()
                            action, action_type, algorithm_cfg.epsilon, qvals = policy(
                                1, current_state[name_agent], iter,
                                algorithm_cfg.epsilon_saturation, 'inference',
                                algorithm_cfg.wait_before_train,
                                algorithm_cfg.num_actions, agent[name_agent])
                            action_word = translate_action(
                                action, algorithm_cfg.num_actions)
                            # Take continuous action
                            agent[name_agent].take_action(
                                action,
                                algorithm_cfg.num_actions,
                                SimMode=cfg.SimMode)
                            old_posit[name_agent] = posit[name_agent]

                            # Verbose and log making
                            s_log = 'Position = ({:<3.2f},{:<3.2f}, {:<3.2f}) Orientation={:<1.3f} Predicted Action: {:<8s}  '.format(
                                x_val, y_val, z_val, yaw, action_word)

                            print(s_log)
                            log_files[name_agent].write(s_log + '\n')

            except Exception as e:
                if str(
                        e
                ) == 'cannot reshape array of size 1 into shape (0,0,3)':
                    print('Recovering from AirSim error')
                    client, old_posit, initZ = connect_drone(
                        ip_address=cfg.ip_address,
                        phase=cfg.mode,
                        num_agents=cfg.num_agents)

                    agent[name_agent].client = client
                else:
                    print('------------- Error -------------')
                    exc_type, exc_obj, exc_tb = sys.exc_info()
                    fname = os.path.split(
                        exc_tb.tb_frame.f_code.co_filename)[1]
                    print(exc_type, fname, exc_tb.tb_lineno)
                    print(exc_obj)
                    automate = False
                    print('Hit r and then backspace to start from this point')
Beispiel #6
0
    def take_action(self, action, num_actions, Mode):
        # Mode
        # static: The drone moves by position. The position corresponding to the action
        # is calculated and the drone moves to the position and remains still
        # dynaic: The drone moves by velocity. The velocity corresponding to the action
        # is calculated and teh drone executes the same velocity command until next velocity
        # command is executed to overwrite it.

        # Set Paramaters
        fov_v = (45 * np.pi / 180) / 1.5
        fov_h = (80 * np.pi / 180) / 1.5
        r = 0.4

        ignore_collision = False
        sqrt_num_actions = np.sqrt(num_actions)

        posit = self.client.simGetVehiclePose(vehicle_name=self.vehicle_name)
        pos = posit.position
        orientation = posit.orientation

        quat = (orientation.w_val, orientation.x_val, orientation.y_val,
                orientation.z_val)
        eulers = euler_from_quaternion(quat)
        alpha = eulers[2]

        theta_ind = int(action[0] / sqrt_num_actions)
        psi_ind = action[0] % sqrt_num_actions

        theta = fov_v / sqrt_num_actions * (theta_ind -
                                            (sqrt_num_actions - 1) / 2)
        psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2)

        if Mode == 'static':
            noise_theta = (fov_v / sqrt_num_actions) / 6
            noise_psi = (fov_h / sqrt_num_actions) / 6

            psi = psi + random.uniform(-1, 1) * noise_psi
            theta = theta + random.uniform(-1, 1) * noise_theta

            x = pos.x_val + r * np.cos(alpha + psi)
            y = pos.y_val + r * np.sin(alpha + psi)
            z = pos.z_val + r * np.sin(
                theta)  # -ve because Unreal has -ve z direction going upwards

            self.client.simSetVehiclePose(airsim.Pose(
                airsim.Vector3r(x, y, z),
                airsim.to_quaternion(0, 0, alpha + psi)),
                                          ignore_collison=ignore_collision,
                                          vehicle_name=self.vehicle_name)
        elif Mode == 'dynamic':
            r_infer = 0.4
            vx = r_infer * np.cos(alpha + psi)
            vy = r_infer * np.sin(alpha + psi)
            vz = r_infer * np.sin(theta)
            # TODO
            # Take average of previous velocities and current to smoothen out drone movement.
            self.client.moveByVelocityAsync(
                vx=vx,
                vy=vy,
                vz=vz,
                duration=1,
                drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
                yaw_mode=airsim.YawMode(is_rate=False,
                                        yaw_or_rate=180 * (alpha + psi) /
                                        np.pi),
                vehicle_name=self.vehicle_name)
            time.sleep(0.07)
            self.client.moveByVelocityAsync(vx=0,
                                            vy=0,
                                            vz=0,
                                            duration=1,
                                            vehicle_name=self.vehicle_name)
Beispiel #7
0
def DeepREINFORCE(cfg, env_process, env_folder):
    algorithm_cfg = read_cfg(config_filename='configs/DeepREINFORCE.cfg',
                             verbose=True)
    algorithm_cfg.algorithm = cfg.algorithm

    if 'GlobalLearningGlobalUpdate-SA' in algorithm_cfg.distributed_algo:
        # algorithm_cfg = update_algorithm_cfg(algorithm_cfg, cfg)
        cfg.num_agents = 1
    client = []
    # Connect to Unreal Engine and get the drone handle: client
    client, old_posit, initZ = connect_drone(ip_address=cfg.ip_address,
                                             phase=cfg.mode,
                                             num_agents=cfg.num_agents,
                                             client=client)
    initial_pos = old_posit.copy()
    # Load the initial positions for the environment
    reset_array, reset_array_raw, level_name, crash_threshold = initial_positions(
        cfg.env_name, initZ, cfg.num_agents)

    # Initialize System Handlers
    process = psutil.Process(getpid())

    # Load PyGame Screen
    screen = pygame_connect(phase=cfg.mode)

    fig_z = []
    fig_nav = []
    debug = False
    # Generate path where the weights will be saved
    cfg, algorithm_cfg = save_network_path(cfg=cfg,
                                           algorithm_cfg=algorithm_cfg)
    current_state = {}
    new_state = {}
    posit = {}
    name_agent_list = []
    data_tuple = {}
    agent = {}
    epi_num = {}
    if cfg.mode == 'train':
        iter = {}
        wait_for_others = {}
        if algorithm_cfg.distributed_algo == 'GlobalLearningGlobalUpdate-MA':
            print_orderly('global', 40)
            # Multiple agent acts as data collecter and one global learner
            global_agent = PedraAgent(algorithm_cfg,
                                      client,
                                      name='DQN',
                                      vehicle_name='global')

        for drone in range(cfg.num_agents):
            name_agent = "drone" + str(drone)
            wait_for_others[name_agent] = False
            iter[name_agent] = 1
            epi_num[name_agent] = 1
            data_tuple[name_agent] = []
            name_agent_list.append(name_agent)
            print_orderly(name_agent, 40)
            # TODO: turn the neural network off if global agent is present
            agent[name_agent] = PedraAgent(algorithm_cfg,
                                           client,
                                           name='DQN',
                                           vehicle_name=name_agent)

            current_state[name_agent] = agent[name_agent].get_state()

    elif cfg.mode == 'infer':
        iter = 1
        name_agent = 'drone0'
        name_agent_list.append(name_agent)
        agent[name_agent] = PedraAgent(algorithm_cfg,
                                       client,
                                       name=name_agent + 'DQN',
                                       vehicle_name=name_agent)

        env_cfg = read_cfg(config_filename=env_folder + 'config.cfg')
        nav_x = []
        nav_y = []
        altitude = {}
        altitude[name_agent] = []
        p_z, f_z, fig_z, ax_z, line_z, fig_nav, ax_nav, nav = initialize_infer(
            env_cfg=env_cfg, client=client, env_folder=env_folder)
        nav_text = ax_nav.text(0, 0, '')

        # Select initial position
        reset_to_initial(0, reset_array, client, vehicle_name=name_agent)
        old_posit[name_agent] = client.simGetVehiclePose(
            vehicle_name=name_agent)

    # Initialize variables

    # iter = 1
    # num_collisions = 0
    episode = {}
    active = True

    print_interval = 1
    automate = True
    choose = False
    print_qval = False
    last_crash = {}
    ret = {}
    distance = {}
    num_collisions = {}
    level = {}
    level_state = {}
    level_posit = {}
    times_switch = {}
    last_crash_array = {}
    ret_array = {}
    distance_array = {}
    epi_env_array = {}
    log_files = {}

    # If the phase is inference force the num_agents to 1
    hyphens = '-' * int((80 - len('Log files')) / 2)
    print(hyphens + ' ' + 'Log files' + ' ' + hyphens)
    for name_agent in name_agent_list:
        ret[name_agent] = 0
        num_collisions[name_agent] = 0
        last_crash[name_agent] = 0
        level[name_agent] = 0
        episode[name_agent] = 0
        level_state[name_agent] = [None] * len(reset_array[name_agent])
        level_posit[name_agent] = [None] * len(reset_array[name_agent])
        times_switch[name_agent] = 0
        last_crash_array[name_agent] = np.zeros(shape=len(
            reset_array[name_agent]),
                                                dtype=np.int32)
        ret_array[name_agent] = np.zeros(shape=len(reset_array[name_agent]))
        distance_array[name_agent] = np.zeros(
            shape=len(reset_array[name_agent]))
        epi_env_array[name_agent] = np.zeros(shape=len(
            reset_array[name_agent]),
                                             dtype=np.int32)
        distance[name_agent] = 0
        # Log file
        log_path = algorithm_cfg.network_path + '/' + name_agent + '/' + cfg.mode + 'log.txt'
        print("Log path: ", log_path)
        log_files[name_agent] = open(log_path, 'w')

    # switch_env = False

    print_orderly('Simulation begins', 80)

    # save_posit = old_posit

    # last_crash_array = np.zeros(shape=len(level_name), dtype=np.int32)
    # ret_array = np.zeros(shape=len(level_name))
    # distance_array = np.zeros(shape=len(level_name))
    # epi_env_array = np.zeros(shape=len(level_name), dtype=np.int32)

    while active:
        try:
            active, automate, algorithm_cfg, client = check_user_input(
                active, automate, agent[name_agent], client,
                old_posit[name_agent], initZ, fig_z, fig_nav, env_folder, cfg,
                algorithm_cfg)

            if automate:

                if cfg.mode == 'train':

                    if iter[name_agent] % algorithm_cfg.switch_env_steps == 0:
                        switch_env = True
                    else:
                        switch_env = False

                    for name_agent in name_agent_list:
                        while not wait_for_others[name_agent]:
                            start_time = time.time()
                            if switch_env:
                                posit1_old = client.simGetVehiclePose(
                                    vehicle_name=name_agent)
                                times_switch[
                                    name_agent] = times_switch[name_agent] + 1
                                level_state[name_agent][level[
                                    name_agent]] = current_state[name_agent]
                                level_posit[name_agent][
                                    level[name_agent]] = posit1_old
                                last_crash_array[name_agent][
                                    level[name_agent]] = last_crash[name_agent]
                                ret_array[name_agent][
                                    level[name_agent]] = ret[name_agent]
                                distance_array[name_agent][
                                    level[name_agent]] = distance[name_agent]
                                epi_env_array[name_agent][
                                    level[name_agent]] = episode[name_agent]

                                level[name_agent] = (
                                    level[name_agent] + 1) % len(
                                        reset_array[name_agent])

                                print(
                                    name_agent + ' :Transferring to level: ',
                                    level[name_agent], ' - ',
                                    level_name[name_agent][level[name_agent]])

                                if times_switch[name_agent] < len(
                                        reset_array[name_agent]):
                                    reset_to_initial(level[name_agent],
                                                     reset_array,
                                                     client,
                                                     vehicle_name=name_agent)
                                else:
                                    current_state[name_agent] = level_state[
                                        name_agent][level[name_agent]]
                                    posit1_old = level_posit[name_agent][
                                        level[name_agent]]
                                    reset_to_initial(level[name_agent],
                                                     reset_array,
                                                     client,
                                                     vehicle_name=name_agent)
                                    client.simSetVehiclePose(
                                        posit1_old,
                                        ignore_collison=True,
                                        vehicle_name=name_agent)
                                    time.sleep(0.1)

                                last_crash[name_agent] = last_crash_array[
                                    name_agent][level[name_agent]]
                                ret[name_agent] = ret_array[name_agent][
                                    level[name_agent]]
                                distance[name_agent] = distance_array[
                                    name_agent][level[name_agent]]
                                episode[name_agent] = epi_env_array[
                                    name_agent][int(level[name_agent] / 3)]
                                # environ = environ^True
                            else:
                                # TODO: policy from one global agent: DONE
                                if algorithm_cfg.distributed_algo == 'GlobalLearningGlobalUpdate-MA':
                                    agent_this_drone = global_agent
                                else:
                                    agent_this_drone = agent[name_agent]

                                action, action_type = policy_REINFORCE(
                                    current_state[name_agent],
                                    agent_this_drone)
                                # print('Evaluated Policy')
                                action_word = translate_action(
                                    action, algorithm_cfg.num_actions)
                                # Take the action
                                # agent[name_agent].take_action(action, algorithm_cfg.num_actions, SimMode=cfg.SimMode)
                                agent[name_agent].take_action(
                                    action,
                                    algorithm_cfg.num_actions,
                                    Mode='static')
                                # print('Took Action')
                                new_state[name_agent] = agent[
                                    name_agent].get_state()
                                # print('got state')
                                new_depth1, thresh = agent[
                                    name_agent].get_CustomDepth(cfg)
                                # print('Got state')

                                # Get GPS information
                                posit[name_agent] = client.simGetVehiclePose(
                                    vehicle_name=name_agent)
                                position = posit[name_agent].position
                                old_p = np.array([
                                    old_posit[name_agent].position.x_val,
                                    old_posit[name_agent].position.y_val
                                ])
                                new_p = np.array(
                                    [position.x_val, position.y_val])

                                # calculate distance
                                distance[name_agent] = distance[
                                    name_agent] + np.linalg.norm(new_p - old_p)
                                old_posit[name_agent] = posit[name_agent]
                                # print('calculated distance')
                                reward, crash = agent[name_agent].reward_gen(
                                    new_depth1, action, crash_threshold,
                                    thresh, debug, cfg)

                                ret[name_agent] = ret[name_agent] + reward
                                agent_state = agent[name_agent].GetAgentState()
                                # print('generated reward')

                                if agent_state.has_collided or distance[
                                        name_agent] < 0.01:
                                    num_collisions[
                                        name_agent] = num_collisions[
                                            name_agent] + 1
                                    if agent_state.has_collided:
                                        print(
                                            'Crash: Collision detected from environment'
                                        )
                                    else:
                                        print(
                                            'Crash: Collision detected from distance'
                                        )
                                    crash = True
                                    reward = -1

                                data_tuple[name_agent].append([
                                    current_state[name_agent], action, reward,
                                    crash
                                ])

                                # Train if episode ends
                                if crash:
                                    # Don't start another episode unless all agents have ended their episodes
                                    wait_for_others[name_agent] = True
                                    if distance[name_agent] < 0.01:
                                        # Drone won't move, reconnect
                                        print(
                                            'Recovering from drone mobility issue'
                                        )

                                        agent[
                                            name_agent].client, old_posit, initZ = connect_drone(
                                                ip_address=cfg.ip_address,
                                                phase=cfg.mode,
                                                num_agents=cfg.num_agents,
                                                client=client)

                                        agent[
                                            name_agent].client, old_posit, initZ = connect_drone(
                                                ip_address=cfg.ip_address,
                                                phase=cfg.mode,
                                                num_agents=cfg.num_agents,
                                                client=client)
                                        time.sleep(2)
                                        wait_for_others[name_agent] = False

                                    else:
                                        agent[
                                            name_agent].network_model.log_to_tensorboard(
                                                tag='Return',
                                                group=name_agent,
                                                value=ret[name_agent],
                                                index=epi_num[name_agent])

                                        agent[
                                            name_agent].network_model.log_to_tensorboard(
                                                tag='Safe Flight',
                                                group=name_agent,
                                                value=distance[name_agent],
                                                index=epi_num[name_agent])

                                        agent[
                                            name_agent].network_model.log_to_tensorboard(
                                                tag='Episode Length',
                                                group=name_agent,
                                                value=len(
                                                    data_tuple[name_agent]),
                                                index=epi_num[name_agent])

                                        # Train episode
                                        train_REINFORCE(
                                            data_tuple[name_agent],
                                            algorithm_cfg.batch_size,
                                            agent_this_drone,
                                            algorithm_cfg.learning_rate,
                                            algorithm_cfg.input_size,
                                            algorithm_cfg.gamma,
                                            epi_num[name_agent])
                                        c = agent_this_drone.network_model.get_vars(
                                        )[15]
                                        agent_this_drone.network_model.log_to_tensorboard(
                                            tag='weight',
                                            group=name_agent,
                                            value=c[0],
                                            index=epi_num[name_agent])

                                        data_tuple[name_agent] = []
                                        epi_num[name_agent] += 1
                                        ret[name_agent] = 0
                                        distance[name_agent] = 0
                                        last_crash[name_agent] = 0

                                        # print('train done, now reset')
                                        reset_to_initial(
                                            level[name_agent],
                                            reset_array,
                                            client,
                                            vehicle_name=name_agent)
                                        # time.sleep(0.2)
                                        current_state[name_agent] = agent[
                                            name_agent].get_state()
                                        old_posit[
                                            name_agent] = client.simGetVehiclePose(
                                                vehicle_name=name_agent)

                                        if epi_num[name_agent] % 100 == 0:
                                            agent_this_drone.network_model.save_network(
                                                algorithm_cfg.network_path,
                                                epi_num[name_agent])

                                        # if all are waiting for others, then we can reset.
                                        if all(wait_for_others.values()):
                                            # Now we can communicate weights
                                            print(
                                                'Communicating the weights and averaging them'
                                            )
                                            communicate_across_agents(
                                                agent, name_agent_list,
                                                algorithm_cfg)
                                            for n in name_agent_list:
                                                wait_for_others[n] = False

                                else:
                                    current_state[name_agent] = new_state[
                                        name_agent]

                                time_exec = time.time() - start_time
                                gpu_memory, gpu_utilization, sys_memory = get_SystemStats(
                                    process, cfg.NVIDIA_GPU)

                                for i in range(0, len(gpu_memory)):
                                    tag_mem = 'GPU' + str(i) + '-Memory-GB'
                                    tag_util = 'GPU' + str(i) + 'Utilization-%'
                                    agent_this_drone.network_model.log_to_tensorboard(
                                        tag=tag_mem,
                                        group='SystemStats',
                                        value=gpu_memory[i],
                                        index=iter[name_agent])
                                    agent_this_drone.network_model.log_to_tensorboard(
                                        tag=tag_util,
                                        group='SystemStats',
                                        value=gpu_utilization[i],
                                        index=iter[name_agent])
                                agent_this_drone.network_model.log_to_tensorboard(
                                    tag='Memory-GB',
                                    group='SystemStats',
                                    value=sys_memory,
                                    index=iter[name_agent])

                                s_log = '{:<6s} - Level {:>2d} - Iter: {:>6d}/{:<5d} {:<8s}-{:>5s} lr: {:>1.8f} Ret = {:>+6.4f} Last Crash = {:<5d} t={:<1.3f} SF = {:<5.4f}  Reward: {:<+1.4f}  '.format(
                                    name_agent, int(level[name_agent]),
                                    iter[name_agent], epi_num[name_agent],
                                    action_word, action_type,
                                    algorithm_cfg.learning_rate,
                                    ret[name_agent], last_crash[name_agent],
                                    time_exec, distance[name_agent], reward)

                                if iter[name_agent] % print_interval == 0:
                                    print(s_log)
                                log_files[name_agent].write(s_log + '\n')

                                last_crash[
                                    name_agent] = last_crash[name_agent] + 1
                                if debug:
                                    cv2.imshow(
                                        name_agent,
                                        np.hstack(
                                            (np.squeeze(
                                                current_state[name_agent],
                                                axis=0),
                                             np.squeeze(new_state[name_agent],
                                                        axis=0))))
                                    cv2.waitKey(1)

                                if epi_num[
                                        name_agent] % algorithm_cfg.total_episodes == 0:
                                    print(automate)
                                    automate = False

                                iter[name_agent] += 1
                    # if iter % algorithm_cfg.communication_interval == 0:
                    #     print('Communicating the weights and averaging them')
                    #     communicate_across_agents(agent, name_agent_list, algorithm_cfg)

                    # iter += 1

                elif cfg.mode == 'infer':
                    # Inference phase
                    agent_state = agent[name_agent].GetAgentState()
                    if agent_state.has_collided:
                        print('Drone collided')
                        print("Total distance traveled: ",
                              np.round(distance[name_agent], 2))
                        active = False
                        client.moveByVelocityAsync(
                            vx=0,
                            vy=0,
                            vz=0,
                            duration=1,
                            vehicle_name=name_agent).join()

                        if nav_x:  # Nav_x is empty if the drone collides in first iteration
                            ax_nav.plot(nav_x.pop(),
                                        nav_y.pop(),
                                        'r*',
                                        linewidth=20)
                        file_path = env_folder + 'results/'
                        fig_z.savefig(file_path + 'altitude_variation.png',
                                      dpi=500)
                        fig_nav.savefig(file_path + 'navigation.png', dpi=500)
                        close_env(env_process)
                        print('Figures saved')
                    else:
                        posit[name_agent] = client.simGetVehiclePose(
                            vehicle_name=name_agent)
                        distance[name_agent] = distance[
                            name_agent] + np.linalg.norm(
                                np.array([
                                    old_posit[name_agent].position.x_val -
                                    posit[name_agent].position.x_val,
                                    old_posit[name_agent].position.y_val -
                                    posit[name_agent].position.y_val
                                ]))
                        # altitude[name_agent].append(-posit[name_agent].position.z_val+p_z)
                        altitude[name_agent].append(
                            -posit[name_agent].position.z_val - f_z)

                        quat = (posit[name_agent].orientation.w_val,
                                posit[name_agent].orientation.x_val,
                                posit[name_agent].orientation.y_val,
                                posit[name_agent].orientation.z_val)
                        yaw = euler_from_quaternion(quat)[2]

                        x_val = posit[name_agent].position.x_val
                        y_val = posit[name_agent].position.y_val
                        z_val = posit[name_agent].position.z_val

                        nav_x.append(env_cfg.alpha * x_val + env_cfg.o_x)
                        nav_y.append(env_cfg.alpha * y_val + env_cfg.o_y)
                        nav.set_data(nav_x, nav_y)
                        nav_text.remove()
                        nav_text = ax_nav.text(
                            25,
                            55,
                            'Distance: ' +
                            str(np.round(distance[name_agent], 2)),
                            style='italic',
                            bbox={
                                'facecolor': 'white',
                                'alpha': 0.5
                            })

                        line_z.set_data(np.arange(len(altitude[name_agent])),
                                        altitude[name_agent])
                        ax_z.set_xlim(0, len(altitude[name_agent]))
                        fig_z.canvas.draw()
                        fig_z.canvas.flush_events()

                        current_state[name_agent] = agent[
                            name_agent].get_state()
                        action, action_type = policy_REINFORCE(
                            current_state[name_agent], agent[name_agent])
                        action_word = translate_action(
                            action, algorithm_cfg.num_actions)
                        # Take continuous action
                        # agent[name_agent].take_action(action, algorithm_cfg.num_actions, Mode='static')

                        agent[name_agent].take_action(
                            action, algorithm_cfg.num_actions, Mode='static')
                        old_posit[name_agent] = posit[name_agent]

                        # Verbose and log making
                        s_log = 'Position = ({:<3.2f},{:<3.2f}, {:<3.2f}) Orientation={:<1.3f} Predicted Action: {:<8s}  '.format(
                            x_val, y_val, z_val, yaw, action_word)

                        print(s_log)
                        log_files[name_agent].write(s_log + '\n')

        except Exception as e:
            if str(e) == 'cannot reshape array of size 1 into shape (0,0,3)':
                print('Recovering from AirSim error')

                client, old_posit, initZ = connect_drone(
                    ip_address=cfg.ip_address,
                    phase=cfg.mode,
                    num_agents=cfg.num_agents,
                    client=client)
                time.sleep(2)
                agent[name_agent].client = client
                wait_for_others[name_agent] = False
            else:
                print('------------- Error -------------')
                exc_type, exc_obj, exc_tb = sys.exc_info()
                fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
                print(exc_type, fname, exc_tb.tb_lineno)
                print(exc_obj)
                automate = False
                print('Hit r and then backspace to start from this point')