# debugging:
# ------------------------------------------------------------------------------
# create a dictionary for data logging
traj_data = {}
traj_data['zmp_reference'] = Z_ref
traj_data['X'] = X
traj_data['Y'] = Y
traj_data['Z_x'] = Z_x
traj_data['Z_y'] = Z_y
# ------------------------------------------------------------------------------
# visualize:
# ------------------------------------------------------------------------------
time = np.arange(0, round(walking_time*T, 2), T)
min_admissible_CoP = Z_ref - np.tile([foot_length/2, foot_width/2], (walking_time,1))
max_admissible_cop = Z_ref + np.tile([foot_length/2, foot_width/2], (walking_time,1))
# time vs CoP and CoM in x-direction (forward): 'A.K.A run rabbit run !'
# -------------------------------------------------
plot_utils.plot_x(time, walking_time, min_admissible_CoP, max_admissible_cop, \
Z_x, X, Z_ref)
# time VS CoP and CoM in y-direction (lateral): 'A.K.A what goes up must go down'
# ----------------------------------------------------------
plot_utils.plot_y(time, walking_time, min_admissible_CoP, max_admissible_cop, \
Z_y, Y, Z_ref)
# plot CoP, CoM in x Vs Cop, CoM in y:
# -----------------------------------
plot_utils.plot_xy(time, walking_time, foot_length, foot_width, Z_ref, \
Z_x, Z_y, X, Y)
#print 'y_hat_k = ', y_hat_k[0], '\n'
#print 'y_hatdot_k = ', Y_k[0,1], '\n'
#print 'y_hatddot_k = ', Y_k[0,2], '\n'
#print 'Z_x_k = ', Z_x_k[0], '\n'
#print 'Z_y_k = ', Z_y_k[0], '\n'
#print 'Z_ref_k = ', Z_ref_k
# ------------------------------------------------------------------------------
# visualize your final trajectory:
# ------------------------------------------------------------------------------
time = np.arange(0, round(desired_walking_time*T, 2), T)
min_admissible_CoP = desired_Z_ref - np.tile([foot_length/2, foot_width/2], (desired_walking_time,1))
max_admissible_cop = desired_Z_ref + np.tile([foot_length/2, foot_width/2], (desired_walking_time,1))
# time vs CoP and CoM in x: 'A.K.A run rabbit run !'
# -------------------------------------------------
plot_utils.plot_x(time, desired_walking_time, min_admissible_CoP, max_admissible_cop, \
Z_x_total, X_total, desired_Z_ref)
# time VS CoP and CoM in y: 'A.K.A what goes up must go down'
# ----------------------------------------------------------
plot_utils.plot_y(time, desired_walking_time, min_admissible_CoP, max_admissible_cop, \
Z_y_total, Y_total, desired_Z_ref)
# plot CoP, CoM in x Vs Cop, CoM in y:
# -----------------------------------
plot_utils.plot_xy(time, desired_walking_time, foot_length, foot_width, desired_Z_ref, \
Z_x_total, Z_y_total, X_total, Y_total)