I tried trapezoidal collocation using to solve a double integrator control problem. I am doing in CVXPY. This is a two point boundary value problem. After solving there is a jump in control beyond limits to meet the end condition. Could not find reason. I have attached the code below. Thanks in advance
import numpy as np
from cvxpy import *
import matplotlib.pyplot as plt
np.random.seed(1)
n= 2
m = 1
T = 50
alpha = 0.02
beta = 5
A = np.zeros((2,2))
A[0,1] = 1
B= np.zeros((2,1))
B[1,0] = 1
dt = 0.02
x_0 = np.zeros(2)
x = Variable((2,T+1))
u = Variable((1,T+1))
cost = 0
constr =[]
for t in range(T):
cost += sum_squares(u[:,t])
constr += [x[0,t+1] == x[0,t] + 0.5*dt*(x[1,t+1]+x[1,t]) ]
constr += [x[1,t+1] == x[1,t] + 0.5*dt*(u[0,t+1]+u[0,t]) ]
constr +=[x[0,T]==1,x[1,T]==0]
constr +=[x[:,0]==x_0 ]
problem = Problem(Minimize(cost),constr)
#problem.solve()
#problem.solve(solver='SCS',eps=1e-5,max_iters=1000,verbose=True)
problem.solve(verbose=True, solver='ECOS')
f = plt.figure()
ax = f.add_subplot(411)
plt.plot(u[0,:].value)
plt.subplot(4,1,3)
x1 = x[0,:].value
plt.plot(x1)
plt.subplot(4,1,4)
x2 = x[1,:].value
plt.plot(x2)
plt.show()
