mpc_python_learn/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py

147 lines
4.0 KiB
Python
Raw Normal View History

2019-12-17 18:37:32 +08:00
import numpy as np
2022-07-22 23:07:47 +08:00
np.seterr(divide="ignore", invalid="ignore")
2020-03-04 20:32:29 +08:00
2019-12-17 18:37:32 +08:00
from scipy.integrate import odeint
from scipy.interpolate import interp1d
import cvxpy as opt
2019-12-17 18:37:32 +08:00
2021-07-08 19:54:48 +08:00
from .utils import *
2020-07-01 00:21:27 +08:00
2021-07-08 19:54:48 +08:00
from .mpc_config import Params
2020-07-01 00:21:27 +08:00
2022-07-22 23:07:47 +08:00
P = Params()
def get_linear_model_matrices(x_bar, u_bar):
2019-12-17 18:37:32 +08:00
"""
Computes the LTI approximated state space model x' = Ax + Bu + C
2019-12-17 18:37:32 +08:00
"""
2022-07-22 23:07:47 +08:00
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]
2022-07-22 23:07:47 +08:00
2020-07-01 22:59:13 +08:00
a = u_bar[0]
delta = u_bar[1]
2022-07-22 23:07:47 +08:00
ct = np.cos(theta)
st = np.sin(theta)
cd = np.cos(delta)
td = np.tan(delta)
2022-07-22 23:07:47 +08:00
A = np.zeros((P.N, P.N))
A[0, 2] = ct
A[0, 3] = -v * st
A[1, 2] = st
A[1, 3] = v * ct
A[3, 2] = v * td / P.L
A_lin = np.eye(P.N) + P.DT * A
B = np.zeros((P.N, P.M))
B[2, 0] = 1
B[3, 1] = v / (P.L * cd**2)
B_lin = P.DT * B
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()
)
# return np.round(A_lin,6), np.round(B_lin,6), np.round(C_lin,6)
return A_lin, B_lin, C_lin
2019-12-17 18:37:32 +08:00
2022-07-22 23:07:47 +08:00
class MPC:
2023-04-23 23:44:43 +08:00
def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost):
2022-07-22 23:07:47 +08:00
""" """
2023-04-23 23:44:43 +08:00
nx = P.N # number of state vars
nu = P.M # umber of input/control vars
if len(state_cost) != nx:
raise ValueError(f"State Error cost matrix shuld be of size {nx}")
if len(final_state_cost) != nx:
raise ValueError(f"End State Error cost matrix shuld be of size {nx}")
if len(input_cost) != nu:
raise ValueError(f"Control Effort cost matrix shuld be of size {nu}")
if len(input_rate_cost) != nu:
raise ValueError(
f"Control Effort Difference cost matrix shuld be of size {nu}"
)
self.dt = P.DT
self.control_horizon = P.T
self.Q = np.diag(state_cost)
self.Qf = np.diag(final_state_cost)
self.R = np.diag(input_cost)
self.P = np.diag(input_rate_cost)
2023-04-22 00:16:46 +08:00
self.u_bounds = np.array([P.MAX_ACC, P.MAX_STEER])
self.du_bounds = np.array([P.MAX_D_ACC, P.MAX_D_STEER])
2022-07-22 23:07:47 +08:00
def optimize_linearized_model(
self,
A,
B,
C,
initial_state,
target,
verbose=False,
):
"""
Optimisation problem defined for the linearised model,
2022-07-22 23:07:47 +08:00
:param A:
:param B:
2022-07-22 23:07:47 +08:00
:param C:
:param initial_state:
:param target:
:param verbose:
:return:
"""
2022-07-22 23:07:47 +08:00
assert len(initial_state) == self.state_len
2022-07-22 23:07:47 +08:00
# Create variables
2023-04-22 00:16:46 +08:00
x = opt.Variable((self.state_len, control_horizon + 1), name="states")
u = opt.Variable((self.action_len, control_horizon), name="actions")
cost = 0
constr = []
2023-04-23 23:44:43 +08:00
for k in range(self.control_horizon):
cost += opt.quad_form(target[:, k] - x[:, k], self.Q)
cost += opt.quad_form(u[:, k], self.R)
2023-04-22 00:16:46 +08:00
# Actuation rate of change
if k < (control_horizon - 1):
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
2022-07-22 23:07:47 +08:00
2023-04-22 00:16:46 +08:00
# Kinematics Constrains
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
2022-07-22 23:07:47 +08:00
2023-04-22 00:16:46 +08:00
# Actuation rate of change limit
if t < (control_horizon - 1):
2023-04-23 23:44:43 +08:00
constr += [
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.du_bounds[0]
]
constr += [
opt.abs(u[1, k + 1] - u[1, k]) / self.dt <= self.du_bounds[1]
]
2022-07-22 23:07:47 +08:00
2023-04-22 00:16:46 +08:00
# Final Point tracking
2023-04-23 23:44:43 +08:00
cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
2022-07-22 23:07:47 +08:00
2023-04-22 00:16:46 +08:00
# initial state
constr += [x[:, 0] == initial_state]
2023-04-22 00:16:46 +08:00
# actuation magnitude
constr += [opt.abs(u[:, 0]) <= self.u_bounds[0]]
constr += [opt.abs(u[:, 1]) <= self.u_bounds[1]]
2022-07-22 23:07:47 +08:00
2023-04-22 00:16:46 +08:00
prob = opt.Problem(opt.Minimize(cost), constr)
solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
2023-04-22 00:24:28 +08:00
return u[:, 0].value