f_gradient_vec_diff = open(
"{0}{1}".format(
results_dir,
"/GradientVecDiff{0}.fso".format(episode_number)), "w",
1)
if episode_number > 0:
td_lambda.decayStatistics(decay=0.0)
# traces.reset()
nessie.disable_behaviours(True)
nessie.nav_reset()
random_yaw = float(randrange(-314, 314, 1)) / 100.0
possible_starting_states = [-1.57, 1.57]
# random_yaw = possible_starting_states[episode_number % 2]
print("YAWING to: {0}".format(random_yaw))
nessie.pilot([0, 0, 0, 0, 0, 1.57])
rospy.sleep(2.0)
nessie.disable_behaviours(False)
if episode_number % CONFIG["log_actions"] == 0:
nessie.logNav(log_nav=True,
dir_path=results_dir,
file_name_descriptor=str(episode_number))
if episode_number > 5 and exploration_sigma > 0.1:
exploration_sigma *= CONFIG["exploration_decay"]
# Loop number of steps
for step_number in range(CONFIG["max_num_steps"]):
print("------ STEP {0} -------".format(step_number))
# --------- STATE AND TRACES ---------
# get current state
"/GradientVecDiff{0}.fso".format(episode_number)), "w",
1)
if episode_number > 0:
td_lambda.decayStatistics(decay=0.0)
# traces.reset()
nessie.disable_behaviours(True)
# print("RESETTING NAV")
# nessie.nav_reset()
random_yaw = float(randrange(-314, 314, 1)) / 100.0
possible_starting_states = [
1.57, 1.57, 1.57, 1.57, 1.57, -1.57, -1.57, -1.57, -1.57, -1.57
]
# random_yaw = possible_starting_states[episode_number % 2]
print("YAWING to: {0}".format(random_yaw))
nessie.pilot(
[0, 0, 0, 0, 0, possible_starting_states[episode_number]])
rospy.sleep(2.0)
nessie.disable_behaviours(False)
# if episode_number % CONFIG["log_actions"] == 0:
# nessie.logNav(log_nav=True, dir_path=results_dir, file_name_descriptor=str(episode_number))
if episode_number > 5 and exploration_sigma > 0.1:
exploration_sigma *= CONFIG["exploration_decay"]
# Loop number of steps
for step_number in range(CONFIG["max_num_steps"]):
print("------ STEP {0} -------".format(step_number))
# --------- STATE AND TRACES ---------
# get current state
step_start_time = time.time()
print("prev distance 1: {0}".format(prev_distance_to_goal))
critic_value_func_params = None
if episode_number == CONFIG["episode_fault_injected"]:
thrusters.inject_surge_thruster_fault(17, 1)
td_lambda.episodeReset()
# Reset the last action value
# last_yaw_mean = yaw_ros_action("gaussian_variance", 0.7, False, 1.0)
# Enable behaviours
ros_behaviour_interface.disable_behaviours(disable=True)
ros_behaviour_interface.nav_reset()
random_yaw = 1.57 + 0.785#float(random.randrange(-314, 314, 5)) / 100
print("Moving to Random Starting Yaw: {0}".format(random_yaw))
hover_completed = ros_behaviour_interface.pilot([0, 0, 0, 0, 0,random_yaw])
rospy.sleep(5)
ros_behaviour_interface.disable_behaviours(disable=False)
print(hover_completed)
rospy.sleep(0.5) # give the nav topic callback to be called before the next episode is started.
# otherwise the next episode may start in a episode termination condition
logNav(log_nav=True, dir_path=results_dir, file_name_descriptor=str(episode_number))
# Loop number of steps
for step_number in range(CONFIG["max_num_steps"]):
# --------- STATE AND TRACES ---------
# get current state
step_start_time = time.time()
print("------ STEP {0} -------".format(step_number))
state_t = {"angle": deepcopy(environmental_data.raw_angle_to_goal),