# q_0_vec = np.matlib.linspace(0,np.pi,N_tot+1).reshape(-1,1)
# q_0_vec = np.hstack((q_0_vec,np.zeros([N_tot+1,1]))) #.reshape(-1,1).flatten().tolist()
# print q_0
# print np.shape(q_0_vec)
# q_0_vec = np.zeros([N_tot+1,nj])
# print q_0_vec
# q_0_vec = np.ones([N+1,nj]) * q_0
qdot_0 = np.zeros(nj).tolist()
qddot_0 = np.zeros(nj).tolist()
# qddot_0 = np.ones(nj,dtype=float)* 0.5
# qddot_0 = qddot_0.tolist()
# HOMING TO INITIAL POSITION
if var_ani:
# TODO: create functionality to choose homing position, default q = 0
init_state.homing(q_0)
# TERMINAL CONDITIONS (NUMERICAL)
J01_pos = [0.0, 0.0, 1.2]
offsetX = 0.0800682
offsetY = 0
offsetZ = 1.2
desX = 0
desY = 0.797
# desY = 0.780
# desY = 0.7
# desY = 0.5
# desY = 0.65
desZ = 0
# desZ = -0.4
# desY = 0.5
import joint_state as js
import init_state
import tameshiwari.pynocchio_casadi as pyn
# =============================================================================
# INITIALIZATION
# =============================================================================
var_pl = True
var_ani = True
var_rec = False
var_inp = False
var_save = False
var_h_opt = False
if var_ani:
init_state.homing()
# =============================================================================
# WEIGHTENING
# =============================================================================
# DEFAULT VALUES
W = 0.2
W_h = 1.0
# TODO: Change the weighenings to have effect on the proper values
# USER INPUTS
if var_inp:
print "================ USER INPUT ================"
usr_h_opt = raw_input("Do you want to optimze for Time? (y/N): ")
if usr_h_opt != "":
try: