set rate of change limit on u_0

mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py

@@ -125,29 +125,25 @@ class MPC:
         # Ak, Bk, Ck = self.get_linear_model_matrices(x_prev[:,k], u_prev[:,k])
         A, B, C = self.get_linear_model_matrices(initial_state, prev_cmd)
 
+        # Tracking error cost
+        for k in range(self.control_horizon):
+            cost += opt.quad_form(x[:, k + 1] - target[:, k], self.Q)
+
+        # Final point tracking cost
+        cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
+
+        # Actuation magnitude cost
         for k in range(self.control_horizon):
-            cost += opt.quad_form(target[:, k] - x[:, k + 1], self.Q)
             cost += opt.quad_form(u[:, k], self.R)
 
-            # Actuation rate of change
+        # Actuation rate of change cost
-            if k < (self.control_horizon - 1):
+        for k in range(1, self.control_horizon):
-                cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
+            cost += opt.quad_form(u[:, k] - u[:, k - 1], self.P)
 
-            # Kinematics Constrains
+        # Kinematics Constrains
+        for k in range(self.control_horizon):
             constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
 
-            # Actuation rate of change bounds
-            if k < (self.control_horizon - 1):
-                constr += [
-                    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.vehicle.max_d_steer
-                ]
-
-        # Final Point tracking
-        cost += opt.quad_form(x[:, -1] - target[:, -1], self.Qf)
-
         # initial state
         constr += [x[:, 0] == initial_state]
 
@@ -155,6 +151,17 @@ class MPC:
         constr += [opt.abs(u[:, 0]) <= self.vehicle.max_acc]
         constr += [opt.abs(u[:, 1]) <= self.vehicle.max_steer]
 
+        # 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
+            ]
+
         prob = opt.Problem(opt.Minimize(cost), constr)
         solution = prob.solve(solver=opt.OSQP, warm_start=True, verbose=False)
         return x, u

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -31,12 +31,9 @@ class MPCSim:
         self.state = np.array([SIM_START_X, SIM_START_Y, SIM_START_V, SIM_START_H])
 
         # helper variable to keep track of mpc output
+        # starting condition is 0,0
         self.action = np.zeros(2)
 
-        # starting guess
-        self.action[0] = 1.0  # a
-        self.action[1] = 0.0  # delta
-
         self.K = int(T / DT)
 
         Q = [20, 20, 10, 20]  # state error cost
@@ -121,16 +118,16 @@ class MPCSim:
                 verbose=False,
             )
             # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
-
             # only the first one is used to advance the simulation
+
             self.action[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
-            self.state = self.predict([self.action[0], self.action[1]], DT)
+            self.state = self.predict(self.state, [self.action[0], self.action[1]], DT)
 
             # use the state trajectory to preview the optimizer output
             self.predicted = self.preview(x_mpc.value)
             self.plot_sim()
 
-    def predict(self, u, dt):
+    def predict(self, state, u, dt):
         def kinematics_model(x, t, u):
             dxdt = x[2] * np.cos(x[3])
             dydt = x[2] * np.sin(x[3])
@@ -141,7 +138,8 @@ class MPCSim:
 
         # solve ODE
         tspan = [0, dt]
-        return odeint(kinematics_model, self.state, tspan, args=(u[:],))[1]
+        new_state = odeint(kinematics_model, state, tspan, args=(u[:],))[1]
+        return new_state
 
     def plot_sim(self):
         self.sim_time = self.sim_time + DT

mpc_pybullet_demo/mpc_demo_pybullet.py

@@ -226,10 +226,8 @@ def run_sim():
     for x_, y_ in zip(path[0, :], path[1, :]):
         p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
 
-    # starting guess
+    # starting conditions
     action = np.zeros(2)
-    action[0] = 1.0  # a
-    action[1] = 0.0  # delta
 
     # Cost Matrices
     Q = [20, 20, 10, 20]  # state error cost [x,y,v,yaw]