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