From 749fe7df67afb65c5bc6c325c57be0f6723fbc95 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Mon, 9 Sep 2024 21:05:44 -0500
Subject: [PATCH] update single agent path tracker
---
guided_mrmp/controllers/path_tracker.py | 23 +++++++++++++----------
1 file changed, 13 insertions(+), 10 deletions(-)
diff --git a/guided_mrmp/controllers/path_tracker.py b/guided_mrmp/controllers/path_tracker.py
index b88e709..29fedd8 100644
--- a/guided_mrmp/controllers/path_tracker.py
+++ b/guided_mrmp/controllers/path_tracker.py
@@ -1,5 +1,3 @@
-#! /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)
\ No newline at end of file
+ x,y,h = sim.run(show_plots=True)
\ No newline at end of file
--
GitLab