mpc_python_learn/mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py

170 lines
5.4 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
import cvxpy as opt
2019-12-17 18:37:32 +08:00
2022-07-22 23:07:47 +08:00
class MPC:
2023-10-24 03:58:21 +08:00
def __init__(
self, vehicle, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost
):
2023-10-21 02:20:11 +08:00
"""
2023-04-23 23:44:43 +08:00
2023-10-25 21:17:25 +08:00
Args:
vehicle ():
T ():
DT ():
state_cost ():
final_state_cost ():
input_cost ():
input_rate_cost ():
"""
2023-10-24 03:58:21 +08:00
self.nx = 4 # number of state vars
self.nu = 2 # umber of input/control vars
if len(state_cost) != self.nx:
raise ValueError(f"State Error cost matrix shuld be of size {self.nx}")
if len(final_state_cost) != self.nx:
raise ValueError(f"End State Error cost matrix shuld be of size {self.nx}")
if len(input_cost) != self.nu:
raise ValueError(f"Control Effort cost matrix shuld be of size {self.nu}")
if len(input_rate_cost) != self.nu:
2023-04-23 23:44:43 +08:00
raise ValueError(
f"Control Effort Difference cost matrix shuld be of size {self.nu}"
2023-04-23 23:44:43 +08:00
)
2023-10-24 03:58:21 +08:00
self.vehicle = vehicle
self.dt = DT
self.control_horizon = int(T / DT)
2023-04-23 23:44:43 +08:00
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)
2022-07-22 23:07:47 +08:00
2023-10-24 02:27:46 +08:00
def get_linear_model_matrices(self, x_bar, u_bar):
"""
2023-10-28 17:47:35 +08:00
Computes the approximated LTI state space model x' = Ax + Bu + C
2023-10-25 21:17:25 +08:00
Args:
2023-10-28 17:47:35 +08:00
x_bar (array-like):
u_bar (array-like):
2023-10-25 21:17:25 +08:00
Returns:
2023-10-24 02:27:46 +08:00
"""
x = x_bar[0]
y = x_bar[1]
v = x_bar[2]
theta = x_bar[3]
a = u_bar[0]
delta = u_bar[1]
ct = np.cos(theta)
st = np.sin(theta)
cd = np.cos(delta)
td = np.tan(delta)
A = np.zeros((self.nx, self.nx))
A[0, 2] = ct
A[0, 3] = -v * st
A[1, 2] = st
A[1, 3] = v * ct
2023-10-24 03:58:21 +08:00
A[3, 2] = v * td / self.vehicle.wheelbase
2023-10-24 02:27:46 +08:00
A_lin = np.eye(self.nx) + self.dt * A
2023-10-24 03:58:21 +08:00
B = np.zeros((self.nx, self.nu))
2023-10-24 02:27:46 +08:00
B[2, 0] = 1
2023-10-24 03:58:21 +08:00
B[3, 1] = v / (self.vehicle.wheelbase * cd**2)
2023-10-24 02:27:46 +08:00
B_lin = self.dt * B
2023-10-24 03:58:21 +08:00
f_xu = np.array([v * ct, v * st, a, v * td / self.vehicle.wheelbase]).reshape(
self.nx, 1
)
2023-10-24 02:27:46 +08:00
C_lin = (
self.dt
* (
f_xu
- np.dot(A, x_bar.reshape(self.nx, 1))
- np.dot(B, u_bar.reshape(self.nu, 1))
).flatten()
)
return A_lin, B_lin, C_lin
def step(
2022-07-22 23:07:47 +08:00
self,
initial_state,
target,
2023-10-24 02:27:46 +08:00
prev_cmd,
2022-07-22 23:07:47 +08:00
verbose=False,
):
"""
2022-07-22 23:07:47 +08:00
2023-10-25 21:17:25 +08:00
Args:
2023-10-28 17:47:35 +08:00
initial_state (array-like): current estimate of [x, y, v, heading]
target (ndarray): state space reference, in the same frame as the provided current state
prev_cmd (array-like): previous [acceleration, steer]. note this is used in bounds and has to be realistic.
verbose (bool):
2023-10-25 21:17:25 +08:00
Returns:
"""
assert len(initial_state) == self.nx
2023-10-28 17:47:35 +08:00
assert len(prev_cmd) == self.nu
assert target.shape == (self.nx, self.control_horizon)
2022-07-22 23:07:47 +08:00
2023-10-24 18:46:56 +08:00
# Create variables needed for setting up cvxpy problem
x = opt.Variable((self.nx, self.control_horizon + 1), name="states")
u = opt.Variable((self.nu, self.control_horizon), name="actions")
2023-04-22 00:16:46 +08:00
cost = 0
constr = []
2023-10-24 18:46:56 +08:00
# NOTE: here the state linearization is performed around the starting condition to simplify the controller.
# This approximation gets more inaccurate as the controller looks at the future.
# To improve performance we can keep track of previous optimized x, u and compute these matrices for each timestep k
# Ak, Bk, Ck = self.get_linear_model_matrices(x_prev[:,k], u_prev[:,k])
2023-10-24 02:27:46 +08:00
A, B, C = self.get_linear_model_matrices(initial_state, prev_cmd)
2023-10-26 00:04:35 +08:00
# Tracking error cost
2023-04-23 23:44:43 +08:00
for k in range(self.control_horizon):
2023-10-26 00:04:35 +08:00
cost += opt.quad_form(x[:, k + 1] - target[:, k], self.Q)
2023-10-26 00:04:35 +08:00
# Final point tracking cost
cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
2022-07-22 23:07:47 +08:00
2023-10-26 00:04:35 +08:00
# Actuation magnitude cost
for k in range(self.control_horizon):
cost += opt.quad_form(u[:, k], self.R)
2022-07-22 23:07:47 +08:00
2023-10-26 00:04:35 +08:00
# Actuation rate of change cost
for k in range(1, self.control_horizon):
cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P)
2022-07-22 23:07:47 +08:00
2023-10-26 00:04:35 +08:00
# Kinematics Constrains
for k in range(self.control_horizon):
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
# initial state
constr += [x[:, 0] == initial_state]
2023-10-24 03:58:21 +08:00
# actuation bounds
constr += [opt.abs(u[:, 0]) <= self.vehicle.max_acc]
constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer]
2022-07-22 23:07:47 +08:00
2023-10-26 00:04:35 +08:00
# Actuation rate of change bounds
constr += [opt.abs(u[0, 0] - prev_cmd[0]) / self.dt <= self.vehicle.max_d_acc]
constr += [opt.abs(u[1, 0] - prev_cmd[1]) / self.dt <= self.vehicle.max_d_steer]
for k in range(1, self.control_horizon):
constr += [
opt.abs(u[0, k] - u[0, k - 1]) / self.dt <= self.vehicle.max_d_acc
]
constr += [
opt.abs(u[1, k] - u[1, k - 1]) / self.dt <= self.vehicle.max_d_steer
]
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)
return x, u