def __init__(self):
args = sys.argv
if "-r" in args:
self.results_dir_name = args[args.index("-r") + 1]
else:
self.results_dir_name = "repeat_pilot_request"
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()
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)
def __init__(self):
args = sys.argv
if "-r" in args:
results_dir_name = args[args.index("-r") + 1]
else:
results_dir_name = "ga_pid_tuning"
self.results_dir = "/home/gordon/data/tmp/{0}{1}".format(
results_dir_name, 0)
if not os.path.exists(self.results_dir):
os.makedirs(self.results_dir)
filename = os.path.basename(sys.argv[0])
os.system("cp {0} {1}".format(filename, self.results_dir))
os.system(
"cp /home/gordon/rosbuild_ws/ros_simple_rl/src/ga_optimization/ga.py {0}"
.format(self.results_dir))
# TODO - add DataFrame for evolution history and generation
self.df_evolution_history = pd.DataFrame()
self.f_evolution_history = open(
"{0}{1}".format(self.results_dir, "/evolution_history.csv"), "w",
1)
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.baseline_response = optimal_control_response()
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)
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
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"]):
if CONFIG["test_policy"]:
CONFIG["dir_of_policies_to_load"] = results_to_validate[run]
if CONFIG["policy_test_type"] == "vali":
results_dir_name = "validate_{0}".format(
CONFIG["dir_of_policies_to_load"].split("/")[-1])
elif CONFIG["policy_test_type"] == "nessie":
results_dir_name = "real_{0}".format(
CONFIG["dir_of_policies_to_load"].split("/")[-1])
elif CONFIG["policy_test_type"] == "online_nessie":
results_dir_name = "online_{0}".format(
CONFIG["dir_of_policies_to_load"].split("/")[-1])
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"]):
if CONFIG["test_policy"]:
CONFIG["dir_of_policies_to_load"] = results_to_validate[run]
if CONFIG["policy_test_type"] == "vali":
results_dir_name = "validate_{0}".format(
CONFIG["dir_of_policies_to_load"].split("/")[-1])
elif CONFIG["policy_test_type"] == "nessie":
results_dir_name = "real_{0}".format(
CONFIG["dir_of_policies_to_load"].split("/")[-1])
# Create logging directory and files
results_dir = "/tmp/{0}{1}".format(results_dir_name, run)
print("results_dir: {0}".format(results_dir))
class RepeatPilotRequest(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 = "repeat_pilot_request"
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()
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)
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
}
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 run(self):
results_dir = "/home/gordon/data/tmp/{0}{1}".format(
self.results_dir_name, 0)
for run in range(CONFIG["num_runs"]):
# Create logging directory and files
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/utilities/repeat_pilot_request.py {0}"
.format(results_dir))
# reset stuff for the run
self.env.nav_reset()
self.env.reset()
self.angle_dt_moving_window.reset()
self.prev_angle_dt_t = 0.0
self.prev_angle_dt_tp1 = 0.0
# create log file
f_actions = open(
"{0}{1}".format(results_dir, "/actions{0}.csv".format(run)),
"w", 1)
start_time = time.time()
end_time = start_time + CONFIG["run_time"]
first_step = True
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 = self.state_t.keys()
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(self.pos_pid_output[5])
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))
parser.add_argument("--repeat",
type=int,
default=1,
help="Number of times to repeat the learning process")
parser.add_argument("--root",
type=str,
default="runs",
help="Root directory name")
args = parser.parse_args()
logger = SimpleLogger()
dir_manager = DirectoryManager(logger, args.root)
ros_env = ROSBehaviourInterface()
if args.validate == "":
for prop_value in PROP_VALUES:
result_dir = dir_manager.get_next_name("root", "ziegler_pid")
filename = os.path.basename(sys.argv[0])
dir_manager["run"] = result_dir
os.system("cp {0} {1}".format(filename, result_dir))
os.system(
"cp /home/gordon/rosbuild_ws/ros_simple_rl/src/ga_optimization/processes/pilot_pid_process.py {0}"
.format(result_dir))
# os.system("cp /home/gordon/rosbuild_ws/ros_simple_rl/src/ga_optimization/ga.py {0}".format(
# indexed_results_dir))
process = PilotPidProcess(ros_env, result_dir)
global episode_number, results_dir
logNav(log_nav=False, dir_path=results_dir, file_name_descriptor=str(episode_number))
if __name__ == '__main__':
args = sys.argv
if "-r" in args:
results_dir_name = args[args.index("-r") + 1]
else:
results_dir_name = "cacla_run"
rospy.init_node("natural_actor_critic")
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])
class OptimizePilotPid(object):
def __init__(self):
args = sys.argv
if "-r" in args:
results_dir_name = args[args.index("-r") + 1]
else:
results_dir_name = "ga_pid_tuning"
self.results_dir = "/home/gordon/data/tmp/{0}{1}".format(
results_dir_name, 0)
if not os.path.exists(self.results_dir):
os.makedirs(self.results_dir)
filename = os.path.basename(sys.argv[0])
os.system("cp {0} {1}".format(filename, self.results_dir))
os.system(
"cp /home/gordon/rosbuild_ws/ros_simple_rl/src/ga_optimization/ga.py {0}"
.format(self.results_dir))
# TODO - add DataFrame for evolution history and generation
self.df_evolution_history = pd.DataFrame()
self.f_evolution_history = open(
"{0}{1}".format(self.results_dir, "/evolution_history.csv"), "w",
1)
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.baseline_response = optimal_control_response()
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)
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
}
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", str(velP))
# rospy.set_param("/pilot/controller/vel_r/ki", str(velI))
# rospy.set_param("/pilot/controller/vel_r/kd", str(velD))
self.env.enable_pilot(True)
def run(self):
# Inputs of the equation.
ga_state = [0.5, 0.0]
# Preparing the population
# Number of the weights we are looking to optimize.
ga_num_weights = len(ga_state)
pop_size = (CONFIG["sol_per_pop"], ga_num_weights)
population = np.random.uniform(low=0.0, high=0.1, size=pop_size)
f_generation_max = open(
"{0}{1}".format(self.results_dir, "/generation_history.csv"), "w",
1)
global_individual_id = 0
# TODO - loop generations
for generation_id in range(CONFIG["num_generations"]):
generation_df = pd.DataFrame()
local_individual_id = 0
print("------- New Generation ----------")
# TODO - loop over population
for individual in population:
print("----Individual----")
# TODO - Check for similar individual in history and if exists, just use it's fitness
# individual_df = pd.DataFrame([tuple(individual)])
# fudge the column names to be consistent with self.df_evolution_history
# individual_df.rename(columns={0: 1, 1: 2}, inplace=True)
run_response = True
match_found = False
if not self.df_evolution_history.empty:
match_found, first_occurance_idx, fitness = self.check_for_existing_individual(
individual, ga_num_weights)
if match_found:
# print("Match found in history for individual, using it's previous fitness: {0}".format(fitness))
run_response = False
if run_response:
response = self.get_response(global_individual_id,
individual)
# print("response:")
# print_friendly_rounded = np.around(response[0:50, :], decimals=2)
# print(print_friendly_rounded)
individual_fitness = self.calculate_fitness(response)
else:
individual_fitness = fitness
individual_copy = deepcopy(individual)
# gen_row2 = np.array([global_individual_id, individual_copy[0], individual_copy[1], individual_fitness])
gen_row = pd.DataFrame()
gen_row[0] = [global_individual_id
] if not match_found else [first_occurance_idx]
gen_row[1] = [individual_copy[0]]
gen_row[2] = [individual_copy[1]]
gen_row[3] = [individual_fitness]
print("Row being added to generation_df: ")
print(gen_row)
generation_df = generation_df.append(gen_row,
ignore_index=True)
# if local_individual_id == 0:
# gen_df_last_index = generation_df.columns.array[-1]
# generation_df.rename(columns={gen_df_last_index: 'fitness'}, inplace=True)
evolution_row = pd.DataFrame()
evolution_row[0] = [global_individual_id]
evolution_row[1] = [individual_copy[0]]
evolution_row[2] = [individual_copy[1]]
evolution_row[3] = [individual_fitness]
self.df_evolution_history = self.df_evolution_history.append(
evolution_row, ignore_index=True)
# print("DataFrame Evolution History: {0}".format(global_individual_id))
# print(self.df_evolution_history)
# if global_individual_id == 0:
# self.df_evolution_history.rename(
# columns={self.df_evolution_history.columns.array[-1]: 'fitness'},
# inplace=True
# )
self.log_entry(self.f_evolution_history, global_individual_id,
individual, individual_fitness)
global_individual_id += 1
local_individual_id += 1
# fitness now calculated for population
# print("----------")
# print(generation_df.head(8))
# print("-----=======")
# print(self.df_evolution_history)
# print("============")
# Log the max fitness for the current generation
best_fitness_idx = generation_df[
generation_df.columns[-1]].idxmin()
print("generation row idx of min fitness: {0}".format(
best_fitness_idx))
generation_max_fitness = generation_df.iloc[
best_fitness_idx, generation_df.columns[-1]]
generation_max_global_id = generation_df.iloc[best_fitness_idx][0]
generation_max_fitness_individual = generation_df.iloc[
best_fitness_idx][1:ga_num_weights + 1]
self.log_entry(f_generation_max, generation_max_global_id,
generation_max_fitness_individual,
generation_max_fitness)
fitness = deepcopy(generation_df.iloc[:, -1].array)
print("Fitness array:")
print(fitness)
# Selecting the best parents in the population for mating.
parents = select_mating_pool(population, fitness,
CONFIG["num_parents_mating"])
print("parents (best {0}): ".format(CONFIG["num_parents_mating"]))
print(parents)
offspring_crossover = crossover(
parents,
offspring_size=(pop_size[0] - parents.shape[0],
ga_num_weights))
print("offspring_crossover: {0}".format(offspring_crossover))
offspring_mutation = mutation(offspring_crossover, num_mutations=2)
# Creating the new population based on the parents and offspring.
population[0:parents.shape[0], :] = parents
population[parents.shape[0]:, :] = offspring_mutation
def check_for_existing_individual(self, individual, ga_num_weights):
# print("check_for_existing_individual-> individual: {0}".format(individual))
evolution_minus_fitness = self.df_evolution_history.iloc[:, 1:
ga_num_weights
+ 1]
# print("check_for_existing_individual-> evolution_minus_fitness: {0}".format(evolution_minus_fitness))
match_found = False
fitness = 9999999
# TODO - remove loop and use more efficient numpy or pandas routine
# individual_exists = evolution_minus_fitness[(evolution_minus_fitness == individual_df).all(axis=1)]
for idx, row in evolution_minus_fitness.iterrows():
is_close = np.allclose(individual, row.array, atol=0.01)
if is_close:
match_found = True
history_entry = self.df_evolution_history.iloc[idx, :]
print("match found: ")
print(history_entry)
fitness = history_entry.array[-1]
break
return match_found, idx, fitness
def log_entry(self, file, id, individual, fitness):
delimiter = "\t"
ind_str = '\t'.join(map(str, individual))
entry = "{0}{1}{2}{3}{4}\n".format(id, delimiter, ind_str, delimiter,
fitness)
file.write(entry)
def calculate_fitness(self, response):
diff = abs(response) - abs(self.baseline_response)
max_idx = 100 # 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, 0, 0, 0)
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], 0, individual[1], 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 = 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 = 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
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())