Collision check over horizon instead of dt

guided_mrmp/planners/multirobot/db_guided_mrmp.py

@@ -85,34 +85,37 @@ class GuidedMRMP:
 
         return trajectory
     
-    def find_all_conflicts(self, desired_controls, dt):
+    def find_all_conflicts(self, desired_controls, desired_trajs, dt):
         """
-        Loop over all the robots, checking for both node conflicts and edge
-        conflicts. 
+        Check for any conflicts that will occur between robots in the next time horizon
         """
         conflicts = []
 
         for r1_idx, r1 in enumerate(self.robots):
-            control = desired_controls[r1_idx]
-            print(f"next control = {control}")
-            next_state = self.dynamics_models[r1_idx].next_state(r1.current_position, control, dt)
-
-            circ1 = Point(next_state[0],next_state[1])
-            circ1 = circ1.buffer(r1.radius)
+            # control = desired_controls[r1_idx]
+            traj1 = desired_trajs[r1_idx]
+            traj1 = list(zip(traj1[0],traj1[1]))
+            
             for r2_idx, r2 in enumerate(self.robots):
                 if r1.label == r2.label:
                     continue
 
-                control = desired_controls[r2_idx]
-                next_state = self.dynamics_models[r2_idx].next_state(r2.current_position, control, dt)
-                circ2 = Point(next_state[0],next_state[1])
-                circ2 = circ2.buffer(r2.radius)
+                # control = desired_controls[r2_idx]
+                traj2 = desired_trajs[r2_idx]
+                traj2 = list(zip(traj2[0],traj2[1]))
+
+                for p1, p2 in zip(traj1, traj2):
+                    circ1 = Point(p1[0],p1[1])
+                    circ1 = circ1.buffer(r1.radius)
+                    circ2 = Point(p2[0],p2[1])
+                    circ2 = circ2.buffer(r2.radius)
+                    # check if the robots overlap
+                    if circ1.intersects(circ2): 
+                        if (r1,r2) not in conflicts and (r2,r1) not in conflicts:
+                            print(f"conflict between {r1.label} and {r2.label}")
+                            conflicts.append((r1,r2))
+                
                 
-                # check if the robots overlap
-                if circ1.intersects(circ2): 
-                    if (r1,r2) not in conflicts and (r2,r1) not in conflicts:
-                        print(f"conflict between {r1.label} and {r2.label}")
-                        conflicts.append((r1,r2))
 
         return conflicts
 
@@ -122,12 +125,13 @@ class GuidedMRMP:
                 return False
         return True
 
-    def get_next_controls(self,screen):
+    def get_next_controls_and_trajectories(self,screen):
         """
         Get the next control for each robot.
         """
 
         next_controls = []
+        next_trajs = []
         for idx, r in enumerate(self.robots):
 
             state = r.current_position
@@ -141,17 +145,11 @@ class GuidedMRMP:
                                              self.DT)
             
             
-            
-            
             mpc = MPC(self.dynamics_models[idx], self.T, self.DT, self.Q, self.Qf, self.R, self.P)
 
             # dynamics w.r.t robot frame
             curr_state = np.array([0, 0, 0])
-            x_mpc, u_mpc = mpc.step(
-                curr_state,
-                target_traj,
-                r.control
-            )
+            x_mpc, u_mpc = mpc.step(curr_state, target_traj, r.control)
 
             self.add_vis_target_traj(screen, r, x_mpc)
             
@@ -159,7 +157,8 @@ class GuidedMRMP:
             control = [u_mpc.value[0, 0], u_mpc.value[1, 0]]
             
             next_controls.append(np.asarray(control))
-        return next_controls
+            next_trajs.append(self.ego_to_global(r, x_mpc.value))
+        return next_controls, next_trajs
 
 
     def advance(self, screen, state, time, dt=0.1):
@@ -173,10 +172,10 @@ class GuidedMRMP:
         """
 
         # get the next control for each robot
-        next_desired_controls = self.get_next_controls(screen)
+        next_desired_controls, trajectories = self.get_next_controls_and_trajectories(screen)
 
         # find all the conflicts at the next timestep
-        # conflicts = self.find_all_conflicts(next_desired_controls, dt)
+        conflicts = self.find_all_conflicts(next_desired_controls, trajectories, dt)
 
         # resolve the conflicts using the database
         # resolver = TrajOptDBResolver(10, 60, conflicts, self.robots)