update single agent path tracker

guided_mrmp/controllers/path_tracker.py

-#! /usr/bin/env python
-
 import numpy as np
 import matplotlib.pyplot as plt
 
@@ -137,7 +135,7 @@ class PathTracker:
             target,
             self.control
         )
-        # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
+        
         # only the first one is used to advance the simulation
         self.control[:] = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
         # self.state = self.predict_next_state(
@@ -146,20 +144,21 @@ class PathTracker:
 
         return x_mpc, self.control
 
-        
-
     def run(self, show_plots=False):
         """
-        [TODO:summary]
-
-        [TODO:description]
+        Run the path tracker algorithm.
+        Parameters:
+        - show_plots (bool): Flag indicating whether to show plots during the simulation. Default is False.
+        Returns:
+        - numpy.ndarray: Array containing the history of x, y, and h coordinates.
         """
+
+        # Add the initial state to the histories
         self.x_history.append(self.state[0])
         self.y_history.append(self.state[1])
         self.h_history.append(self.state[2])
         if show_plots: self.plot_sim()
         
-
         while 1:
             if (np.sqrt((self.state[0] - self.path[0, -1]) ** 2 + (self.state[1] - self.path[1, -1]) ** 2) < 0.1):
                 print("Success! Goal Reached")
@@ -263,6 +262,10 @@ class PathTracker:
 
         plt.tight_layout()
 
+        ax = plt.gca()
+        ax.set_xlim([0, 10])
+        ax.set_ylim([0, 10])
+
         plt.draw()
         plt.pause(0.1)
 
@@ -303,4 +306,4 @@ if __name__ == "__main__":
     wp = [[0, 3, 4, 6, 10, 12, 13, 13, 6, 1, 0],
           [0, 0, 2, 4, 3, 3, -1, -2, -6, -2, -2]]
     sim = PathTracker(initial_position=initial_pos, dynamics=dynamics, target_v=target_vocity, T=T, DT=DT, waypoints=wp)
-    x,y,h = sim.run(show_plots=False)
+    x,y,h = sim.run(show_plots=True)
\ No newline at end of file