mpc_python_learn/mpc_pybullet_demo/cvxpy_mpc.py

115 lines
3.0 KiB
Python
Raw Normal View History

2019-12-17 18:37:32 +08:00
import numpy as np
2020-03-04 20:32:29 +08:00
np.seterr(divide='ignore', invalid='ignore')
2019-12-17 18:37:32 +08:00
from scipy.integrate import odeint
from scipy.interpolate import interp1d
import cvxpy as cp
2020-07-01 00:21:27 +08:00
from utils import road_curve, f, df
from mpc_config import Params
P=Params()
2019-12-17 18:37:32 +08:00
def get_linear_model(x_bar,u_bar):
"""
"""
2020-07-01 22:59:13 +08:00
L=0.3
2019-12-17 18:37:32 +08:00
x = x_bar[0]
y = x_bar[1]
2020-07-01 22:59:13 +08:00
v = x_bar[2]
theta = x_bar[3]
2020-01-13 20:25:26 +08:00
2020-07-01 22:59:13 +08:00
a = u_bar[0]
delta = u_bar[1]
2020-01-13 20:25:26 +08:00
2020-07-01 00:21:27 +08:00
A = np.zeros((P.N,P.N))
2020-07-01 22:59:13 +08:00
A[0,2]=np.cos(theta)
A[0,3]=-v*np.sin(theta)
A[1,2]=np.sin(theta)
A[1,3]=v*np.cos(theta)
A[3,2]=v*np.tan(delta)/L
2020-07-01 00:21:27 +08:00
A_lin=np.eye(P.N)+P.dt*A
2020-01-13 20:25:26 +08:00
2020-07-01 00:21:27 +08:00
B = np.zeros((P.N,P.M))
2020-07-01 22:59:13 +08:00
B[2,0]=1
B[3,1]=v/(L*np.cos(delta)**2)
2020-07-01 00:21:27 +08:00
B_lin=P.dt*B
2020-01-13 20:25:26 +08:00
2020-07-01 22:59:13 +08:00
f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(P.N,1)
2020-07-01 00:21:27 +08:00
C_lin = P.dt*(f_xu - np.dot(A,x_bar.reshape(P.N,1)) - np.dot(B,u_bar.reshape(P.M,1)))
2020-01-13 20:25:26 +08:00
2020-07-01 22:59:13 +08:00
return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)
2019-12-17 18:37:32 +08:00
2020-07-01 22:59:13 +08:00
def optimize(state,u_bar,track,ref_vel=1.):
2019-12-17 18:37:32 +08:00
'''
2020-07-01 00:21:27 +08:00
:param state:
2019-12-17 18:37:32 +08:00
:param u_bar:
:param track:
:returns:
'''
2020-07-06 23:54:22 +08:00
MAX_SPEED = ref_vel*1.5
MAX_STEER = np.pi/4
2020-07-01 22:59:13 +08:00
MAX_ACC = 1.0
2019-12-17 18:37:32 +08:00
2020-07-01 00:21:27 +08:00
# compute polynomial coefficients of the track
K=road_curve(state,track)
2019-12-17 18:37:32 +08:00
2020-07-01 00:21:27 +08:00
# dynamics starting state w.r.t vehicle frame
x_bar=np.zeros((P.N,P.T+1))
2020-07-01 22:59:13 +08:00
x_bar[2,0]=state[2]
2019-12-17 18:37:32 +08:00
2020-07-01 00:21:27 +08:00
#prediction for linearization of costrains
for t in range (1,P.T+1):
xt=x_bar[:,t-1].reshape(P.N,1)
ut=u_bar[:,t-1].reshape(P.M,1)
2019-12-17 18:37:32 +08:00
A,B,C=get_linear_model(xt,ut)
xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
x_bar[:,t]= xt_plus_one
#CVXPY Linear MPC problem statement
cost = 0
constr = []
2020-07-01 00:21:27 +08:00
x = cp.Variable((P.N, P.T+1))
u = cp.Variable((P.M, P.T))
2019-12-17 18:37:32 +08:00
2020-07-01 00:21:27 +08:00
for t in range(P.T):
2020-01-13 20:25:26 +08:00
2020-07-06 23:54:22 +08:00
cost += 20*cp.sum_squares(x[3,t]-np.clip(np.arctan(df(x_bar[0,t],K)),-np.pi,np.pi) ) # psi
cost += 40*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte
cost += 20*cp.sum_squares(ref_vel-x[2,t]) # desired v
2019-12-17 18:37:32 +08:00
# Actuation rate of change
2020-07-01 00:21:27 +08:00
if t < (P.T - 1):
2020-07-01 22:59:13 +08:00
cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(P.M))
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
# Actuation effort
2020-07-01 22:59:13 +08:00
cost += cp.quad_form( u[:, t],10*np.eye(P.M))
2020-01-13 20:25:26 +08:00
2020-07-01 00:21:27 +08:00
# Kinrmatics Constrains (Linearized model)
2019-12-17 18:37:32 +08:00
A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])
2020-07-01 00:21:27 +08:00
constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]
2019-12-17 18:37:32 +08:00
# sums problem objectives and concatenates constraints.
2020-07-01 00:21:27 +08:00
constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition
2020-07-01 22:59:13 +08:00
constr += [x[2, :] <= MAX_SPEED]
constr += [x[2, :] >= 0.0]
constr += [cp.abs(u[0, :]) <= MAX_ACC]
constr += [cp.abs(u[1, :]) <= MAX_STEER]
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
# Solve
prob = cp.Problem(cp.Minimize(cost), constr)
2020-07-06 23:54:22 +08:00
prob.solve(solver=cp.OSQP, verbose=False)
if "optimal" not in prob.status:
print("WARN: No optimal solution")
return u_bar
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
#retrieved optimized U and assign to u_bar to linearize in next step
2020-07-06 23:54:22 +08:00
u_opt=np.vstack((np.array(u.value[0, :]).flatten(),
2019-12-17 18:37:32 +08:00
(np.array(u.value[1, :]).flatten())))
2020-01-13 20:25:26 +08:00
2020-07-06 23:54:22 +08:00
return u_opt