def obstacle_matrix_spherical_base_frame(obstacles_buffer, ee, resolution, min_distance_activation_threshold):
spherical_pixel_matrix = np.zeros(resolution)
elevation_pixel_size = pi/resolution[0]
azimuth_pixel_size = 2*pi/resolution[1]
for obstacle in obstacles_buffer:
center = obstacle[0:3]
radius = obstacle[3]
if radius < 0.00001:
continue
diff_vector = vector(center) - vector(ee) # in the base-frame of the robot, i.e. each position in the matrix represents static directions for the ur5 because it does not move or rotate its base frame
(distance_to_center, elevation, azimuth) = spherical_coordinates(diff_vector)
#elevation += pi/2 # So it goes from 0 to pi instead of -pi/2 to pi/2
#azimuth += pi # So it goes from 0 to 2*pi instead of -pi to pi
#print(elevation)
#print(azimuth)
#print('')
elevation_pixel = int(elevation/elevation_pixel_size)
azimuth_pixel = int(azimuth/azimuth_pixel_size)
distance_to_border = distance_to_center - (radius + _circle_of_acceptance)
if distance_to_border < min_distance_activation_threshold:
#element_activation = 1 - distance_to_border / min_distance_activation_threshold # 1 when on obstacle border, 0 to 1 when distance to border less than threshold, 0 otherwise
#element_activation /= 10
#if spherical_pixel_matrix[elevation_pixel, azimuth_pixel] < element_activation:
#spherical_pixel_matrix[elevation_pixel, azimuth_pixel] = element_activation # Keep the largest activation
spherical_pixel_matrix[elevation_pixel, azimuth_pixel] = 1#12
return spherical_pixel_matrix
def get_episode(telemetrypath, episode_num):
environment_df = pd.read_csv(telemetrypath + '/metrics/environments.csv')
environment_df.replace(["NaN", 'NaT', 'infty'], np.nan, inplace=True)
q_0 = environment_df.values[episode_num, 1:7]
ee_0 = environment_df.values[episode_num, 7:10]
ee_d = environment_df.values[episode_num, 10:13]
cartesian_obstacles = []
for i in range(max_obstacles):
center = environment_df.values[episode_num,
13 + i * 4:13 + (i + 1) * 4 - 1]
r = environment_df.values[episode_num, 13 + (i + 1) * 4 - 1]
illegal = False
for e in center + [r]:
if np.isnan(e):
illegal = True
break
if illegal:
continue
print(center)
print(r)
cartesian_obstacles += [
cartesian_sphere(center[0], center[1], center[2], r)
]
print(cartesian_obstacles)
return vector(q_0), (vector(ee_0), vector(ee_d)), cartesian_obstacles
def simulation_input_to_output(self, q, ee, ee_d, a, a_d, obstacles_buffer,
max_obstacles):
input_to_model = self.step_input_parser_func(q, ee, ee_d, a, a_d,
obstacles_buffer,
max_obstacles)
output_from_model = self.keras_model.predict(input_to_model)
q_dot_ref = vector(self.step_output_parser_func(output_from_model))
q_dot_ref = vector(np.squeeze(q_dot_ref))
return q_dot_ref
def observe_episode(seed,
datapath,
modelpath,
modelname,
episode_configuration_init,
episode_waypoint_cartesian_positions,
episode_cartesian_obstacles,
max_timesteps,
from_latest_checkpoint=True,
exit_criteria=exit_criteria_at_end_waypoint_or_i_max,
max_obstacles=max_obstacles,
actuators='perfect_position',
record=True):
(random, randomstate, seed) = CAI_random(seed)
if not modelpath.endswith('/'):
modelpath = modelpath + '/'
model = None
if from_latest_checkpoint:
model, _ = load_model_and_history_from_latest_checkpoint(
random, modelpath, modelname)
else:
model, _ = load_model_and_history(random,
modelpath + modelname + '.h5')
model.summary()
inference_model = Inference_Model(modelname, model, datapath)
history = pandas_episode_trajectory_initialize(max_timesteps,
max_obstacles)
ca_tasks = [0 for _ in episode_cartesian_obstacles]
for obst_i, obstacle in enumerate(episode_cartesian_obstacles):
ca_tasks[obst_i] = ca_task(
vector([0, 0, 0]),
vector([obstacle.center_x, obstacle.center_y, obstacle.center_z]),
obstacle.radius, np.infty)
top_priority_set_based_tasks = ca_tasks
rtpwrapped = rtpUR5()
simulate(top_priority_set_based_tasks, (episode_configuration_init, 0),
episode_waypoint_cartesian_positions,
rtpwrapped,
max_timesteps,
history,
actuators=actuators,
exit_criteria=exit_criteria,
random=random,
record=record,
inference_model=inference_model)
def CAVIKAUGee_sphere_input_from_CAVIKee_slots_IO(slotted_input, max_obstacles, mean, std):
obstacles_buffer = make_obstacles_buffer(slotted_input[9:], max_obstacles)
ee = forward_kinematic_position(vector(slotted_input[0:6]))
obstacle_pixel_matrix = obstacle_matrix_spherical_base_frame(obstacles_buffer, ee, CAVIKAUGee_sphere_resolution, _min_distance_activation_threshold)
obstacle_pixel_array = [ pixel for row in obstacle_pixel_matrix for pixel in row ]
sphereified_input = np.hstack(((slotted_input[0:9]-mean)/std, obstacle_pixel_array))
return sphereified_input
def get_episode_better(path, episode_num):
episode_summaries = pd.read_csv(path + '/episodes_summaries.csv',
index_col=[0, 1])
episode_summary = episode_summaries.xs('episode' + str(episode_num) +
'.csv',
level='filename')
initial_config = [
episode_summary.loc['waypoint_0']['q1'],
episode_summary.loc['waypoint_0']['q2'],
episode_summary.loc['waypoint_0']['q3'],
episode_summary.loc['waypoint_0']['q4'],
episode_summary.loc['waypoint_0']['q5'],
episode_summary.loc['waypoint_0']['q6']
]
initial_position = [
episode_summary.loc['waypoint_0']['x'],
episode_summary.loc['waypoint_0']['y'],
episode_summary.loc['waypoint_0']['z']
]
desired_position = [
episode_summary.loc['waypoint_1']['x'],
episode_summary.loc['waypoint_1']['y'],
episode_summary.loc['waypoint_1']['z']
]
obstacles = episode_summary[episode_summary.index.str.contains(
'obstacle_')]
cartesian_obstacles = [
cartesian_sphere(obstacles.loc[obstacle_name]['x'],
obstacles.loc[obstacle_name]['y'],
obstacles.loc[obstacle_name]['z'],
obstacles.loc[obstacle_name]['radius'])
for obstacle_name in obstacles.index
]
print(initial_config)
return (vector(initial_config),
0), (vector(initial_position),
vector(desired_position)), cartesian_obstacles
def plot_ca_compare(histories, cartesian_obstacles,show=False, init=True):
fig, ax = _plot_init(None, None, init=init)
markers = ['v', '|', '.', '1', '4']
axs = [ plt.subplot(len(histories), 1, i+1) for i in range(len(histories)) ]
y_inside_obstacle = [[ 0 for _ in range(history.values.shape[0]) ] for history in histories]
y_max = [0, 0, 0]
for obst_i, (obstacle, marker) in enumerate(zip(cartesian_obstacles, markers)):
oc = vector([obstacle.center_x, obstacle.center_y, obstacle.center_z])
r = obstacle.radius
for ax_index, (history_i, ax_i) in enumerate(zip(histories, axs)):
plot_position_task_sigma(history_i, oc, r, fig=fig, ax=ax_i, marker=marker, label='|d' + str(obst_i) + '|')
for index, yi in enumerate(_phold.y):
if yi <= 0:
y_inside_obstacle[ax_index][index] += 1
if yi > y_max[ax_index]:
y_max[ax_index] = yi
for ax_index, (y_ax, ax_i) in enumerate(zip(y_inside_obstacle, axs)):
prev = 0
for index, ynum_inside in enumerate(y_ax):
if prev == 0 and ynum_inside > 0:
ax_i.plot((index*sim_timestep, index*sim_timestep), (0.16*y_max[ax_index], -0.16*y_max[ax_index]), 'red')
if ynum_inside == 0 and prev > 0:
ax_i.plot((index*sim_timestep, index*sim_timestep), (0.16*y_max[ax_index], -0.16*y_max[ax_index]), 'red')
prev = ynum_inside
for ax_i in axs:
plot_dashed_line_at_y(0, fig=fig, ax=ax_i)
ax_i.legend(prop={'size': 6}, loc=1)
ax_i.set_xlabel('Time [s]')
ax_i.set_ylabel('Distance [m]')
plt.subplots_adjust(hspace=0.5)
if show:
plt.show()
return fig, ax
def plot_tracking_and_ca(history, ee_d, cartesian_obstacles, show=False, init=True):
fig, ax = plot_position_task_sigma(history, ee_d, 0, init=init, marker='*')
markers = ['v', '|', '.', '1', '4']
for obstacle, marker in zip(cartesian_obstacles, markers):
oc = vector([obstacle.center_x, obstacle.center_y, obstacle.center_z])
r = obstacle.radius
plot_position_task_sigma(history, oc, r, fig=fig, ax=ax, marker=marker)
ax.legend(['|eed - ee|'] + ['|d' + str(obst_i) + '|' for obst_i in range(len(cartesian_obstacles))], loc=1)
ax.set_xlabel('Time [s]')
ax.set_ylabel('Distance [m]')
at_y = 0
plot_dashed_line_at_y(at_y, fig=fig, ax=ax)
if show:
plt.show()
return fig, ax
def observe(seed,
datapath,
modelpath,
modelnames,
modelinitials,
num_episodes,
max_timesteps,
max_obstacles,
from_episode_num=None,
from_latest_checkpoint=True,
from_checkpoint_num=None,
actuators='position',
verbose=False):
(random, randomstate, seed) = CAI_random(seed)
if from_episode_num is not None:
num_episodes = 1
plotpath = datapath + '/sessions/CAI/plots'
if not modelpath.endswith('/'):
modelpath = modelpath + '/'
inference_models = []
for modelname in modelnames:
model = None
if from_latest_checkpoint:
model, _ = load_model_and_history_from_latest_checkpoint(
random, modelpath, modelname)
else:
model = thesis_load_model(
random, modelpath,
modelname + '_checkpoint_' + str(from_checkpoint_num))
model.summary()
inference_models += [Inference_Model(modelname, model, datapath)]
simulation_histories = [[] for _ in range(num_episodes)]
simulation_waypoints = [[] for _ in range(num_episodes)]
model_histories = [[] for _ in modelinitials]
telemetries = []
for modelinitial in modelinitials:
telemetries += [
Telemetry(num_episodes, max_timesteps, max_obstacles, seed,
plotpath, modelinitial)
]
rtpwrapped = rtpUR5()
for i in range(num_episodes):
if from_episode_num is not None:
i = from_episode_num
history_infer = pandas_episode_trajectory_initialize(
max_timesteps, max_obstacles)
history_solution = pandas_episode_trajectory_initialize(
max_timesteps, max_obstacles)
history_no_ca = pandas_episode_trajectory_initialize(
max_timesteps, max_obstacles)
if from_episode_num is None:
(
(waypoint_configurations, waypoint_cartesian_positions,
cartesian_obstacles, _, _, _, _, _, _), _, _
) = generate_and_simulate_forced_bias_pattern_near_trajectory_infer_and_compare(
random,
history_infer,
history_solution,
history_no_ca,
inference_models[0],
None,
max_timesteps,
exit_criteria=exit_criteria_at_end_waypoint_or_i_max,
max_obstacles=max_obstacles,
actuators='perfect_position',
record=True)
ca_tasks = [0 for _ in cartesian_obstacles]
for obst_i, obstacle in enumerate(cartesian_obstacles):
ca_tasks[obst_i] = ca_task(
vector([0, 0, 0]),
vector([
obstacle.center_x, obstacle.center_y, obstacle.center_z
]), obstacle.radius, np.infty)
top_priority_set_based_tasks = ca_tasks
else:
waypoint_configurations, waypoint_cartesian_positions, cartesian_obstacles = get_episode_better(
datapath + '/rawdata/ca_trackpos/', from_episode_num)
ca_tasks = [0 for _ in cartesian_obstacles]
for obst_i, obstacle in enumerate(cartesian_obstacles):
ca_tasks[obst_i] = ca_task(
vector([0, 0, 0]),
vector([
obstacle.center_x, obstacle.center_y, obstacle.center_z
]), obstacle.radius, np.infty)
top_priority_set_based_tasks = ca_tasks
simulate(top_priority_set_based_tasks,
waypoint_configurations,
waypoint_cartesian_positions,
rtpwrapped,
max_timesteps,
history_infer,
actuators=actuators,
exit_criteria=exit_criteria_at_end_waypoint_or_i_max,
random=random,
record=True,
inference_model=inference_models[0])
simulate(top_priority_set_based_tasks,
waypoint_configurations,
waypoint_cartesian_positions,
rtpwrapped,
max_timesteps,
history_solution,
actuators=actuators,
exit_criteria=exit_criteria_at_end_waypoint_or_i_max,
random=random,
record=True,
inference_model=None)
simulate([],
waypoint_configurations,
waypoint_cartesian_positions,
rtpwrapped,
max_timesteps,
history_no_ca,
actuators=actuators,
exit_criteria=exit_criteria_at_end_waypoint_or_i_max,
random=random,
record=True,
inference_model=None)
if from_episode_num is None:
telemetries[0].gather(history_infer, history_solution,
history_no_ca,
waypoint_cartesian_positions[0],
waypoint_cartesian_positions[1],
waypoint_configurations[0],
cartesian_obstacles)
else:
telemetries[0].gather(
history_infer,
history_solution,
history_no_ca,
waypoint_cartesian_positions[0],
waypoint_cartesian_positions[1],
waypoint_configurations[0],
cartesian_obstacles,
alternate_episode_index_name=from_episode_num)
model_histories[0] = copy.deepcopy(history_infer)
model_histories_index = 1
for telemetry, inference_model in zip(telemetries[1:],
inference_models[1:]):
history_infer = pandas_episode_trajectory_initialize(
max_timesteps, max_obstacles)
simulate(top_priority_set_based_tasks,
waypoint_configurations,
waypoint_cartesian_positions,
rtpwrapped,
max_timesteps,
history_infer,
actuators=actuators,
exit_criteria=exit_criteria_at_end_waypoint_or_i_max,
random=random,
record=True,
inference_model=inference_model)
if from_episode_num is None:
telemetry.gather(history_infer, history_solution,
history_no_ca,
waypoint_cartesian_positions[0],
waypoint_cartesian_positions[1],
waypoint_configurations[0],
cartesian_obstacles)
else:
telemetry.gather(history_infer,
history_solution,
history_no_ca,
waypoint_cartesian_positions[0],
waypoint_cartesian_positions[1],
waypoint_configurations[0],
cartesian_obstacles,
alternate_episode_index_name=from_episode_num)
model_histories[model_histories_index] = copy.deepcopy(
history_infer)
model_histories_index += 1
fig, _ = plot_ca_compare(model_histories, cartesian_obstacles)
save_plot(
fig, plotpath + '/seed' + str(seed) + '_nume' + str(num_episodes) +
'_numt' + str(max_timesteps) +
'_border_distance_model_comparisons', 'episode_' + str(i))
fig, _ = plot_tracking_compare_models(model_histories,
('sphere', 'slot', 'control'),
waypoint_cartesian_positions[1],
markevery=1000)
save_plot(
fig, plotpath + '/seed' + str(seed) + '_nume' + str(num_episodes) +
'_numt' + str(max_timesteps) + '_position_error_model_comparisons',
'episode_' + str(i))
for telemetry in telemetries:
telemetry.end_and_save()
def gather(self,
history_inference,
history_solution,
history_no_ca,
ee_0,
ee_d,
q_0,
cartesian_obstacles,
alternate_episode_index_name=None):
if alternate_episode_index_name is None:
alternate_episode_index_name = self.episode_index
history_inference = self.remove_nan_from_df(history_inference)
history_solution = self.remove_nan_from_df(history_solution)
ee_inf = history_inference.values[:, 6:9]
ee_sol = history_solution.values[:, 6:9]
inf_numsteps = ee_inf.shape[0]
sol_numsteps = ee_sol.shape[0]
most_timesteps = inf_numsteps if inf_numsteps > sol_numsteps else sol_numsteps
inf_distance_to_ee_d = self.task_distance(ee_inf, ee_d, 0)
sol_distance_to_ee_d = self.task_distance(ee_sol, ee_d, 0)
inf_tracking_error_against_solution = self.task_inference_error(
ee_inf, ee_sol, ee_d, 0)
solution_obstacle_border_distance = np.zeros(
(self.max_obstacles, sol_numsteps))
inference_obstacle_border_distance = np.zeros(
(self.max_obstacles, inf_numsteps))
sol_actively_avoided_with_success = np.zeros(self.max_obstacles)
inf_actively_avoided_with_success = np.zeros(self.max_obstacles)
sol_failed_avoidance = np.zeros(self.max_obstacles)
inf_failed_avoidance = np.zeros(self.max_obstacles)
for obst_i, obstacle in enumerate(cartesian_obstacles):
solution_obstacle_border_distance[obst_i, :] = self.task_distance(
ee_sol,
vector(
[obstacle.center_x, obstacle.center_y, obstacle.center_z]),
obstacle.radius)
inference_obstacle_border_distance[obst_i, :] = self.task_distance(
ee_inf,
vector(
[obstacle.center_x, obstacle.center_y, obstacle.center_z]),
obstacle.radius)
sol_actively_avoided_with_success[obst_i] = 1 if all(
border_distance > 0 for border_distance in
solution_obstacle_border_distance[obst_i, :]) and any(
border_distance < sim_coa
for border_distance in solution_obstacle_border_distance[
obst_i, :]) else 0
inf_actively_avoided_with_success[obst_i] = 1 if all(
border_distance > 0 for border_distance in
inference_obstacle_border_distance[obst_i, :]) and any(
border_distance < sim_coa
for border_distance in inference_obstacle_border_distance[
obst_i, :]) else 0
sol_failed_avoidance[obst_i] = 1 if any(
border_distance < 0
for border_distance in solution_obstacle_border_distance[
obst_i, :]) else 0
inf_failed_avoidance[obst_i] = 1 if any(
border_distance < 0
for border_distance in inference_obstacle_border_distance[
obst_i, :]) else 0
inference_converged = self.did_converge(ee_inf, ee_d)
solution_converged = self.did_converge(ee_sol, ee_d)
both_converged = solution_converged * inference_converged
inf_converged_but_sol_did_not = inference_converged * (
1 - solution_converged)
sol_converged_but_inf_did_not = solution_converged * (
1 - inference_converged)
both_actively_avoided_with_success = sum(
sol_actively_avoided_with_success *
inf_actively_avoided_with_success)
inf_actively_avoided_when_sol_failed = sum(
inf_actively_avoided_with_success * sol_failed_avoidance)
sol_actively_avoided_when_inf_failed = sum(
sol_actively_avoided_with_success * inf_failed_avoidance)
both_failed_avoidance = sum(sol_failed_avoidance *
inf_failed_avoidance)
inf_failed_avoidance_but_sol_did_not = sum(inf_failed_avoidance *
(1 - sol_failed_avoidance))
sol_failed_avoidance_but_inf_did_not = sum(sol_failed_avoidance *
(1 - inf_failed_avoidance))
self.task_performance_df.at[
self.episode_index, 'inference_converged'] = inference_converged
self.task_performance_df.at[self.episode_index,
'solution_converged'] = solution_converged
self.task_performance_df.at[self.episode_index,
'both_converged'] = both_converged
self.task_performance_df.at[
self.episode_index,
'inf_converged_but_sol_did_not'] = inf_converged_but_sol_did_not
self.task_performance_df.at[
self.episode_index,
'sol_converged_but_inf_did_not'] = sol_converged_but_inf_did_not
self.task_performance_df.at[self.episode_index,
'sol_actively_avoided_with_success'] = sum(
sol_actively_avoided_with_success)
self.task_performance_df.at[self.episode_index,
'inf_actively_avoided_with_success'] = sum(
inf_actively_avoided_with_success)
self.task_performance_df.at[
self.episode_index,
'both_actively_avoided_with_success'] = both_actively_avoided_with_success
self.task_performance_df.at[
self.episode_index,
'sol_actively_avoided_when_inf_failed'] = sol_actively_avoided_when_inf_failed
self.task_performance_df.at[
self.episode_index,
'inf_actively_avoided_when_sol_failed'] = inf_actively_avoided_when_sol_failed
self.task_performance_df.at[self.episode_index,
'sol_failed_avoidance'] = sum(
sol_failed_avoidance)
self.task_performance_df.at[self.episode_index,
'inf_failed_avoidance'] = sum(
inf_failed_avoidance)
self.task_performance_df.at[
self.episode_index,
'both_failed_avoidance'] = both_failed_avoidance
self.task_performance_df.at[
self.episode_index,
'sol_failed_avoidance_but_inf_did_not'] = sol_failed_avoidance_but_inf_did_not
self.task_performance_df.at[
self.episode_index,
'inf_failed_avoidance_but_sol_did_not'] = inf_failed_avoidance_but_sol_did_not
self.obst_metrics_df.ix[:, self.episode_index *
4] = sol_actively_avoided_with_success
self.obst_metrics_df.ix[:, self.episode_index * 4 +
1] = inf_actively_avoided_with_success
self.obst_metrics_df.ix[:, self.episode_index * 4 +
2] = sol_failed_avoidance
self.obst_metrics_df.ix[:, self.episode_index * 4 +
3] = inf_failed_avoidance
self.environments_df.ix[self.episode_index, 0:6] = q_0[:, 0]
self.environments_df.ix[self.episode_index, 6:9] = ee_0[:, 0]
self.environments_df.ix[self.episode_index, 9:12] = ee_d[:, 0]
for obst_i, obstacle in enumerate(cartesian_obstacles):
self.environments_df.ix[self.episode_index, 12 + obst_i * 4:12 +
(1 + obst_i) * 4] = obstacle
# Count averages if the solution converged (if not, it is likely singular or stuck)
#if solution_converged: # If not, then its num timesteps will be zero, and it will not have added anything to the averages, i.e. the remaining code still correctly calculates the averages
self.inference_episodes_numsteps[self.episode_index] = inf_numsteps
self.solution_episodes_numsteps[self.episode_index] = sol_numsteps
self.avg_inf_distance_to_ee_d[:inf_numsteps] += np.array(
inf_distance_to_ee_d)
self.avg_sol_distance_to_ee_d[:sol_numsteps] += np.array(
sol_distance_to_ee_d)
self.avg_inf_tracking_error_against_solution[:most_timesteps] += np.array(
inf_tracking_error_against_solution)
# Plots
fig, _ = plot_tracking_compare_models([history_inference], ['Error'],
ee_d) # ToDo: Function name, etc
save_plot(fig, self.telemetrypath + '/00inference_tracking_distance',
'episode_' + str(alternate_episode_index_name))
fig, _ = plot_tracking_and_ca(history_inference, ee_d,
cartesian_obstacles)
save_plot(
fig,
self.telemetrypath + '/01inference_tracking_and_border_distance',
'episode_' + str(alternate_episode_index_name))
fig, _ = plot_tracking_and_ca(history_solution, ee_d,
cartesian_obstacles)
save_plot(
fig,
self.telemetrypath + '/02solution_tracking_and_border_distance',
'episode_' + str(alternate_episode_index_name))
fig, _ = plot_tracking_compare(history_inference, history_solution,
ee_d)
save_plot(fig, self.telemetrypath + '/03tracking_comparison',
'episode_' + str(alternate_episode_index_name))
fig, _ = plot_ca_compare(
[history_solution, history_inference, history_no_ca],
cartesian_obstacles)
save_plot(fig, self.telemetrypath + '/04border_distance_comparison',
'episode_' + str(alternate_episode_index_name))
# Remember episode number
self.episode_index += 1
