improve naming

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -32,7 +32,7 @@ class MPCSim:
 
         # helper variable to keep track of mpc output
         # starting condition is 0,0
-        self.action = np.zeros(2)
+        self.control = np.zeros(2)
 
         self.K = int(T / DT)
 
@@ -57,7 +57,7 @@ class MPCSim:
         self.h_history = []
         self.a_history = []
         self.d_history = []
-        self.predicted = None
+        self.optimized_trajectory = None
 
         # Initialise plot
         plt.style.use("ggplot")
@@ -65,24 +65,26 @@ class MPCSim:
         plt.ion()
         plt.show()
 
-    def ego_to_world(self, mpc_out):
+    def ego_to_global(self, mpc_out):
         """
+        transforms optimized trajectory XY points from ego(car) reference
+        into global(map) frame
 
         Args:
             mpc_out ():
         """
-        predicted = np.zeros((2, self.K))
-        predicted[:, :] = mpc_out[0:2, 1:]
+        trajectory = np.zeros((2, self.K))
+        trajectory[:, :] = mpc_out[0:2, 1:]
         Rotm = np.array(
             [
                 [np.cos(self.state[3]), np.sin(self.state[3])],
                 [-np.sin(self.state[3]), np.cos(self.state[3])],
             ]
         )
-        predicted = (predicted.T.dot(Rotm)).T
-        predicted[0, :] += self.state[0]
-        predicted[1, :] += self.state[1]
-        return predicted
+        trajectory = (trajectory.T.dot(Rotm)).T
+        trajectory[0, :] += self.state[0]
+        trajectory[1, :] += self.state[1]
+        return trajectory
 
     def run(self):
         """
@@ -114,19 +116,19 @@ class MPCSim:
             x_mpc, u_mpc = self.mpc.step(
                 curr_state,
                 target,
-                self.action,
+                self.control,
                 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.control[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
             self.state = self.predict_next_state(
-                self.state, [self.action[0], self.action[1]], DT
+                self.state, [self.control[0], self.control[1]], DT
             )
 
             # use the optimizer output to preview the predicted state trajectory
-            self.predicted = self.ego_to_world(x_mpc.value)
+            self.optimized_trajectory = self.ego_to_global(x_mpc.value)
             self.plot_sim()
 
     def predict_next_state(self, state, u, dt):
@@ -149,8 +151,8 @@ class MPCSim:
         self.y_history.append(self.state[1])
         self.v_history.append(self.state[2])
         self.h_history.append(self.state[3])
-        self.a_history.append(self.action[0])
-        self.d_history.append(self.action[1])
+        self.a_history.append(self.control[0])
+        self.d_history.append(self.control[1])
 
         plt.clf()
 
@@ -178,10 +180,10 @@ class MPCSim:
             label="vehicle trajectory",
         )
 
-        if self.predicted is not None:
+        if self.optimized_trajectory is not None:
             plt.plot(
-                self.predicted[0, :],
-                self.predicted[1, :],
+                self.optimized_trajectory[0, :],
+                self.optimized_trajectory[1, :],
                 c="tab:green",
                 marker="+",
                 alpha=0.5,

mpc_pybullet_demo/mpc_demo_pybullet.py

@@ -227,7 +227,7 @@ def run_sim():
         p.addUserDebugLine([x_, y_, 0], [x_, y_, 0.33], [0, 0, 1])
 
     # starting conditions
-    action = np.zeros(2)
+    control = np.zeros(2)
 
     # Cost Matrices
     Q = [20, 20, 10, 20]  # state error cost [x,y,v,yaw]
@@ -271,21 +271,21 @@ def run_sim():
         # simulate one timestep actuation delay
         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]) * DT
-        ego_state[2] = ego_state[2] + action[0] * DT
-        ego_state[3] = ego_state[3] + action[0] * np.tan(action[1]) / L * DT
+        ego_state[2] = ego_state[2] + control[0] * DT
+        ego_state[3] = ego_state[3] + control[0] * np.tan(control[1]) / L * DT
 
         # optimization loop
         start = time.time()
 
         # MPC step
-        _, u_mpc = mpc.step(ego_state, target, action, verbose=False)
-        action[0] = u_mpc.value[0, 0]
-        action[1] = u_mpc.value[1, 0]
+        _, u_mpc = mpc.step(ego_state, target, control, verbose=False)
+        control[0] = u_mpc.value[0, 0]
+        control[1] = u_mpc.value[1, 0]
 
         elapsed = time.time() - start
         print("CVXPY Optimization Time: {:.4f}s".format(elapsed))
 
-        set_ctrl(car, state[2], action[0], action[1])
+        set_ctrl(car, state[2], control[0], control[1])
 
         if DT - elapsed > 0:
             time.sleep(DT - elapsed)