diff --git a/guided_mrmp/controllers/multi_path_tracking.py b/guided_mrmp/controllers/multi_path_tracking.py
index 89fe8ee6ba1ceb9957add2015b45edc675adbd47..1bf1b726cc669ba9c97c5e76f077e4a2162fa4c7 100644
--- a/guided_mrmp/controllers/multi_path_tracking.py
+++ b/guided_mrmp/controllers/multi_path_tracking.py
@@ -88,10 +88,10 @@ class MultiPathTracker:
# For a circular robot (easy dynamics)
- Q = [40, 40, 0] # state error cost
- Qf = [20,20, 0] # state final error cost
- R = [10, 10] # input cost
- P = [10, 10] # input rate of change cost
+ Q = settings['model_predictive_controller']['Q'] # state error cost
+ Qf = settings['model_predictive_controller']['Qf'] # state final error cost
+ R = settings['model_predictive_controller']['R'] # input cost
+ P = settings['model_predictive_controller']['P'] # input rate of change cost
self.mpc = MultiMPC(self.num_robots, dynamics, T, DT, Q, Qf, R, P, settings['model_predictive_controller'], settings['environment']['circle_obstacles'])
self.circle_obs = settings['environment']['circle_obstacles']
@@ -101,6 +101,8 @@ class MultiPathTracker:
for wp in waypoints:
self.paths.append(compute_path_from_wp(wp[0], wp[1], 0.05))
+ self.visited_points_on_guide_paths = [[]]*self.num_robots
+
print(f"paths = {len(self.paths)}")
@@ -381,6 +383,9 @@ class MultiPathTrackerDatabase(MultiPathTracker):
# the initial guess for time is the length of the longest path in the solution
initial_guess_T = 2*max([len(grid_solution[i]) for i in range(num_robots)])
+ # reverse the order of grid solution
+ grid_solution = grid_solution[::-1]
+
for i in range(num_robots):
print(f"Robot {i+1} solution:")
@@ -388,23 +393,20 @@ class MultiPathTrackerDatabase(MultiPathTracker):
points = []
for point in rough_points:
if point[0] == -1: break
- points.append(point)
+ # append the point mutiplied by the cell size
+ points.append([point[0]*self.cell_size, point[1]*self.cell_size])
points = np.array(points)
print(f"points = {points}")
- smoothed_curve, _ = smooth_path(points, N+1, cp_dist)
-
- print(f"smoothed_curve = {smoothed_curve}")
-
+ # plot the points
+ plt.plot(points[:, 0], points[:, 1], 'o-')
- # translate the smoothed curve so that the first point is at the current robot position
- # smoothed_curve[:, 0] += current_robot_x_pos
- # smoothed_curve[:, 1] += current_robot_y_pos
+ smoothed_curve, _ = smooth_path(points, N+1, cp_dist)
- initial_guess_state[i*3, :] = (smoothed_curve[:, 0])*self.cell_size # x
- initial_guess_state[i*3 + 1, :] = (smoothed_curve[:, 1])*self.cell_size # y
+ initial_guess_state[i*3, :] = smoothed_curve[:, 0] # x
+ initial_guess_state[i*3 + 1, :] = smoothed_curve[:, 1] # y
current_robot_x_pos = self.states[i][0]
current_robot_y_pos = self.states[i][1]
@@ -416,7 +418,7 @@ class MultiPathTrackerDatabase(MultiPathTracker):
# translate the initial guess so that the first point is at the current robot position
initial_guess_state[i*3, :] += current_robot_x_pos
- initial_guess_state[i*3 + 1, :] += current_robot_y_pos + self.cell_size
+ initial_guess_state[i*3 + 1, :] += current_robot_y_pos
headings = calculate_headings(smoothed_curve)
@@ -424,7 +426,7 @@ class MultiPathTrackerDatabase(MultiPathTracker):
initial_guess_state[i*3 + 2, :] = headings
-
+ plt.show()
initial_guess_control = np.zeros((num_robots*2, N))
@@ -459,7 +461,7 @@ class MultiPathTrackerDatabase(MultiPathTracker):
we will only place a grid of size self.grid_size x self.grid_size
inputs:
- - robot_locations (list): locations of robots involved in conflict
+ - robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...]
outputs:
- grid (numpy array): The grid that we placed
- top_left (tuple): The top left corner of the grid in continuous space
@@ -760,9 +762,17 @@ class MultiPathTrackerDatabase(MultiPathTracker):
# 1. Get the reference trajectory for each robot
targets = []
for i in range(self.num_robots):
- ref = get_ref_trajectory(np.array(state[i]), np.array(self.paths[i]), self.target_v, self.T, self.DT,0)
+ ref, visited_guide_points = get_ref_trajectory(np.array(state[i]),
+ np.array(self.paths[i]),
+ self.target_v,
+ self.T,
+ self.DT,
+ self.visited_points_on_guide_paths[i])
- print(f"Robot {i} reference trajectory = {ref}")
+ print(f"visited_guide_points = {visited_guide_points}")
+ self.visited_points_on_guide_paths[i] = visited_guide_points
+ print(f"visited_points_on_guide_paths[i] = {self.visited_points_on_guide_paths[i]}")
+
targets.append(ref)
self.trajs = targets
@@ -837,6 +847,10 @@ class MultiPathTrackerDatabase(MultiPathTracker):
# plan from the last point of the ref path to the robot's goal
# plan an RRT path from the current state to the goal
+
+ # last_point = new_ref[:, 0]
+ # x_start = (last_point[0], last_point[1])
+
x_start = (new_ref[:, -1][0], new_ref[:, -1][1])
x_goal = (old_paths[i][:, -1][0], old_paths[i][:, -1][1])
@@ -847,6 +861,7 @@ class MultiPathTrackerDatabase(MultiPathTracker):
rrtstarpath2 = list(reversed(rrtstarpath2))
xs = new_ref[0, :].tolist()
ys = new_ref[1, :].tolist()
+
for node in rrtstarpath2:
xs.append(node[0])
ys.append(node[1])
@@ -858,7 +873,17 @@ class MultiPathTrackerDatabase(MultiPathTracker):
targets = []
for i in range(self.num_robots):
- ref = get_ref_trajectory(np.array(state[i]), np.array(self.paths[i]), self.target_v, self.T, self.DT,0)
+ ref, visited_guide_points = get_ref_trajectory(np.array(state[i]),
+ np.array(self.paths[i]),
+ self.target_v,
+ self.T,
+ self.DT,
+ self.visited_points_on_guide_paths[i])
+
+ print(f"visited_guide_points = {visited_guide_points}")
+ self.visited_points_on_guide_paths[i] = visited_guide_points
+ print(f"visited_points_on_guide_paths[i] = {self.visited_points_on_guide_paths[i]}")
+
print(f"Robot {i} reference trajectory = {ref}")
targets.append(ref)
diff --git a/guided_mrmp/controllers/utils.py b/guided_mrmp/controllers/utils.py
index bbd9291da2336bdcae85981aa239b9204b3519af..cc243e82b1d3f89e7163beedf940f29cd4d25de8 100644
--- a/guided_mrmp/controllers/utils.py
+++ b/guided_mrmp/controllers/utils.py
@@ -39,18 +39,32 @@ def compute_path_from_wp(start_xp, start_yp, step=0.1):
theta = np.arctan2(dy, dx)
return np.vstack((final_xp, final_yp, theta))
-def get_nn_idx(state, path):
+def get_nn_idx(state, path, visited=[]):
"""
Helper function to find the index of the nearest path point to the current state.
+
+ The "nearest" point is defined as the point with the smallest Euclidean distance
+ to the current state that has not already been visited.
+
Args:
state (array-like): Current state [x, y, theta]
- path (ndarray): Path points
+ path (ndarray): Path points [[x1, y1], [x2, y2], ...]
+ visited (array-like): Visited path points [[x1, y1], [x2, y2], ...]
Returns:
int: Index of the nearest path point
"""
- # distances = np.hypot(path[0, :] - state[0], path[1, :] - state[1])
- distances = np.linalg.norm(path[:2]-state[:2].reshape(2,1), axis=0)
+ # Calculate the Euclidean distance between the current state and all path points
+ distances = np.linalg.norm(path[:2] - state[:2].reshape(2, 1), axis=0)
+
+ # Set the distance to infinity for visited points
+ for point in visited:
+ point = np.array(point)
+ # print(f"point = {point}")
+ # print(f"path[:2] = {path[:2]}")
+ # Set the distance to infinity for visited points
+ distances = np.where(np.linalg.norm(path[:2] - point.reshape(2, 1), axis=0) < 1e-3, np.inf, distances)
+
return np.argmin(distances)
def fix_angle_reference(angle_ref, angle_init):
@@ -68,13 +82,14 @@ def fix_angle_reference(angle_ref, angle_init):
diff_angle = np.unwrap(diff_angle)
return angle_init + diff_angle
-def get_ref_trajectory(state, path, target_v, T, DT):
+def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
"""
Generates a reference trajectory for the Roomba.
Args:
state (array-like): Current state [x, y, theta]
path (ndarray): Path points [x, y, theta] in the global frame
+ path_visited_points (array-like): Visited path points [[x, y], [x, y], ...]
target_v (float): Desired speed
T (float): Control horizon duration
DT (float): Control horizon time-step
@@ -84,10 +99,15 @@ def get_ref_trajectory(state, path, target_v, T, DT):
"""
K = int(T / DT)
+ print(f"path_visited_points = {path_visited_points}")
+
xref = np.zeros((3, K)) # Reference trajectory for [x, y, theta]
# find the nearest path point to the current state
- ind = get_nn_idx(state, path)
+ ind = get_nn_idx(state, path, path_visited_points)
+
+ print(f"closest point = {path[:, ind]}")
+ path_visited_points.append([path[0, ind], path[1, ind]])
# calculate the cumulative distance along the path
cdist = np.append([0.0], np.cumsum(np.hypot(np.diff(path[0, :]), np.diff(path[1, :]))))
@@ -113,4 +133,6 @@ def get_ref_trajectory(state, path, target_v, T, DT):
xref[2, :] = (xref[2, :] + np.pi) % (2.0 * np.pi) - np.pi
xref[2, :] = fix_angle_reference(xref[2, :], xref[2, 0])
- return xref
\ No newline at end of file
+ print(f"returning path_visited_points = {path_visited_points}")
+
+ return xref, path_visited_points
\ No newline at end of file