def cost_fun(y_in, x_in, other_params): roll_des = other_params['roll_des'] pitch_des = other_params['pitch_des'] A_vel = other_params['A_vel'] A_acc = other_params['A_acc'] A_jerk = other_params['A_jerk'] rho_vel = other_params['rho_vel'] rho_acc = other_params['rho_acc'] rho_jerk = other_params['rho_jerk'] rho_b = other_params['rho_b'] rho_orient = other_params['rho_orient'] q_1, q_2, q_3, q_4, q_5, q_6, q_7 = utils.unstack_y(y_in) q_1_init, q_2_init, q_3_init, q_4_init, q_5_init, q_6_init, q_7_init, q_1_fin, q_2_fin, q_3_fin, q_4_fin, q_5_fin, q_6_fin, q_7_fin = unstack_x( x_in) roll, pitch = utils.get_roll_pitch(y_in) f_orient_cost = np.sum((roll - roll_des)**2 + (pitch - pitch_des)**2) f_smoothness_boundary = ((q_1[0]-q_1_init)**2+(q_1[-1]-q_1_fin)**2+(q_2[0]-q_2_init)**2+ \ (q_2[-1]-q_2_fin)**2+(q_3[0]-q_3_init)**2+(q_3[-1]-q_3_fin)**2+ \ (q_4[0]-q_4_init)**2+(q_4[-1]-q_4_fin)**2+(q_5[0]-q_5_init)**2+ \ (q_5[-1]-q_5_fin)**2+(q_6[0]-q_6_init)**2+(q_6[-1]-q_6_fin)**2+ \ (q_7[0]-q_7_init)**2+(q_7[-1]-q_7_fin)**2) f_smoothness_vel = np.sum(np.dot(A_vel, q_1)**2)+np.sum(np.dot(A_vel, q_2)**2)+ \ np.sum(np.dot(A_vel, q_3)**2)+np.sum(np.dot(A_vel, q_4)**2)+ \ np.sum(np.dot(A_vel, q_5)**2)+np.sum(np.dot(A_vel, q_6)**2)+ \ np.sum(np.dot(A_vel, q_7)**2) f_smoothness_acc = np.sum(np.dot(A_acc, q_1)**2)+np.sum(np.dot(A_acc, q_2)**2)+ \ np.sum(np.dot(A_acc, q_3)**2)+np.sum(np.dot(A_acc, q_4)**2)+ \ np.sum(np.dot(A_acc, q_5)**2)+np.sum(np.dot(A_acc, q_6)**2)+ \ np.sum(np.dot(A_acc, q_7)**2) f_smoothness_jerk = np.sum(np.dot(A_jerk, q_1)**2)+np.sum(np.dot(A_jerk, q_2)**2)+ \ np.sum(np.dot(A_jerk, q_3)**2)+np.sum(np.dot(A_jerk, q_4)**2)+ \ np.sum(np.dot(A_jerk, q_5)**2)+np.sum(np.dot(A_jerk, q_6)**2)+ \ np.sum(np.dot(A_jerk, q_7)**2) f_smoothness_cost = rho_vel * f_smoothness_vel + rho_acc * f_smoothness_acc + rho_b * f_smoothness_boundary + rho_jerk * f_smoothness_jerk cost = rho_orient * f_orient_cost + f_smoothness_cost return cost
def cost_fun(y_in, x_in, other_params): roll_des = other_params['roll_des'] pitch_des = other_params['pitch_des'] A_vel = other_params['A_vel'] A_acc = other_params['A_acc'] A_jerk = other_params['A_jerk'] rho_vel = other_params['rho_vel'] rho_acc = other_params['rho_acc'] rho_jerk = other_params['rho_jerk'] rho_b = other_params['rho_b'] rho_orient = other_params['rho_orient'] rho_pos = other_params['rho_pos'] q_1_init = other_params['q_1_init'] q_2_init = other_params['q_2_init'] q_3_init = other_params['q_3_init'] q_4_init = other_params['q_4_init'] q_5_init = other_params['q_5_init'] q_6_init = other_params['q_6_init'] q_7_init = other_params['q_7_init'] x_fin = other_params['x_fin'] y_fin = other_params['y_fin'] z_fin = other_params['z_fin'] mid_index = other_params['mid_index'] q_1, q_2, q_3, q_4, q_5, q_6, q_7 = utils.unstack_y(y_in) x_mid, y_mid, z_mid = unstack_x(x_in) x, y, z, tr, ax1, ax2, ax3 = utils.fk_franka_traj(y_in) roll, pitch = utils.get_roll_pitch(y_in) f_orient_cost = np.sum((roll - roll_des)**2 + (pitch - pitch_des)**2) f_pos_cost = (x[-1] - x_fin)**2 + (y[-1] - y_fin)**2 + (z[-1] - z_fin)**2 f_pos_mid = (x[mid_index] - x_mid)**2 + (y[mid_index] - y_mid)**2 + ( z[mid_index] - z_mid)**2 f_smoothness_boundary = ((q_1[0]-q_1_init)**2+(q_2[0]-q_2_init)**2+ \ (q_3[0]-q_3_init)**2+ (q_4[0]-q_4_init)**2+ \ (q_5[0]-q_5_init)**2+ (q_6[0]-q_6_init)**2+\ (q_7[0]-q_7_init)**2) f_smoothness_vel = np.sum(np.dot(A_vel, q_1)**2)+np.sum(np.dot(A_vel, q_2)**2)+ \ np.sum(np.dot(A_vel, q_3)**2)+np.sum(np.dot(A_vel, q_4)**2)+ \ np.sum(np.dot(A_vel, q_5)**2)+np.sum(np.dot(A_vel, q_6)**2)+ \ np.sum(np.dot(A_vel, q_7)**2) f_smoothness_acc = np.sum(np.dot(A_acc, q_1)**2)+np.sum(np.dot(A_acc, q_2)**2)+ \ np.sum(np.dot(A_acc, q_3)**2)+np.sum(np.dot(A_acc, q_4)**2)+ \ np.sum(np.dot(A_acc, q_5)**2)+np.sum(np.dot(A_acc, q_6)**2)+ \ np.sum(np.dot(A_acc, q_7)**2) f_smoothness_jerk = np.sum(np.dot(A_jerk, q_1)**2)+np.sum(np.dot(A_jerk, q_2)**2)+ \ np.sum(np.dot(A_jerk, q_3)**2)+np.sum(np.dot(A_jerk, q_4)**2)+ \ np.sum(np.dot(A_jerk, q_5)**2)+np.sum(np.dot(A_jerk, q_6)**2)+ \ np.sum(np.dot(A_jerk, q_7)**2) f_smoothness_cost = rho_vel * f_smoothness_vel + rho_acc * f_smoothness_acc + rho_b * f_smoothness_boundary + rho_jerk * f_smoothness_jerk cost = rho_orient * f_orient_cost + f_smoothness_cost + rho_pos * ( f_pos_cost + f_pos_mid) return f_orient_cost, f_smoothness_boundary, f_smoothness_vel, f_smoothness_acc, f_smoothness_jerk, f_pos_cost, f_pos_mid
def main(perturb, save=False): all_list = [] for trajcount in range(6): for i in range(40): try: folder = '../output/' + EXPERIMENT_NAME + '_perturbation/' + perturb + '/traj' + str( trajcount) + '/' + str(i) data = np.load(folder + "/data.npy", allow_pickle=True) data = data.item() q_init = data['q_init'] q_fin = data['q_fin'] x_fin, y_fin, z_fin, _, _, _, _ = utils.fk_franka_traj(q_fin) x_fin = x_fin[0] y_fin = y_fin[0] z_fin = z_fin[0] other_params = get_other_params(q_init, x_fin, y_fin, z_fin) x_mid = data['x_mid'] y_mid = data['y_mid'] z_mid = data['z_mid'] x_mid_new = data['x_mid_new'] y_mid_new = data['y_mid_new'] z_mid_new = data['z_mid_new'] params = stack_x(x_mid_new, y_mid_new, z_mid_new) q = data['q'] q_new = data['q_new'] q_pred = data['q_pred'] f_orient_cost, f_smoothness_boundary, f_smoothness_vel, f_smoothness_acc, f_smoothness_jerk, f_pos_cost, f_mid_cost = cost_fun( q_pred, params, other_params) f_orient_cost_new, f_smoothness_boundary_new, f_smoothness_vel_new, f_smoothness_acc_new, f_smoothness_jerk_new, f_pos_cost_new, f_mid_cost_new = cost_fun( q_new, params, other_params) param_des = params.copy() xmid, ymid, zmid = get_mid_point(q_new, other_params['mid_index']) param_new = stack_x(xmid, ymid, zmid) xmid, ymid, zmid = get_mid_point(q_pred, other_params['mid_index']) param_pred = stack_x(xmid, ymid, zmid) l_inf_new = max(abs(param_new - param_des)) l_inf_pred = max(abs(param_pred - param_des)) ratio_metric = l_inf_pred / l_inf_new roll_pred, pitch_pred = utils.get_roll_pitch(q_pred) roll_new, pitch_new = utils.get_roll_pitch(q_new) all_list.append([i, data['cost_3'], data['cost_2'], np.linalg.norm(q_new-q_pred), \ f_smoothness_boundary, f_smoothness_boundary_new, f_orient_cost, f_orient_cost_new, \ f_smoothness_vel, f_smoothness_vel_new, f_pos_cost, f_pos_cost_new, \ f_mid_cost, f_mid_cost_new, l_inf_pred, l_inf_new, ratio_metric, max(abs(roll_new-roll_pred)), max(abs(pitch_new-pitch_pred))]) except: pass column_names = [ "traj_num", "best_cost", "actual_cost", "best_norm", "best_f_boundary", "actual_f_boundary", "best_f_orient", "actual_f_orient", "best_f_vel", "actual_f_vel", "best_f_pos_cost", "actual_f_pos_cost", "best_f_mid_cost", "actual_f_mid_cost", "param_res_best", "param_res_actual", "ratio_metric", "roll_abs_diff", "pitch_abs_diff" ] df = pd.DataFrame(all_list, columns=column_names) df['cost_diff'] = df.apply(get_cost_diff, axis=1) df['boundary_diff'] = df.apply(get_pos_cost_diff, axis=1) df['orient_diff'] = df.apply(get_orient_cost_diff, axis=1) df['vel_diff'] = df.apply(get_vel_smoothness_cost_diff, axis=1) df['pos_diff'] = df.apply(get_pos_cost_diff_new, axis=1) df['mid_diff'] = df.apply(get_mid_cost_diff, axis=1) if (save): if not os.path.exists(OUTPUT_FOLDER): os.makedirs(OUTPUT_FOLDER) # a = df['roll_abs_diff'].to_numpy() # print("Roll : ", np.count_nonzero(a<0.1), " Total: ", a.shape) # b = df['pitch_abs_diff'].to_numpy() # print("Pitch : ", np.count_nonzero(b<0.1), " Total: ", b.shape) # c = df['vel_diff'].to_numpy() # print(np.mean(c)) # print("Vel cost : ", np.count_nonzero(c<0.01348), " Total: ", c.shape) # e1 = df['ratio_metric'].to_numpy() # print("ratio metric : ", np.count_nonzero(e1<1.2), " Total: ", e1.shape) # v1 = df['actual_f_vel'].to_numpy() # v2 = df['best_f_vel'].to_numpy() # v3 = np.concatenate((v1,v2)) # print("Mean cost : ", np.mean(v3), " no of trajs: ", 2*a.shape[0]) # print(v3.shape) # roll_abs_diff fig, axs = plt.subplots(1, 1, figsize=(6, 6)) _ = axs.hist(df['roll_abs_diff'].tolist(), bins=10, color=colors[0]) _ = axs.grid(ls='-.', lw=1) _ = axs.set_xlabel('$L_{\infty}$ of Roll Difference (rad)') _ = axs.set_ylabel('No. of Trajectories') # axs.xaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0e')) # _ = axs.set_xlim([-0.25, 3.0]) # _ = axs.set_ylim([0, 130]) # _ = axs.set_yticks(np.arange(0, 61, 10)) plt.savefig(OUTPUT_FOLDER + 'roll_cost.pdf', bbox_inches='tight', dpi=200) plt.close() # pitch_abs_diff fig, axs = plt.subplots(1, 1, figsize=(6, 6)) _ = axs.hist(df['pitch_abs_diff'].tolist(), bins=10, color=colors[0]) _ = axs.grid(ls='-.', lw=1) _ = axs.set_xlabel('$L_{\infty}$ of Pitch Difference (rad)') _ = axs.set_ylabel('No. of Trajectories') # axs.xaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0e')) # _ = axs.set_xlim([-0.25, 3.0]) # _ = axs.set_ylim([0, 110]) # _ = axs.set_yticks(np.arange(0, 61, 10)) plt.savefig(OUTPUT_FOLDER + 'pitch_cost.pdf', bbox_inches='tight', dpi=200) plt.close() # velocity_diff fig, axs = plt.subplots(1, 1, figsize=(6, 6)) _ = axs.hist(df['vel_diff'].tolist(), bins=10, color=colors[0]) _ = axs.grid(ls='-.', lw=1) _ = axs.set_xlabel('Smoothness Cost Difference') _ = axs.set_ylabel('No. of Trajectories') # _ = axs.set_xticks(np.arange(0.0, 0.004, 0.001)) # axs.xaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0e')) # _ = axs.set_ylim([0, 150]) # _ = axs.set_yticks(np.arange(0, 61, 10)) plt.savefig(OUTPUT_FOLDER + 'vel_cost.pdf', bbox_inches='tight', dpi=200) plt.close() # boundary_diff fig, axs = plt.subplots(1, 1, figsize=(6, 6)) _ = axs.hist(df['ratio_metric'].tolist(), bins=10, color=colors[0]) # _ = axs.hist(df['param_res_actual'].tolist(), bins=10, color=colors[1], alpha=0.4, label='Solver') _ = axs.grid(ls='-.', lw=1) _ = axs.set_xlabel('Residual Ratio of Via-Point. ($L_{\infty}$)') _ = axs.set_ylabel('No. of Trajectories') # _ = axs.set_xticks(np.arange(0.0, 0.02, 0.005)) # axs.xaxis.set_major_formatter(ticker.FormatStrFormatter('%0.0e')) # _ = axs.set_ylim([0, 150]) # _ = axs.set_yticks(np.arange(0, 61, 10)) plt.savefig(OUTPUT_FOLDER + 'param_cost.pdf', bbox_inches='tight', dpi=200) plt.close() return df