def getFullMpcOutput(self, state):
# finish iter
X = np.zeros((self.horizon + 1, self.NX))
U = np.zeros((self.horizon, self.NU))
prev_x = np.zeros((self.horizon + 1, self.NX))
ref_traj, terminal_state = self.getReference(state)
for i in range(self.max_iteration):
X, U = acado.mpc(0, 1, self.toAcadoState(state), X, U, ref_traj,
terminal_state,
np.transpose(np.tile(self.Q,
self.horizon)), self.Qf, 0)
if (np.linalg.norm(X - prev_x) < self.THRESHOLD):
# print("CONTROL: Input mpc terminating iteration at ", i)
break
prev_x = X #Update prev
return (X, U)
def linear_mpc_control(xref, xbar, x0, dref):
# see acado.c for parameter details
_x0=np.zeros((1, defs.NX))
X=np.zeros((defs.T+1, defs.NX))
U=np.zeros((defs.T, defs.NU))
Y=np.zeros((defs.T, defs.NY))
yN=np.zeros((1, defs.NYN))
_x0[0,:]=np.transpose(x0) # initial state
for t in range(defs.T):
Y[t,:] = np.transpose(xref[:,t]) # reference state
X[t,:] = np.transpose(xbar[:,t]) # predicted state
X[-1,:] = X[-2,:]
yN[0,:]=Y[-1,:defs.NYN] # reference terminal state
#print(Y.shape)
X, U = acado.mpc(0, 1, _x0, X,U,Y,yN, np.transpose(np.tile(defs.Q,defs.T)), defs.Qf, 0)
ox_mpc = utils.get_nparray_from_matrix(X[:,0])
oy_mpc = utils.get_nparray_from_matrix(X[:,1])
ov_mpc = utils.get_nparray_from_matrix(X[:,2])
oyaw_mpc = utils.get_nparray_from_matrix(X[:,3])
oa_mpc = utils.get_nparray_from_matrix(U[:,0])
ow_mpc = utils.get_nparray_from_matrix(U[:,1])
return oa_mpc, ow_mpc, ox_mpc, oy_mpc, oyaw_mpc, ov_mpc
def linear_mpc_control(xref, xbar, x0, dref):
# see acado.c for parameter details
_x0 = np.zeros((1, NX))
X = np.zeros((T + 1, NX))
U = np.zeros((T, NU))
Y = np.zeros((T, NY))
yN = np.zeros((1, NYN))
_x0[0, :] = np.transpose(x0) # initial state
for t in range(T):
Y[t, :] = np.transpose(xref[:, t]) # reference state
X[t, :] = np.transpose(xbar[:, t]) # predicted state
X[-1, :] = X[-2, :]
yN[0, :] = Y[-1, :NYN] # reference terminal state
X, U = acado.mpc(0, 1, _x0, X, U, Y, yN, np.transpose(np.tile(Q, T)), Qf,
0)
ox = get_nparray_from_matrix(X[:, 0])
oy = get_nparray_from_matrix(X[:, 1])
ov = get_nparray_from_matrix(X[:, 2])
oyaw = get_nparray_from_matrix(X[:, 3])
oa = get_nparray_from_matrix(U[:, 0])
odelta = get_nparray_from_matrix(U[:, 1])
return oa, odelta, ox, oy, oyaw, ov
def mpc(T, x0, y0, v0, yaw0, st0, tx, ty, X, U):
x0 = np.array( [x0, y0, v0, yaw0, st0] ).reshape( (1,NX) )
Y=np.zeros((T,NY))
yN=np.zeros((1,NY))
Q = np.diag([1.,1.]) # state cost matrix ([0.5, 0.5, 1.0, 1.0])
#Qf = np.diag([1.0, 1.0, 0.1, 1.0]) # state cost matrix
Qf = Q
## ? czemu nie przesuwamy o -1 ?
Y[:,0]=np.array(tx)
Y[:,1]=np.array(ty)
yN[0,0]=tx[T-1]
yN[0,1]=ty[T-1]
for i in range(1):
ax, ay, av, ayaw, ast = predict_motion(X, U, dt, T) #X, x0 ?
for j in range(T):
X[j,:] = ax[j], ay[j], av[j], ayaw[j], ast[j]
result, objective, X, U = acado.mpc(0, 1, x0,X,U,Y,yN, np.transpose(np.tile(Q,T)), Qf, 0)
print (result, objective)
return (objective, X, U)
Y = np.zeros((T, NY))
for i in range(0, T):
Y[i, 2] = -1
Y[i, 3] = 1
Y[i, 10] = 1
# Final value.
yN = np.zeros((1, NYN))
yN[0, 2] = -1
yN[0, 3] = 1
Q = np.diag([
100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 10.0, 10.0, 10.0, 1.0,
1.0, 1.0, 1.0
])
Qf = Q[:10, :10]
counter = 0
# x0 [1, NX]
# X [N+1, NX]
# U [N, NU]
# Y [N, NY]
# yN [1, NYN]
for i in range(0, 1000):
X, U = acado.mpc(0, 1, x0, X, U, Y, yN, np.transpose(np.tile(Q, T)), Qf, 0)
if counter % 1000 == 0:
print(counter)
counter = counter + 1
print('U', U)
print('X', X)