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

update notation

master
mcarfagno 2023-10-12 19:28:38 +01:00
parent 0abc666052
commit 279625b4c1
2 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
mpc_pybullet_demo/mpcpy/cvxpy_mpc.py
View File

@ -75,7 +75,7 @@ class MPC:
)
self.dt = P.DT
self.control_horizon = P.T
self.control_horizon = P.T / P.DT
self.Q = np.diag(state_cost)
self.Qf = np.diag(final_state_cost)
self.R = np.diag(input_cost)
@ -112,18 +112,18 @@ class MPC:
constr = []
for k in range(self.control_horizon):
cost += opt.quad_form(target[:, k] - x[:, k], self.Q)
cost += opt.quad_form(target[:, k] - x[:, k + 1], self.Q)
cost += opt.quad_form(u[:, k], self.R)
# Actuation rate of change
if k < (control_horizon - 1):
if k < (self.control_horizon - 1):
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
# Kinematics Constrains
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
# Actuation rate of change limit
if t < (control_horizon - 1):
if k < (self.control_horizon - 1):
constr += [
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.du_bounds[0]
]

20
mpc_pybullet_demo/mpcpy/mpc_config.py
View File

@ -5,13 +5,13 @@ class Params:
def __init__(self):
self.N = 4 # number of state variables
self.M = 2 # number of control variables
self.T = 10 # Prediction Horizon
self.DT = 0.2 # discretization step
self.path_tick = 0.05
self.L = 0.3 # vehicle wheelbase
self.TARGET_SPEED = 1.0 # m/s
self.MAX_SPEED = 1.5 # m/s
self.MAX_ACC = 1.0 # m/ss
self.MAX_D_ACC = 1.0 # m/sss
self.MAX_STEER = np.radians(30) # rad
self.MAX_D_STEER = np.radians(30) # rad/s
self.T = 10 # Prediction Horizon [s]
self.DT = 0.2 # discretization step [s]
self.path_tick = 0.05 # [m]
self.L = 0.3 # vehicle wheelbase [m]
self.TARGET_SPEED = 1.0 # [m/s
self.MAX_SPEED = 1.5 # [m/s
self.MAX_ACC = 1.0 # [m/ss
self.MAX_D_ACC = 1.0 # [m/sss
self.MAX_STEER = np.radians(30) # [rad]
self.MAX_D_STEER = np.radians(30) # [rad/s]