示例#1
0
    def __init__(self, env, results_parent_dir):
        self.results_dir = results_parent_dir
        self.position_normaliser = DynamicNormalizer([-2.4, 2.4], [-1.0, 1.0])
        self.position_deriv_normaliser = DynamicNormalizer([-1.75, 1.75],
                                                           [-1.0, 1.0])
        self.angle_normaliser = DynamicNormalizer([-3.14, 3.14], [-1.0, 1.0])
        self.angle_deriv_normaliser = DynamicNormalizer([-0.02, 0.02],
                                                        [-1.0, 1.0])

        self.angle_dt_moving_window = SlidingWindow(5)
        self.last_150_episode_returns = SlidingWindow(150)

        self.thrusters = Thrusters()
        self.env = env
        self.environment_info = EnvironmentInfo()

        sub_pilot_position_controller_output = rospy.Subscriber(
            "/pilot/position_pid_output", FloatArray,
            self.positionControllerCallback)
        self.prev_action = 0.0
        self.pos_pid_output = np.zeros(6)

        self.baseline_response = optimal_control_response()
示例#2
0
    def __init__(self):
        args = sys.argv
        if "-r" in args:
            self.results_dir_name = args[args.index("-r") + 1]
        else:
            self.results_dir_name = "nessie_run"

        self.position_normaliser = DynamicNormalizer([-2.4, 2.4], [-1.0, 1.0])
        self.position_deriv_normaliser = DynamicNormalizer([-1.75, 1.75],
                                                           [-1.0, 1.0])
        self.angle_normaliser = DynamicNormalizer([-3.14, 3.14], [-1.0, 1.0])
        self.angle_deriv_normaliser = DynamicNormalizer([-0.02, 0.02],
                                                        [-1.0, 1.0])

        self.angle_dt_moving_window = SlidingWindow(5)
        self.last_150_episode_returns = SlidingWindow(150)

        self.thrusters = Thrusters()
        self.env = ROSBehaviourInterface()
        self.environment_info = EnvironmentInfo()

        self.ounoise = OUNoise()

        self.prev_action = 0.0
示例#3
0

if __name__ == '__main__':
    rospy.init_node("stober_cacla_nessie")
    rospy.set_param("~NumRuns", CONFIG["num_runs"])
    rospy.set_param("~NumEpisodes", CONFIG["num_episodes"])
    rospy.set_param("~NumSteps", CONFIG["max_num_steps"])
    args = sys.argv
    if "-r" in args:
        results_dir_name = args[args.index("-r") + 1]
    else:
        results_dir_name = "cartpole_run"

    # initialise some global variables/objects
    # global normalisers
    position_normaliser = DynamicNormalizer([-1.0, 1.0], [-1.0, 1.0])
    position_deriv_normaliser = DynamicNormalizer([-3.0, 3.0], [-1.0, 1.0])
    distance_normaliser = DynamicNormalizer([0.0, 25.0], [-1.0, 1.0])
    distance_reward_normaliser = DynamicNormalizer([0.0, 15.0], [0.0, 1.0])
    angle_normaliser = DynamicNormalizer([-3.14, 3.14], [-1.0, 1.0])
    angle_deriv_normaliser = DynamicNormalizer([-0.15, 0.15], [-1.0, 1.0])

    angle_between_vectors = AngleBetweenVectors()
    angle_dt_moving_window = SlidingWindow(5)

    thrusters = Thrusters()
    nessie = ROSBehaviourInterface()
    environment_info = EnvironmentInfo()

    # Loop number of runs
    for run in range(CONFIG["num_runs"]):
示例#4
0
#! /usr/bin/python

# Source modified for RL from: http://www.wildml.com/2015/09/implementing-a-neural-network-from-scratch/

import numpy as np
from math import exp
from copy import deepcopy
from utilities.variable_normalizer import DynamicNormalizer

scale = DynamicNormalizer(input_range=[0.0, 1.0], output_range=[-1.0, 1.0])
# Sigmoid activation function for hidden layer nuerons
def sigmoid(X):
    R = []
    for x in X[0]:
        R.append(scale.scale_value(1.0/(1.0+exp(-x))))
    return np.array(R)

class ANNApproximator(object):
    def __init__(self, num_input_dim, nn_hdim, hlayer_activation_func="tanh"):
        self.name = "scratch_ann"
        self.nn_input_dim = num_input_dim # input layer dimensionality
        self.nn_output_dim = 1 # output layer dimensionality

        # Gradient descent parameters (I picked these by hand)
        self.alpha = 0.01 # learning rate for gradient descent
        self.h = 0.001
        self.reg_lambda = 0.01 # regularization strength
        self.build_model(nn_hdim, hlayer_activation_func)
        self.last_input = None
        self.num_params = None
        self.params_minus_one = self.getParams()
    "alpha_decay": 0.0,  # was 0.00005
    "exploration_sigma": 0.1,
    "exploration_decay": 0.0005,
    "min_trace_value": 0.3
}

if __name__ == '__main__':
    args = sys.argv
    if "-r" in args:
        results_dir_name = args[args.index("-r") + 1]
    else:
        results_dir_name = "cartpole_run"

    # initialise some global variables/objects
    # global normalisers
    position_normaliser = DynamicNormalizer([-1.0, 1.0], [-1.0, 1.0])
    position_deriv_normaliser = DynamicNormalizer([-3.0, 3.0], [-1.0, 1.0])
    angle_normaliser = DynamicNormalizer([-0.2, 0.2], [-1.0, 1.0])
    angle_deriv_normaliser = DynamicNormalizer([-2.0, 2.0], [-1.0, 1.0])

    # Loop number of runs
    for run in range(CONFIG["num_runs"]):
        # Create logging directory and files
        results_dir = "/tmp/{0}{1}".format(results_dir_name, run)
        if not os.path.exists(results_dir):
            os.makedirs(results_dir)
        filename = os.path.basename(sys.argv[0])
        os.system("cp {0} {1}".format(filename, results_dir))
        os.system(
            "cp /home/gordon/software/simple-rl/srl/basis_functions/simple_basis_functions.py {0}"
            .format(results_dir))
        results_dir_name = args[args.index("-r") + 1]
    else:
        results_dir_name = "cartpole_run"

    rospy.init_node(results_dir_name.replace(".", ""))
    rospy.set_param("~NumRuns", CONFIG["num_runs"])
    rospy.set_param("~NumEpisodes", CONFIG["num_episodes"])
    rospy.set_param("~NumSteps", CONFIG["max_num_steps"])
    # sub = rospy.Subscriber("EndRun", Empty, manualEndRun)

    angle_between_vectors = AngleBetweenVectors()


    # initialise some global variables/objects
    # global normalisers
    position_normaliser = DynamicNormalizer([-2.4, 2.4], [-1.0, 1.0])
    position_deriv_normaliser = DynamicNormalizer([-3.0, 3.0], [-1.0, 1.0])
    angle_normaliser = DynamicNormalizer([-0.2, 0.2], [-1.0, 1.0])
    angle_deriv_normaliser = DynamicNormalizer([-2.0, 2.0], [-1.0, 1.0])

    while not rospy.is_shutdown():

        # Loop number of runs
        for run in range(CONFIG["num_runs"]):
            # Create logging directory and files
            results_dir = "/tmp/{0}{1}".format(results_dir_name, run)
            if not os.path.exists(results_dir):
                os.makedirs(results_dir)
            filename = os.path.basename(sys.argv[0])
            os.system("cp {0} {1}".format(filename, results_dir))
            os.system("cp /home/gordon/software/simple-rl/srl/basis_functions/simple_basis_functions.py {0}".format(results_dir))
    navigation = Nav()
    environmental_data = EnvironmentInfo()
    angle_between_vectors = AngleBetweenVectors()
    ros_behaviour_interface = ROSBehaviourInterface()

    # Set ROS spin rate
    rate = rospy.Rate(CONFIG["spin_rate"])
    rospy.on_shutdown(on_rospyShutdown)

    # Set Thruster Status
    thrusters = Thrusters()

    # initialise some global variables/objects
    # global normalisers
    yaw_velocity_normaliser = DynamicNormalizer([-1.0, 1.0], [0.0, 1.0])
    surge_velocity_normaliser = DynamicNormalizer([-0.6, 0.6], [0.0, 1.0])
    codependant_normaliser = DynamicNormalizer([-0.4, 0.4], [0.0, 1.0])
    distance_normaliser = DynamicNormalizer([0.0, 12.0], [0.0, 1.0])
    angle_to_goal_normaliser = DynamicNormalizer([0.0, 3.14], [0.0, 1.0])
    angle_dt_normaliser = DynamicNormalizer([-0.1, 0.1], [0.0, 1.0])
    angle_and_velocity_normaliser = DynamicNormalizer([-1.6, 1.6], [0.0, 1.0])
    angles_normaliser = DynamicNormalizer([0.0, 3.14], [0.0, 1.0])
    north_normaliser = DynamicNormalizer([0.0, 14.0], [0.0, 1.0])
    east_normaliser = DynamicNormalizer([-5.0, 5.0], [0.0, 1.0])
    reward_normaliser = DynamicNormalizer([-3.14, 0.0], [-1.0, -0.1])

    # Set the global basis functions and approximator objects
    # basis_functions = RBFBasisAction()

示例#8
0
class PilotPidProcess(object):
    def __init__(self, env, results_parent_dir):
        self.results_dir = results_parent_dir
        self.position_normaliser = DynamicNormalizer([-2.4, 2.4], [-1.0, 1.0])
        self.position_deriv_normaliser = DynamicNormalizer([-1.75, 1.75],
                                                           [-1.0, 1.0])
        self.angle_normaliser = DynamicNormalizer([-3.14, 3.14], [-1.0, 1.0])
        self.angle_deriv_normaliser = DynamicNormalizer([-0.02, 0.02],
                                                        [-1.0, 1.0])

        self.angle_dt_moving_window = SlidingWindow(5)
        self.last_150_episode_returns = SlidingWindow(150)

        self.thrusters = Thrusters()
        self.env = env
        self.environment_info = EnvironmentInfo()

        sub_pilot_position_controller_output = rospy.Subscriber(
            "/pilot/position_pid_output", FloatArray,
            self.positionControllerCallback)
        self.prev_action = 0.0
        self.pos_pid_output = np.zeros(6)

        self.baseline_response = optimal_control_response()

    def positionControllerCallback(self, msg):
        self.pos_pid_output = msg.values

    def update_state_t(self):
        raw_angle = deepcopy(self.environment_info.raw_angle_to_goal)
        # print("raw angle:")
        # raw_angle_dt = raw_angle - self.prev_angle_dt_t
        # print("raw angle dt: {0}".format(raw_angle_dt))
        self.state_t = {
            "angle": self.angle_normaliser.scale_value(raw_angle),
            "angle_deriv": self.prev_angle_dt_t
        }
        # print("state t: {0}".format(self.state_t))
        self.prev_angle_dt_t = deepcopy(raw_angle)

    def update_state_t_p1(self):
        raw_angle = deepcopy(self.environment_info.raw_angle_to_goal)
        angle_tp1 = self.angle_normaliser.scale_value(raw_angle)
        angle_t = self.state_t["angle"]

        abs_angle_tp1 = np.abs(angle_tp1)
        abs_angle_t = np.abs(angle_t)
        if abs_angle_tp1 > abs_angle_t:
            sign = -1
        else:
            sign = 1
        angle_change = sign * abs(abs_angle_tp1 - abs_angle_t)

        # print("angle t: {0}".format(abs_angle_t))
        # print("angle tp1: {0}".format(abs_angle_tp1))
        # print("angle change: {0}".format(angle_change))

        tmp_angle_change = sum(
            self.angle_dt_moving_window.getWindow(angle_change)) / 5.0
        self.state_t_plus_1 = {
            "angle":
            self.angle_normaliser.scale_value(raw_angle),
            "angle_deriv":
            self.angle_deriv_normaliser.scale_value(tmp_angle_change)
        }
        self.prev_angle_dt_t = self.angle_deriv_normaliser.scale_value(
            tmp_angle_change)

    def setPidGains(self, posP, posI, posD, velP, velI, velD):
        self.env.enable_pilot(False)
        rospy.set_param("/pilot/controller/pos_n/kp", float(posP))
        rospy.set_param("/pilot/controller/pos_n/ki", float(posI))
        rospy.set_param("/pilot/controller/pos_n/kd", float(posD))
        # rospy.set_param("/pilot/controller/vel_r/kp", float(velP))
        # rospy.set_param("/pilot/controller/vel_r/ki", float(velI))
        # rospy.set_param("/pilot/controller/vel_r/kd", float(velD))
        self.env.enable_pilot(True)
        self.env.enable_pilot(False)
        self.env.enable_pilot(True)

    def calculate_fitness(self, response):
        max_idx = response.shape[0]
        step_errors = []

        for idx in range(max_idx):
            r = response[idx, 1]
            b = self.baseline_response[idx, 1]
            step_error = 0.0
            if r > b:
                step_error = r - b
            else:
                step_error = b - r
            step_errors.append(step_error)

        fitness = sum(step_errors)  # / len(step_errors)
        return fitness

    def get_response(self, id, individual):

        # reset stuff for the run
        self.env.nav_reset()
        # Set usable gains for DoHover action to get to initial position again
        # position sim gains: { "kp": 0.35, "ki": 0.0, "kd": 0.0 }
        # velocity sim gains: { "kp": 35.0, "ki": 0.0, "kd": 0.0 }
        self.setPidGains(0.35, 0, 0, 35.0, 0, 0)
        rospy.sleep(1)
        self.env.reset(disable_behaviours=False)
        self.angle_dt_moving_window.reset()
        self.prev_angle_dt_t = 0.0
        self.prev_angle_dt_tp1 = 0.0

        # Set the gains to those of the individual/solution
        self.setPidGains(individual[0], individual[1], individual[2], 0, 0, 0)

        # create log file
        f_actions = open(
            "{0}{1}".format(self.results_dir, "/actions{0}.csv".format(id)),
            "w", 1)

        start_time = time.time()
        end_time = start_time + CONFIG["run_time"]

        first_step = True
        response = np.zeros([350, 2])
        timestep = 0
        while time.time() < end_time and not rospy.is_shutdown():

            # send pilot request
            self.env.pilotPublishPositionRequest([0, 0, 0, 0, 0, 0])

            # perform a 'step'
            self.update_state_t()
            rospy.sleep(0.1)
            self.update_state_t_p1()

            # log the current state information
            if first_step:
                first_step = False
                state_keys = list(self.state_t.keys())
                state_keys.append("baseline_angle")
                state_keys.append("action")
                label_logging_format = "#{" + "}\t{".join(
                    [str(state_keys.index(el)) for el in state_keys]) + "}\n"
                f_actions.write(label_logging_format.format(*state_keys))

            logging_list = list(self.state_t.values())
            logging_list.append(self.baseline_response[timestep, 1])
            logging_list.append(self.pos_pid_output[5])
            action_logging_format = "{" + "}\t{".join(
                [str(logging_list.index(el)) for el in logging_list]) + "}\n"
            response[timestep, :] = [timestep, logging_list[0]]
            timestep += 1
            f_actions.write(action_logging_format.format(*logging_list))
        return response
示例#9
0
class NessieRlSimulation(object):
    def __init__(self):
        args = sys.argv
        if "-r" in args:
            self.results_dir_name = args[args.index("-r") + 1]
        else:
            self.results_dir_name = "nessie_run"

        self.position_normaliser = DynamicNormalizer([-2.4, 2.4], [-1.0, 1.0])
        self.position_deriv_normaliser = DynamicNormalizer([-1.75, 1.75],
                                                           [-1.0, 1.0])
        self.angle_normaliser = DynamicNormalizer([-3.14, 3.14], [-1.0, 1.0])
        self.angle_deriv_normaliser = DynamicNormalizer([-0.02, 0.02],
                                                        [-1.0, 1.0])

        self.angle_dt_moving_window = SlidingWindow(5)
        self.last_150_episode_returns = SlidingWindow(150)

        self.thrusters = Thrusters()
        self.env = ROSBehaviourInterface()
        self.environment_info = EnvironmentInfo()

        self.ounoise = OUNoise()

        self.prev_action = 0.0

    def update_critic(self, reward, update):
        state_t_value = self.approx_critic.computeOutput(self.state_t.values())
        state_t_p1_value = self.approx_critic.computeOutput(
            self.state_t_plus_1.values())
        # print("state t: {0}".format(state_t_value))
        # print("state tp1: {0}".format(state_t_p1_value))

        if CONFIG["critic algorithm"] == "ann_trad":
            td_error = reward + (CONFIG["gamma"] *
                                 state_t_p1_value) - state_t_value
        elif CONFIG["critic algorithm"] == "ann_true":
            td_error = reward + (CONFIG["gamma"] * state_t_p1_value) - \
            self.approx_critic.computeOutputThetaMinusOne(self.state_t.values())
        prev_critic_weights = self.approx_critic.getParams()
        critic_gradient = self.approx_critic.calculateGradient(
            self.state_t.values())
        self.traces_policy.updateTrace(
            self.approx_policy.calculateGradient(self.state_t.values()), 1.0)

        if update:
            p = self.approx_critic.getParams()
            if CONFIG["critic algorithm"] == "ann_trad":
                self.traces_critic.updateTrace(critic_gradient,
                                               1.0)  # for standard TD(lambda)
                X, T = self.traces_critic.getTraces()
                for x, trace in zip(X, T):
                    # print("updating critic using gradient vector: {0}\t{1}".format(x, trace))
                    p += critic_config["alpha"] * td_error * (x * trace)
                # self.approx_critic.setParams(prev_critic_weights + CONFIG["critic_config"]["alpha"] * td_error * critic_gradient)
            elif CONFIG["critic algorithm"] == "ann_true":
                # For True TD(lambda)
                #print("UPDATING ANN CRITC with TRUE TD(lambda)")
                self.traces_critic.updateTrace(
                    critic_gradient)  # for True TD(lambda)
                part_1 = td_error * self.traces_critic.e
                part_2 = critic_config["alpha"] * \
                        np.dot((self.approx_critic.computeOutputThetaMinusOne(self.state_t.values()) - state_t_value), critic_gradient)
                p += part_1 + part_2

            self.approx_critic.setParams(p)
        return (td_error, critic_gradient, state_t_value, state_t_p1_value)

    def update_state_t(self):
        raw_angle = deepcopy(self.environment_info.raw_angle_to_goal)
        # print("raw angle:")
        # raw_angle_dt = raw_angle - self.prev_angle_dt_t
        # print("raw angle dt: {0}".format(raw_angle_dt))
        self.state_t = {
            "angle": self.angle_normaliser.scale_value(raw_angle),
            "angle_deriv": self.prev_angle_dt_t
        }
        self.prev_angle_dt_t = deepcopy(raw_angle)

    def update_state_t_p1(self):
        raw_angle = deepcopy(self.environment_info.raw_angle_to_goal)
        angle_tp1 = self.angle_normaliser.scale_value(raw_angle)
        angle_t = self.state_t["angle"]

        # if (abs(angle_t)) > 0.5:
        #     if angle_t > 0 and angle_tp1 < 0:
        #         angle_change = (1.0 - angle_t) + (-1.0 - angle_tp1)
        #     elif angle_t < 0 and angle_tp1 > 0:
        #         angle_change = (1.0 - angle_tp1) + (-1.0 - angle_t)
        #     else:
        #         angle_change = angle_tp1 - angle_t
        # else:
        abs_angle_tp1 = np.abs(angle_tp1)
        abs_angle_t = np.abs(angle_t)
        if abs_angle_tp1 > abs_angle_t:
            sign = -1
        else:
            sign = 1
        angle_change = sign * abs(abs_angle_tp1 - abs_angle_t)

        # print("angle t: {0}".format(abs_angle_t))
        # print("angle tp1: {0}".format(abs_angle_tp1))
        # print("angle change: {0}".format(angle_change))

        tmp_angle_change = sum(
            self.angle_dt_moving_window.getWindow(angle_change)) / 5.0
        self.state_t_plus_1 = {
            "angle":
            self.angle_normaliser.scale_value(raw_angle),
            "angle_deriv":
            self.angle_deriv_normaliser.scale_value(tmp_angle_change)
        }
        self.prev_angle_dt_t = self.angle_deriv_normaliser.scale_value(
            tmp_angle_change)

    def update_policy(self, td_error, exploration):
        UPDATE_CONDITION = False
        if CONFIG["actor update rule"] == "cacla":
            if td_error > 0.0:
                UPDATE_CONDITION = True
            else:
                UPDATE_CONDITION = False
        elif CONFIG["actor update rule"] == "td lambda":
            UPDATE_CONDITION = True

        if UPDATE_CONDITION:
            # get original values
            params = self.approx_policy.getParams()
            old_action = self.approx_policy.computeOutput(
                self.state_t.values())
            policy_gradient = self.approx_policy.calculateGradient()

            # now update
            if CONFIG["actor update rule"] == "cacla":
                # policy.setParams(params + actor_config["alpha"] * (policy_gradient * exploration))
                X, T = self.traces_policy.getTraces()
                p = self.approx_policy.getParams()
                #print("Number of traces: {0}".format(len(T)))
                for x, trace in zip(X, T):
                    # print("updating critic using gradient vector: {0}\t{1}".format(x, trace))
                    p += actor_config["alpha"] * (x * trace) * exploration
                self.approx_policy.setParams(p)
            else:
                self.approx_policy.setParams(params + actor_config["alpha"] *
                                             (policy_gradient * td_error))

    def run(self):
        # Loop number of runs
        if CONFIG["test_policy"]:
            runs = TEST_CONFIG["run_numbers"]
        else:
            runs = range(CONFIG["num_runs"])

        for run in runs:
            if CONFIG["test_policy"]:
                self.results_dir_name = "nessie_validate_{0}".format(
                    TEST_CONFIG["folder"])
                results_to_load_directory = "/tmp/{0}{1}".format(
                    TEST_CONFIG["folder"], run)
            # Create logging directory and files
            results_dir = "/home/gordon/data/tmp/{0}{1}".format(
                self.results_dir_name, run)
            if not os.path.exists(results_dir):
                os.makedirs(results_dir)
            filename = os.path.basename(sys.argv[0])
            os.system("cp {0} {1}".format(filename, results_dir))
            os.system(
                "cp /home/gordon/rosbuild_ws/ros_simple_rl/src/srl/environments/ros_behaviour_interface.py {0}"
                .format(results_dir))
            os.system(
                "cp /home/gordon/rosbuild_ws/ros_simple_rl/src/utilities/orstein_exploration.py {0}"
                .format(results_dir))

            if CONFIG["test_policy"]:
                os.system("cp {0}/Epi* {1}/LearningEpisodeReturn.fso".format(
                    results_to_load_directory, results_dir))
                os.system("cp {0}/basic* {1}/LearningMainScript.py".format(
                    results_to_load_directory, results_dir))

            f_returns = open(
                "{0}{1}".format(results_dir, "/EpisodeReturn.fso"), "w", 1)

            # policies and critics
            self.approx_critic = ANNApproximator(
                actor_config["num_input_dims"],
                actor_config["num_hidden_units"],
                hlayer_activation_func="tanh")
            if not CONFIG["generate_initial_weights"]:
                critic_init = "/home/gordon/data/tmp/critic_params_48h.npy"
                self.approx_critic.setParams(list(np.load(critic_init)))

            self.approx_policy = ANNApproximator(
                actor_config["num_input_dims"],
                actor_config["num_hidden_units"],
                hlayer_activation_func="tanh")
            if not CONFIG["generate_initial_weights"]:
                policy_init = "/home/gordon/data/tmp/initial_2dim_48h_policy_params.npy"
                self.approx_policy.setParams(list(np.load(policy_init)))

            # if CONFIG["test_policy"] is True:
            #    if not os.path.exists("/tmp/{0}".format(results_to_validate)):
            #        continue
            #self.approx_policy.setParams()
            prev_critic_gradient = np.zeros(
                self.approx_critic.getParams().shape)

            # Set up trace objects
            if CONFIG["critic algorithm"] == "ann_trad":
                self.traces_critic = Traces(CONFIG["lambda"],
                                            CONFIG["min_trace_value"])
            elif CONFIG["critic algorithm"] == "ann_true":
                self.traces_critic = TrueTraces(critic_config["alpha"],
                                                CONFIG["gamma"],
                                                CONFIG["lambda"])
            self.traces_policy = Traces(CONFIG["lambda"],
                                        CONFIG["min_trace_value"])

            exploration_sigma = CONFIG["exploration_sigma"]

            for episode_number in range(CONFIG["num_episodes"]):

                if CONFIG["test_policy"] and episode_number not in TEST_CONFIG[
                        "episode_numbers"]:
                    # don't do anything for the episode number if we are testing policies and
                    # this episodes policy is not in the list to test
                    continue

                reward_cum = 0.0
                reward_cum_greedy = 0.0

                if episode_number % CONFIG["log_actions"] == 0:
                    f_actions = open(
                        "{0}{1}".format(
                            results_dir,
                            "/actions{0}.csv".format(episode_number)), "w", 1)

                # If testing a learnt policy, load it
                if CONFIG["test_policy"]:
                    policy_to_load = "{0}/policy_params{1}.npy".format(
                        results_to_load_directory, episode_number)
                    critic_to_load = "{0}/critic_params{1}.npy".format(
                        results_to_load_directory, episode_number)

                    print("policy_to_load: {0}".format(policy_to_load))
                    self.approx_policy.setParams(list(np.load(policy_to_load)))
                    self.approx_critic.setParams(list(np.load(critic_to_load)))

                # reset everything for the next episode
                self.traces_critic.reset()
                self.traces_policy.reset()

                # self.env.nav_reset()
                self.env.reset()
                self.ounoise.reset()

                self.angle_dt_moving_window.reset()

                episode_ended = False
                episode_ended_learning = False

                # if episode_number > 5 and exploration_sigma > 0.1:
                exploration_sigma *= CONFIG["exploration_decay"]

                self.prev_angle_dt_t = 0.0
                self.prev_angle_dt_tp1 = 0.0

                if CONFIG["generate_initial_weights"]:
                    self.approx_policy = ANNApproximator(
                        actor_config["num_input_dims"],
                        actor_config["num_hidden_units"],
                        hlayer_activation_func="tanh")

                for step_number in range(CONFIG["max_num_steps"]):
                    # Update the state for timestep t
                    self.update_state_t()

                    action_t_deterministic = self.approx_policy.computeOutput(
                        self.state_t.values())

                    # if episode_number > 9:
                    #     control_rate = 0.5
                    # else:
                    control_rate = 3
                    if step_number % (control_rate * CONFIG["spin_rate"]) == 0:
                        # exploration = self.ounoise.get_action(action_t_deterministic)
                        # exploration = np.random.normal(0.0, exploration_sigma)
                        tmp_action = self.ounoise.get_action(
                            action_t_deterministic)[0]
                        exploration = tmp_action - action_t_deterministic

                        # exploration = self.ounoise.function(action_t_deterministic, 0, 0.2, 0.1)[0]
                    # else:
                    #    action_t = deepcopy(self.prev_action)

                    # self.prev_action = deepcopy(action_t)

                    if not CONFIG["generate_initial_weights"] and not CONFIG[
                            "test_policy"]:
                        action_t = np.clip(
                            action_t_deterministic + exploration, -10, 10)
                    else:
                        action_t = np.clip(action_t_deterministic, -10, 10)

                    # TODO - investigate what happens with the action!!!
                    self.env.performAction("gaussian_variance", action_t)

                    # TODO - time rather than rospy.sleep?!
                    time.sleep(1.0 / CONFIG["spin_rate"])

                    # Update the state for timestep t + 1, after action is performed
                    self.update_state_t_p1()

                    to_end = False
                    # if self.state_t["angle"] > 0.9:
                    #     reward = -10
                    #     to_end = True
                    # else:
                    reward = self.env.getReward(self.state_t_plus_1, action_t)

                    if not episode_ended_learning:
                        if not CONFIG["generate_initial_weights"]:
                            # ---- Critic Update ----
                            (td_error, critic_gradient, state_t_value,
                             state_tp1_value) = self.update_critic(
                                 reward, not CONFIG["test_policy"])

                            if episode_number % CONFIG["log_actions"] == 0:
                                if step_number == 0:
                                    state_keys = self.state_t.keys()
                                    state_keys.append("exploration")
                                    state_keys.append("reward")
                                    state_keys.append("tde")
                                    state_keys.append("st")
                                    state_keys.append("stp1")
                                    state_keys.append("explore_action")
                                    state_keys.append("action")
                                    label_logging_format = "#{" + "}\t{".join([
                                        str(state_keys.index(el))
                                        for el in state_keys
                                    ]) + "}\n"
                                    f_actions.write(
                                        label_logging_format.format(
                                            *state_keys))

                                logging_list = self.state_t.values()
                                logging_list.append(exploration)
                                logging_list.append(reward)
                                logging_list.append(td_error)
                                logging_list.append(state_t_value)
                                logging_list.append(state_tp1_value)
                                logging_list.append(action_t)
                                logging_list.append(action_t_deterministic)
                                action_logging_format = "{" + "}\t{".join([
                                    str(logging_list.index(el))
                                    for el in logging_list
                                ]) + "}\n"
                                f_actions.write(
                                    action_logging_format.format(
                                        *logging_list))

                            if not CONFIG["test_policy"]:
                                # ---- Policy Update -------
                                self.update_policy(td_error, exploration)

                            prev_critic_gradient = deepcopy(critic_gradient)

                        reward_cum += reward
                        if to_end:
                            reward_cum = -3000  # for logging only
                            break

                    # TODO - add check for if episode ended early. i.e. moving average
                    """ episode_ended_learning = self.env.episodeEnded()

                     if episode_ended_learning:
                        # episode complete, start a new one
                        break """
                # episode either ended early due to failure or completed max number of steps
                print("Episode ended - Learning {0} {1}".format(
                    episode_number, reward_cum))

                f_returns.write("{0}\t{1}\n".format(episode_number,
                                                    reward_cum))

                np.save(
                    "{0}/policy_params{1}".format(results_dir, episode_number),
                    self.approx_policy.getParams())
                np.save(
                    "{0}/critic_params{1}".format(results_dir, episode_number),
                    self.approx_critic.getParams())