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