update usage in visualization only demo

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -18,7 +18,7 @@ SIM_START_V = 0.0
 SIM_START_H = 0.0
 L = 0.3
 
-P = mpcpy.Params()
+params = mpcpy.Params()
 
 # Params
 VEL = 1.0  # m/s
@@ -31,25 +31,26 @@ class MPCSim:
         self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
 
         # starting guess
-        self.action = np.zeros(P.M)
+        self.action = np.zeros(params.M)
-        self.action[0] = P.MAX_ACC / 2  # a
+        self.action[0] = params.MAX_ACC / 2  # a
         self.action[1] = 0.0  # delta
 
-        self.opt_u = np.zeros((P.M, P.T))
+        self.K = int(params.T / params.DT)
+        self.opt_u = np.zeros((params.M, self.K))
 
         # Cost Matrices
-        Q = np.diag([20, 20, 10, 20])  # state error cost
+        Q = [20, 20, 10, 20]  # state error cost
-        Qf = np.diag([30, 30, 30, 30])  # state final error cost
+        Qf = [30, 30, 30, 30]  # state final error cost
-        R = np.diag([10, 10])  # input cost
+        R = [10, 10]  # input cost
-        R_ = np.diag([10, 10])  # input rate of change cost
+        P = [10, 10]  # input rate of change cost
 
-        self.mpc = mpcpy.MPC(P.N, P.M, Q, R)
+        self.mpc = mpcpy.MPC(Q, Qf, R, P)
 
         # Interpolated Path to follow given waypoints
         self.path = compute_path_from_wp(
             [0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0],
             [0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2],
-            P.path_tick,
+            0.05,
         )
 
         # Sim help vars
@@ -113,9 +114,7 @@ class MPCSim:
             # State Matrices
             A, B, C = mpcpy.get_linear_model_matrices(curr_state, self.action)
             # Get Reference_traj -> inputs are in worldframe
-            target, _ = mpcpy.get_ref_trajectory(
+            target, _ = mpcpy.get_ref_trajectory(self.state, self.path, VEL)
-                self.state, self.path, VEL, dl=P.path_tick
-            )
 
             x_mpc, u_mpc = self.mpc.optimize_linearized_model(
                 A,
@@ -123,13 +122,13 @@ class MPCSim:
                 C,
                 curr_state,
                 target,
-                time_horizon=P.T,
                 verbose=False,
             )
+            # NOTE: used only for preview purposes
             self.opt_u = np.vstack(
                 (
                     np.array(u_mpc.value[0, :]).flatten(),
-                    (np.array(u_mpc.value[1, :]).flatten()),
+                    np.array(u_mpc.value[1, :]).flatten(),
                 )
             )
             self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
@@ -143,12 +142,12 @@ class MPCSim:
             dxdt = x[2] * np.cos(x[3])
             dydt = x[2] * np.sin(x[3])
             dvdt = u[0]
-            dtheta0dt = x[2] * np.tan(u[1]) / P.L
+            dtheta0dt = x[2] * np.tan(u[1]) / params.L
             dqdt = [dxdt, dydt, dvdt, dtheta0dt]
             return dqdt
 
         # solve ODE
-        tspan = [0, P.DT]
+        tspan = [0, params.DT]
         self.state = odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
 
     def plot_sim(self):
@@ -157,7 +156,7 @@ class MPCSim:
 
         [TODO:description]
         """
-        self.sim_time = self.sim_time + P.DT
+        self.sim_time = self.sim_time + params.DT
         self.x_history.append(self.state[0])
         self.y_history.append(self.state[1])
         self.v_history.append(self.state[2])
@@ -231,7 +230,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:] * P.DT)
+        plt.xticks(locs[1:], locs[1:] * params.DT)
         plt.ylabel("a(t) [m/ss]")
         plt.xlabel("t [s]")
 
@@ -240,7 +239,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:] * P.DT)
+        plt.xticks(locs[1:], locs[1:] * params.DT)
         plt.xlabel("t [s]")
 
         plt.tight_layout()

mpc_pybullet_demo/mpcpy/cvxpy_mpc.py

@@ -60,22 +60,22 @@ class MPC:
     def __init__(self, state_cost, final_state_cost, input_cost, input_rate_cost):
         """ """
 
-        nx = P.N  # number of state vars
+        self.nx = P.N  # number of state vars
-        nu = P.M  # umber of input/control vars
+        self.nu = P.M  # umber of input/control vars
 
-        if len(state_cost) != nx:
+        if len(state_cost) != self.nx:
-            raise ValueError(f"State Error cost matrix shuld be of size {nx}")
+            raise ValueError(f"State Error cost matrix shuld be of size {self.nx}")
-        if len(final_state_cost) != nx:
+        if len(final_state_cost) != self.nx:
-            raise ValueError(f"End State Error cost matrix shuld be of size {nx}")
+            raise ValueError(f"End State Error cost matrix shuld be of size {self.nx}")
-        if len(input_cost) != nu:
+        if len(input_cost) != self.nu:
-            raise ValueError(f"Control Effort cost matrix shuld be of size {nu}")
+            raise ValueError(f"Control Effort cost matrix shuld be of size {self.nu}")
-        if len(input_rate_cost) != nu:
+        if len(input_rate_cost) != self.nu:
             raise ValueError(
-                f"Control Effort Difference cost matrix shuld be of size {nu}"
+                f"Control Effort Difference cost matrix shuld be of size {self.nu}"
             )
 
         self.dt = P.DT
-        self.control_horizon = P.T / P.DT
+        self.control_horizon = int(P.T / P.DT)
         self.Q = np.diag(state_cost)
         self.Qf = np.diag(final_state_cost)
         self.R = np.diag(input_cost)
@@ -103,11 +103,11 @@ class MPC:
         :return:
         """
 
-        assert len(initial_state) == self.state_len
+        assert len(initial_state) == self.nx
 
         # Create variables
-        x = opt.Variable((self.state_len, control_horizon + 1), name="states")
+        x = opt.Variable((self.nx, self.control_horizon + 1), name="states")
-        u = opt.Variable((self.action_len, control_horizon), name="actions")
+        u = opt.Variable((self.nu, self.control_horizon), name="actions")
         cost = 0
         constr = []
 
@@ -143,4 +143,4 @@ class MPC:
 
         prob = opt.Problem(opt.Minimize(cost), constr)
         solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
-        return u[:, 0].value
+        return x, u

mpc_pybullet_demo/mpcpy/mpc_config.py

@@ -5,9 +5,8 @@ class Params:
     def __init__(self):
         self.N = 4  # number of state variables
         self.M = 2  # number of control variables
-        self.T = 10  # Prediction Horizon [s]
+        self.T = 5  # 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

mpc_pybullet_demo/mpcpy/utils.py

@@ -66,14 +66,14 @@ def normalize_angle(angle):
     return angle
 
 
-def get_ref_trajectory(state, path, target_v, dl=0.1):
+def get_ref_trajectory(state, path, target_v):
     """
     For each step in the time horizon
     modified reference in robot frame
     """
-    xref = np.zeros((P.N, P.T + 1))
+    K = int(P.T / P.DT)
-    dref = np.zeros((1, P.T + 1))
+    xref = np.zeros((P.N, K + 1))
-    # sp = np.ones((1,T +1))*target_v #speed profile
+    dref = np.zeros((1, K + 1))
     ncourse = path.shape[1]
     ind = get_nn_idx(state, path)
     dx = path[0, ind] - state[0]
@@ -84,7 +84,8 @@ def get_ref_trajectory(state, path, target_v, dl=0.1):
     xref[3, 0] = normalize_angle(path[2, ind] - state[3])  # Theta
     dref[0, 0] = 0.0  # Steer operational point should be 0
     travel = 0.0
-    for i in range(1, P.T + 1):
+    dl = np.hypot(path[0, 1] - path[0, 0], path[1, 1] - path[1, 0])
+    for i in range(1, K + 1):
         travel += abs(target_v) * P.DT
         dind = int(round(travel / dl))
         if (ind + dind) < ncourse: