Skip to content
Snippets Groups Projects
Commit 1ebd79e4 authored by rachelmoan's avatar rachelmoan
Browse files

Check against global x_mpc what finding waiting robots

parent d8daf08d
No related branches found
No related tags found
No related merge requests found
......@@ -427,8 +427,9 @@ class MultiPathTrackerDB(MultiPathTracker):
self.control[i]
)
x_mpc_global = self.ego_to_global_roomba(state[i], x_mpc)
u_next[i] = [u_mpc[0, 0], u_mpc[1, 0]]
x_next[i] = x_mpc
x_next[i] = x_mpc_global
self.trajs = targets
return x_next, u_next
......@@ -584,7 +585,9 @@ class MultiPathTrackerDB(MultiPathTracker):
)
u_next[i] = [u_mpc[0, 0], u_mpc[1, 0]]
x_next[i] = x_mpc
x_mpc_global = self.ego_to_global_roomba(state[i], x_mpc)
x_next[i] = x_mpc_global
self.trajs[i] = ref
......@@ -595,18 +598,19 @@ class MultiPathTrackerDB(MultiPathTracker):
all_waiting_robots.extend(c_wait)
# Discrete solver failed, resolve the conflict with coupled MPC
# dynamycs w.r.t robot frame
curr_states = np.zeros((self.num_robots, 3))
x_mpc, u_mpc = self.coupled_mpc.step(
curr_states,
self.trajs,
self.control
)
for i in c:
u_next[i] = [u_mpc[i*2, 0], u_mpc[i*2+1, 0]]
x_next[i] = x_mpc[i]
else:
# dynamycs w.r.t robot frame
curr_states = np.zeros((self.num_robots, 3))
x_mpc, u_mpc = self.coupled_mpc.step(
curr_states,
self.trajs,
self.control
)
for i in c:
u_next[i] = [u_mpc[i*2, 0], u_mpc[i*2+1, 0]]
x_next[i] = [x_mpc[i*3, 1], x_mpc[i*3+1, 1], x_mpc[i*3+2, 1]]
else:
print("The robots are too far apart to place a grid")
print("Proceeding with the current paths")
......@@ -639,7 +643,9 @@ class MultiPathTrackerDB(MultiPathTracker):
for i in range(self.num_robots):
if i in waiting: continue
for j in waiting:
if np.linalg.norm(x_mpc[i][0][:2] - state[j][:2]) < 2*self.radius:
print(f"x_mpc[i] = {x_mpc[i]}")
i_state = [x_mpc[i][0][1], x_mpc[i][1][1]]
if np.linalg.norm(i_state - state[j][:2]) < 2*self.radius:
waiting.append(i)
self.find_all_waiting_robots(waiting, x_mpc)
break
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment