improve docstring clarity

mpc_pybullet_demo/cvxpy_mpc/cvxpy_mpc.py

@@ -44,11 +44,11 @@ class MPC:
 
     def get_linear_model_matrices(self, x_bar, u_bar):
         """
-        Computes the LTI approximated state space model x' = Ax + Bu + C
+        Computes the approximated LTI state space model x' = Ax + Bu + C
 
         Args:
-            x_bar ():
-            u_bar ():
+            x_bar (array-like):
+            u_bar (array-like):
 
         Returns:
 
@@ -103,15 +103,17 @@ class MPC:
         """
 
         Args:
-            initial_state ():
-            target ():
-            prev_cmd ():
-            verbose ():
+            initial_state (array-like): current estimate of [x, y, v, heading]
+            target (ndarray): state space reference, in the same frame as the provided current state
+            prev_cmd (array-like): previous [acceleration, steer]. note this is used in bounds and has to be realistic.
+            verbose (bool):
 
         Returns:
 
         """
         assert len(initial_state) == self.nx
+        assert len(prev_cmd) == self.nu
+        assert target.shape == (self.nx, self.control_horizon)
 
         # Create variables needed for setting up cvxpy problem
         x = opt.Variable((self.nx, self.control_horizon + 1), name="states")

mpc_pybullet_demo/cvxpy_mpc/utils.py

@@ -70,14 +70,15 @@ def get_ref_trajectory(state, path, target_v, T, DT):
     """
 
     Args:
-        state (array-like): state of the vehicle in world frame
-        path (ndarray): 2D array representing the path as x,y,heading points in world frame
-        target_v (float): reference speed
-        T (float): trajectory duration
-        DT (float): trajectory time-step
+        state (array-like): state of the vehicle in global frame
+        path (ndarray): 2D array representing the path as x,y,heading points in global frame
+        target_v (float): desired speed
+        T (float): control horizon duration
+        DT (float):  control horizon time-step
 
     Returns:
-        ndarray: 2D array representing state space reference trajectory expressed w.r.t ego state
+        ndarray: 2D array representing state space trajectory [x_k, y_k, v_k, theta_k] w.r.t ego frame.
+        Interpolated according to the time-step and the desired velocity
     """
     K = int(T / DT)
 
@@ -101,7 +102,7 @@ def get_ref_trajectory(state, path, target_v, T, DT):
     stop_idx = np.where(xref_cdist == cdist[-1])
     xref[2, stop_idx] = 0.0
 
-    # transform in car ego frame
+    # transform in ego frame
     dx = xref[0, :] - state[0]
     dy = xref[1, :] - state[1]
     xref[0, :] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])  # X
@@ -110,7 +111,7 @@ def get_ref_trajectory(state, path, target_v, T, DT):
 
     def fix_angle_reference(angle_ref, angle_init):
         """
-        Removes jumps greater than 2PI
+        Removes jumps greater than 2PI to smooth the heading
 
         Args:
             angle_ref (array-like):

mpc_pybullet_demo/mpc_demo_nosim.py

@@ -65,7 +65,7 @@ class MPCSim:
         plt.ion()
         plt.show()
 
-    def preview(self, mpc_out):
+    def ego_to_world(self, mpc_out):
         """
 
         Args:
@@ -121,13 +121,15 @@ class MPCSim:
             # 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.state, [self.action[0], self.action[1]], DT)
+            self.state = self.predict_next_state(
+                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)
+            # use the optimizer output to preview the predicted state trajectory
+            self.predicted = self.ego_to_world(x_mpc.value)
             self.plot_sim()
 
-    def predict(self, state, u, dt):
+    def predict_next_state(self, state, u, dt):
         def kinematics_model(x, t, u):
             dxdt = x[2] * np.cos(x[3])
             dydt = x[2] * np.sin(x[3])