12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

formatted mpc class with black

master
mcarfagno 2022-07-22 16:07:47 +01:00
parent 936dcd3642
commit 04609f8cf6
1 changed files with 79 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
mpc_pybullet_demo/mpcpy/cvxpy_mpc.py
View File

@ -1,5 +1,6 @@
import numpy as np import numpy as np
np.seterr(divide='ignore', invalid='ignore')
np.seterr(divide="ignore", invalid="ignore")
from scipy.integrate import odeint from scipy.integrate import odeint
from scipy.interpolate import interp1d from scipy.interpolate import interp1d
@ -8,61 +9,78 @@ import cvxpy as opt
from .utils import * from .utils import *
from .mpc_config import Params from .mpc_config import Params
P=Params()
def get_linear_model_matrices(x_bar,u_bar): P = Params()
def get_linear_model_matrices(x_bar, u_bar):
""" """
Computes the LTI approximated state space model x' = Ax + Bu + C Computes the LTI approximated state space model x' = Ax + Bu + C
""" """
x = x_bar[0] x = x_bar[0]
y = x_bar[1] y = x_bar[1]
v = x_bar[2] v = x_bar[2]
theta = x_bar[3] theta = x_bar[3]
a = u_bar[0] a = u_bar[0]
delta = u_bar[1] delta = u_bar[1]
ct = np.cos(theta) ct = np.cos(theta)
st = np.sin(theta) st = np.sin(theta)
cd = np.cos(delta) cd = np.cos(delta)
td = np.tan(delta) td = np.tan(delta)
A = np.zeros((P.N,P.N)) A = np.zeros((P.N, P.N))
A[0,2] = ct A[0, 2] = ct
A[0,3] = -v*st A[0, 3] = -v * st
A[1,2] = st A[1, 2] = st
A[1,3] = v*ct A[1, 3] = v * ct
A[3,2] = v*td/P.L A[3, 2] = v * td / P.L
A_lin = np.eye(P.N)+P.DT*A A_lin = np.eye(P.N) + P.DT * A
B = np.zeros((P.N,P.M)) B = np.zeros((P.N, P.M))
B[2,0]=1 B[2, 0] = 1
B[3,1]=v/(P.L*cd**2) B[3, 1] = v / (P.L * cd**2)
B_lin=P.DT*B B_lin = P.DT * B
f_xu=np.array([v*ct, v*st, a, v*td/P.L]).reshape(P.N,1) f_xu = np.array([v * ct, v * st, a, v * td / P.L]).reshape(P.N, 1)
C_lin = P.DT*(f_xu - np.dot(A, x_bar.reshape(P.N,1)) - np.dot(B, u_bar.reshape(P.M,1))).flatten() C_lin = (
P.DT
#return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6) * (
f_xu - np.dot(A, x_bar.reshape(P.N, 1)) - np.dot(B, u_bar.reshape(P.M, 1))
).flatten()
)
# return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)
return A_lin, B_lin, C_lin return A_lin, B_lin, C_lin
class MPC():
class MPC:
def __init__(self, N, M, Q, R): def __init__(self, N, M, Q, R):
""" """ """
"""
self.state_len = N self.state_len = N
self.action_len = M self.action_len = M
self.state_cost = Q self.state_cost = Q
self.action_cost = R self.action_cost = R
def optimize_linearized_model(self, A, B, C, initial_state, target, time_horizon=10, Q=None, R=None, verbose=False): def optimize_linearized_model(
self,
A,
B,
C,
initial_state,
target,
time_horizon=10,
Q=None,
R=None,
verbose=False,
):
""" """
Optimisation problem defined for the linearised model, Optimisation problem defined for the linearised model,
:param A: :param A:
:param B: :param B:
:param C: :param C:
:param initial_state: :param initial_state:
:param Q: :param Q:
:param R: :param R:
@ -71,47 +89,53 @@ class MPC():
:param verbose: :param verbose:
:return: :return:
""" """
assert len(initial_state) == self.state_len assert len(initial_state) == self.state_len
if (Q == None or R==None): if Q == None or R == None:
Q = self.state_cost Q = self.state_cost
R = self.action_cost R = self.action_cost
# Create variables # Create variables
x = opt.Variable((self.state_len, time_horizon + 1), name='states') x = opt.Variable((self.state_len, time_horizon + 1), name="states")
u = opt.Variable((self.action_len, time_horizon), name='actions') u = opt.Variable((self.action_len, time_horizon), name="actions")
# Loop through the entire time_horizon and append costs # Loop through the entire time_horizon and append costs
cost_function = [] cost_function = []
for t in range(time_horizon): for t in range(time_horizon):
_cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) +\ _cost = opt.quad_form(target[:, t + 1] - x[:, t + 1], Q) + opt.quad_form(
opt.quad_form(u[:, t], R) u[:, t], R
)
_constraints = [x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,
u[0, t] >= -P.MAX_ACC, u[0, t] <= P.MAX_ACC, _constraints = [
u[1, t] >= -P.MAX_STEER, u[1, t] <= P.MAX_STEER] x[:, t + 1] == A @ x[:, t] + B @ u[:, t] + C,
#opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1] u[0, t] >= -P.MAX_ACC,
u[0, t] <= P.MAX_ACC,
u[1, t] >= -P.MAX_STEER,
u[1, t] <= P.MAX_STEER,
]
# opt.norm(target[:, t + 1] - x[:, t + 1], 1) <= 0.1]
# Actuation rate of change # Actuation rate of change
if t < (time_horizon - 1): if t < (time_horizon - 1):
_cost += opt.quad_form(u[:,t + 1] - u[:,t], R * 1) _cost += opt.quad_form(u[:, t + 1] - u[:, t], R * 1)
_constraints += [opt.abs(u[0, t + 1] - u[0, t])/P.DT <= P.MAX_D_ACC] _constraints += [opt.abs(u[0, t + 1] - u[0, t]) / P.DT <= P.MAX_D_ACC]
_constraints += [opt.abs(u[1, t + 1] - u[1, t])/P.DT <= P.MAX_D_STEER] _constraints += [opt.abs(u[1, t + 1] - u[1, t]) / P.DT <= P.MAX_D_STEER]
if t == 0: if t == 0:
#_constraints += [opt.norm(target[:, time_horizon] - x[:, time_horizon], 1) <= 0.01, # _constraints += [opt.norm(target[:, time_horizon] - x[:, time_horizon], 1) <= 0.01,
# x[:, 0] == initial_state] # x[:, 0] == initial_state]
_constraints += [x[:, 0] == initial_state] _constraints += [x[:, 0] == initial_state]
cost_function.append(opt.Problem(opt.Minimize(_cost), constraints=_constraints)) cost_function.append(
opt.Problem(opt.Minimize(_cost), constraints=_constraints)
)
# Add final cost # Add final cost
problem = sum(cost_function) problem = sum(cost_function)
# Minimize Problem # Minimize Problem
problem.solve(verbose=verbose, solver=opt.OSQP) problem.solve(verbose=verbose, solver=opt.OSQP)
return x, u return x, u