from casadi import *
T = 10
N = 20#dt = 0.2s
opti = Opti()
X = opti.variable(2,N+1)
# print(X)
U = opti.variable(1,N)
x0 = X[0,:]
x1 = X[1,:]
f = lambda x,u:vertcat( ((1-x[1])**2*x[0]-x[1]+u[0]),x[1])#x' = f(x,u)
dt = T/N
J = 0
X[0,0]==0
X[1,0]==1
for k in range(N):
k1 = f(X[:,k], U[:,k])
k2 = f(X[:,k]+dt/2*k1, U[:,k])
k3 = f(X[:,k]+dt/2*k2, U[:,k])
k4 = f(X[:,k]+dt*k3, U[:,k])
x_next = X[:,k] + dt/6*(k1+2*k2+2*k3+k4)
opti.subject_to(X[:,k+1]==x_next)
J = J + X[0,k]**2+X[1,k]**2+U[0,k]**2
opti.minimize(J)
opti.subject_to(x1>=-0.25)
opti.subject_to(x0[0] == 0)
opti.subject_to(x1[0] == 1)
opti.set_initial(U,0)
opti.set_initial(x0,0)
opti.set_initial(x1,0)
opti.subject_to(opti.bounded(-1,U,1))
opti.solver("ipopt")
print(opti)
sol = opti.solve()