From 279625b4c16aa152515775a135fbc7e48dae3e20 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Thu, 12 Oct 2023 19:28:38 +0100 Subject: [PATCH] update notation --- mpc_pybullet_demo/mpcpy/cvxpy_mpc.py | 8 ++++---- mpc_pybullet_demo/mpcpy/mpc_config.py | 20 ++++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py index b14bb55..723ba28 100755 --- a/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py +++ b/mpc_pybullet_demo/mpcpy/cvxpy_mpc.py @@ -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] ] diff --git a/mpc_pybullet_demo/mpcpy/mpc_config.py b/mpc_pybullet_demo/mpcpy/mpc_config.py index 523904c..af32e4a 100644 --- a/mpc_pybullet_demo/mpcpy/mpc_config.py +++ b/mpc_pybullet_demo/mpcpy/mpc_config.py @@ -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]