WIP: remove Params class

mpc_pybullet_demo/cvxpy_mpc/__init__.py

@@ -1,2 +1,2 @@
 from .cvxpy_mpc import MPC
-from .mpc_config import Params
+from .vehicle_model import VehicleModel

mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py

@@ -4,22 +4,23 @@ np.seterr(divide="ignore", invalid="ignore")
 
 import cvxpy as opt
 
-from .mpc_config import Params
-
-P = Params()
-
 
 class MPC:
-    def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost):
+    def __init__(
+        self, vehicle, T, DT, state_cost, final_state_cost, input_cost, input_rate_cost
+    ):
         """
+        :param vehicle:
+        :param T:
+        :param DT:
         :param state_cost:
         :param final_state_cost:
         :param input_cost:
         :param input_rate_cost:
         """
 
-        self.nx = P.N  # number of state vars
+        self.nx = 4  # number of state vars
-        self.nu = P.M  # umber of input/control 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}")
@@ -32,14 +33,13 @@ class MPC:
                 f"Control Effort Difference cost matrix shuld be of size {self.nu}"
             )
 
-        self.dt = P.DT
+        self.vehicle = vehicle
-        self.control_horizon = int(P.T / P.DT)
+        self.dt = DT
+        self.control_horizon = int(T / DT)
         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)
-        self.u_bounds = np.array([P.MAX_ACC, P.MAX_STEER])
-        self.du_bounds = np.array([P.MAX_D_ACC, P.MAX_D_STEER])
 
     def get_linear_model_matrices(self, x_bar, u_bar):
         """
@@ -67,15 +67,17 @@ class MPC:
         A[0, 3] = -v * st
         A[1, 2] = st
         A[1, 3] = v * ct
-        A[3, 2] = v * td / P.L
+        A[3, 2] = v * td / self.vehicle.wheelbase
         A_lin = np.eye(self.nx) + self.dt * A
 
-        B = np.zeros((P.N, P.M))
+        B = np.zeros((self.nx, self.nu))
         B[2, 0] = 1
-        B[3, 1] = v / (P.L * cd**2)
+        B[3, 1] = v / (self.vehicle.wheelbase * cd**2)
         B_lin = self.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 / self.vehicle.wheelbase]).reshape(
+            self.nx, 1
+        )
         C_lin = (
             self.dt
             * (
@@ -122,13 +124,13 @@ class MPC:
             # Kinematics Constrains
             constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
 
-            # Actuation rate of change limit
+            # Actuation rate of change bounds
             if k < (self.control_horizon - 1):
                 constr += [
-                    opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.du_bounds[0]
+                    opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.vehicle.max_d_acc
                 ]
                 constr += [
-                    opt.abs(u[1, k + 1] - u[1, k]) / self.dt <= self.du_bounds[1]
+                    opt.abs(u[1, k + 1] - u[1, k]) / self.dt <= self.vehicle.max_d_steer
                 ]
 
         # Final Point tracking
@@ -137,9 +139,9 @@ class MPC:
         # initial state
         constr += [x[:, 0] == initial_state]
 
-        # actuation magnitude
+        # actuation bounds
-        constr += [opt.abs(u[:, 0]) <= self.u_bounds[0]]
+        constr += [opt.abs(u[:, 0]) <= self.vehicle.max_acc]
-        constr += [opt.abs(u[:, 1]) <= self.u_bounds[1]]
+        constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer]
 
         prob = opt.Problem(opt.Minimize(cost), constr)
         solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)

mpc_pybullet_demo/cvxpy_mpc/mpc_config.py

@@ -1,16 +0,0 @@
-import numpy as np
-
-
-class Params:
-    def __init__(self):
-        self.N = 4  # number of state variables
-        self.M = 2  # number of control variables
-        self.T = 5  # Prediction Horizon [s]
-        self.DT = 0.2  # discretization step [s]
-        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]

mpc_pybullet_demo/cvxpy_mpc/utils.py

@@ -1,8 +1,5 @@
 import numpy as np
 from scipy.interpolate import interp1d
-from .mpc_config import Params
-
-P = Params()
 
 
 def compute_path_from_wp(start_xp, start_yp, step=0.1):
@@ -62,7 +59,7 @@ def get_nn_idx(state, path):
     return target_idx
 
 
-def get_ref_trajectory(state, path, target_v):
+def get_ref_trajectory(state, path, target_v, double T, double DT):
     """
     Reinterpolate the trajectory to get a set N desired target states
     :param state:
@@ -70,9 +67,9 @@ def get_ref_trajectory(state, path, target_v):
     :param target_v:
     :return:
     """
-    K = int(P.T / P.DT)
+    K = int(T / DT)
 
-    xref = np.zeros((P.N, K))
+    xref = np.zeros((4, K))
     ind = get_nn_idx(state, path)
 
     cdist = np.append(

mpc_pybullet_demo/cvxpy_mpc/vehicle_model.py

@@ -0,0 +1,15 @@
+import numpy as np
+
+
+class VehicleModel:
+    """
+    Helper class to hold vehicle info
+    """
+
+    def __init__(self):
+        self.wheelbase = 0.3  # vehicle wheelbase [m]
+        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]

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -5,21 +5,23 @@ import matplotlib.pyplot as plt
 from scipy.integrate import odeint
 
 from cvxpy_mpc.utils import compute_path_from_wp, get_ref_trajectory
-from cvxpy_mpc import MPC, Params
+from cvxpy_mpc import MPC, VehicleModel
 
 import sys
 
+
 # Robot Starting position
 SIM_START_X = 0.0
 SIM_START_Y = 0.5
 SIM_START_V = 0.0
 SIM_START_H = 0.0
-L = 0.3
 
-params = Params()
 
 # Params
-VEL = 1.0  # m/s
+TARGET_VEL = 1.0  # m/s
+T = 5  # Prediction Horizon [s]
+DT = 0.2  # discretization step [s]
+L = 0.3  # vehicle wheelbase [m]
 
 
 # Classes
@@ -29,12 +31,12 @@ class MPCSim:
         self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
 
         # starting guess
-        self.action = np.zeros(params.M)
+        self.action = np.zeros(2)
-        self.action[0] = params.MAX_ACC / 2  # a
+        self.action[0] = 1.0  # a
         self.action[1] = 0.0  # delta
 
-        self.K = int(params.T / params.DT)
+        self.K = int(T / DT)
-        self.opt_u = np.zeros((params.M, self.K))
+        self.opt_u = np.zeros((2, self.K))
 
         # Weights for Cost Matrices
         Q = [20, 20, 10, 20]  # state error cost
@@ -42,7 +44,7 @@ class MPCSim:
         R = [10, 10]  # input cost
         P = [10, 10]  # input rate of change cost
 
-        self.mpc = MPC(Q, Qf, R, P)
+        self.mpc = MPC(VehicleModel(), T, DT, Q, Qf, R, P)
 
         # Interpolated Path to follow given waypoints
         self.path = compute_path_from_wp(
@@ -110,7 +112,7 @@ class MPCSim:
             # dynamycs w.r.t robot frame
             curr_state = np.array([0, 0, self.state[2], 0])
             # Get Reference_traj -> inputs are in worldframe
-            target = get_ref_trajectory(self.state, self.path, VEL)
+            target = get_ref_trajectory(self.state, self.path, TARGET_VEL)
 
             x_mpc, u_mpc = self.mpc.step(
                 curr_state,
@@ -127,21 +129,21 @@ class MPCSim:
             )
             self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
             # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
-            self.predict([self.action[0], self.action[1]])
+            self.predict([self.action[0], self.action[1]], DT)
             self.preview(x_mpc.value)
             self.plot_sim()
 
-    def predict(self, u):
+    def predict(self, u, dt):
         def kinematics_model(x, t, u):
             dxdt = x[2] * np.cos(x[3])
             dydt = x[2] * np.sin(x[3])
             dvdt = u[0]
-            dthetadt = x[2] * np.tan(u[1]) / params.L
+            dthetadt = x[2] * np.tan(u[1]) / L
             dqdt = [dxdt, dydt, dvdt, dthetadt]
             return dqdt
 
         # solve ODE
-        tspan = [0, params.DT]
+        tspan = [0, dt]
         self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
 
     def plot_sim(self):
@@ -150,7 +152,7 @@ class MPCSim:
 
         [TODO:description]
         """
-        self.sim_time = self.sim_time + params.DT
+        self.sim_time = self.sim_time + DT
         self.x_history.append(self.state[0])
         self.y_history.append(self.state[1])
         self.v_history.append(self.state[2])
@@ -224,7 +226,7 @@ class MPCSim:
         # plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
         plt.plot(self.a_history, c="tab:orange")
         locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:] * params.DT)
+        plt.xticks(locs[1:], locs[1:] * DT)
         plt.ylabel("a(t) [m/ss]")
         plt.xlabel("t [s]")
 
@@ -233,7 +235,7 @@ class MPCSim:
         plt.plot(np.degrees(self.d_history), c="tab:orange")
         plt.ylabel("gamma(t) [deg]")
         locs, _ = plt.xticks()
-        plt.xticks(locs[1:], locs[1:] * params.DT)
+        plt.xticks(locs[1:], locs[1:] * DT)
         plt.xlabel("t [s]")
 
         plt.tight_layout()

mpc_pybullet_demo/mpc_demo_pybullet.py

@@ -2,9 +2,7 @@ import numpy as np
 import matplotlib.pyplot as plt
 
 from cvxpy_mpc.utils import compute_path_from_wp, get_ref_trajectory
-from cvxpy_mpc import MPC, Params
+from cvxpy_mpc import MPC, VehicleModel
-
-params = Params()
 
 import sys
 import time
@@ -12,6 +10,12 @@ import pathlib
 import pybullet as p
 import time
 
+# Params
+TARGET_VEL = 1.0  # m/s
+L = 0.3  # vehicle wheelbase [m]
+T = 5  # Prediction Horizon [s]
+DT = 0.2  # discretization step [s]
+
 
 def get_state(robotId):
     """ """
@@ -34,7 +38,7 @@ def set_ctrl(robotId, currVel, acceleration, steeringAngle):
     wheels = [8, 15]
     maxForce = 50
 
-    targetVelocity = currVel + acceleration * params.DT
+    targetVelocity = currVel + acceleration * DT
 
     for wheel in wheels:
         p.setJointMotorControl2(
@@ -210,8 +214,8 @@ def run_sim():
         p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
 
     # starting guess
-    action = np.zeros(params.M)
+    action = np.zeros(2)
-    action[0] = params.MAX_ACC / 2  # a
+    action[0] = 1.0  # a
     action[1] = 0.0  # delta
 
     # Cost Matrices
@@ -220,7 +224,7 @@ def run_sim():
     R = [10, 10]  # input cost [acc ,steer]
     P = [10, 10]  # input rate of change cost [acc ,steer]
 
-    mpc = MPC(Q, Qf, R, P)
+    mpc = MPC(VehicleModel(), Q, Qf, R, P)
     x_history = []
     y_history = []
 
@@ -246,7 +250,7 @@ def run_sim():
 
         # Get Reference_traj
         # NOTE: inputs are in world frame
-        target = get_ref_trajectory(state, path, params.TARGET_SPEED)
+        target = get_ref_trajectory(state, path, TARGET_VEL)
 
         # for MPC base link frame is used:
         # so x, y, yaw are 0.0, but speed is the same
@@ -254,12 +258,10 @@ def run_sim():
 
         # to account for MPC latency
         # simulate one timestep actuation delay
-        ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * params.DT
+        ego_state[0] = ego_state[0] + ego_state[2] * np.cos(ego_state[3]) * DT
-        ego_state[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * params.DT
+        ego_state[1] = ego_state[1] + ego_state[2] * np.sin(ego_state[3]) * DT
-        ego_state[2] = ego_state[2] + action[0] * params.DT
+        ego_state[2] = ego_state[2] + action[0] * DT
-        ego_state[3] = (
+        ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
-            ego_state[3] + action[0] * np.tan(action[1]) / params.L * params.DT
-        )
 
         # optimization loop
         start = time.time()
@@ -274,8 +276,8 @@ def run_sim():
 
         set_ctrl(car, state[2], action[0], action[1])
 
-        if params.DT - elapsed > 0:
+        if DT - elapsed > 0:
-            time.sleep(params.DT - elapsed)
+            time.sleep(DT - elapsed)
 
 
 if __name__ == "__main__":