When turning the db solution into a continuous, first send robot to the first...

When turning the db solution into a continuous, first send robot to the first point from its solution

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -138,7 +138,7 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         return unpadded_grid_solution
     
-    def convert_db_sol_to_continuous(self, state, grid_solution, robots_in_conflict, N, cp_dist, smoothing=0.0):
+    def convert_db_sol_to_continuous(self, state, grid_solution, grid_origin, robots_in_conflict, N, cp_dist, smoothing=0.0):
         """
         Takes a solution from the discrete solver and converts it to a continuous space solution
         The points on the grid solution are treated as points on a bezier curve. 
@@ -153,36 +153,44 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         returns:
             - estimated_state (np.ndarray): the estimated state of the robots in continuous space
-
         """
         num_robots = len(robots_in_conflict)
-        estimated_state = np.zeros((num_robots*3, N+1))
+        estimated_state = np.zeros((num_robots*3, N+2))
 
         for i in range(num_robots):
             points = np.array(grid_solution[i])
             
             # smooth the path using bezier curves and gaussian smoothing
             smoothed_curve, _ = smooth_path(points, N+1, cp_dist, smoothing)
- 
-            estimated_state[i*3, :] = smoothed_curve[:, 0]        # x
-            estimated_state[i*3 + 1, :] = smoothed_curve[:, 1]    # y
-
+            
             current_robot_x_pos = state[robots_in_conflict[i]][0]
             current_robot_y_pos = state[robots_in_conflict[i]][1]
 
+            # insert the current robot position as the first point of the path
+            estimated_state[i*3, 0] = current_robot_x_pos
+            estimated_state[i*3 + 1, 0] = current_robot_y_pos
+            estimated_state[i*3 + 2, 0] = state[robots_in_conflict[i]][2]
+ 
+            estimated_state[i*3, 1:] = smoothed_curve[:, 0]        # x
+            estimated_state[i*3 + 1, 1:] = smoothed_curve[:, 1]    # y
+
+            
+
             # translate the initial guess so that the first point is at (0,0)
-            estimated_state[i*3, :] -= estimated_state[i*3, 0]
-            estimated_state[i*3 + 1, :] -= estimated_state[i*3+1, 0]
+            estimated_state[i*3, 1:] -= estimated_state[i*3, 1]
+            estimated_state[i*3 + 1, 1:] -= estimated_state[i*3+1, 1]
+
+            smoothed_curve_first_point = self.get_grid_cell_location(points[0][0], points[0][1], grid_origin)
 
             # translate the initial guess so that the first point is at the current robot position
-            estimated_state[i*3, :] += current_robot_x_pos
-            estimated_state[i*3 + 1, :] += current_robot_y_pos 
+            estimated_state[i*3, 1:] += smoothed_curve_first_point[0]
+            estimated_state[i*3 + 1, 1:] += smoothed_curve_first_point[1] 
 
             
             headings = calculate_headings(smoothed_curve)
             headings.append(headings[-1])
 
-            estimated_state[i*3 + 2, :] = headings
+            estimated_state[i*3 + 2, 1:] = headings
 
         return estimated_state
 
@@ -214,7 +222,6 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         return initial_guess_control
 
-
     def get_obstacle_map(self, grid_origin, grid_size, radius):
         """
         Create a map of the environment with obstacles
@@ -427,7 +434,8 @@ class MultiPathTrackerDB(MultiPathTracker):
                 if grid_solution:
                     # if we found a grid solution, we can use it to reroute the robots
                     continuous_soln = self.convert_db_sol_to_continuous(state, 
-                                                                        grid_solution, 
+                                                                        grid_solution,
+                                                                        grid_origin, 
                                                                         c,
                                                                         self.settings["database"]["num_intervals"],
                                                                         self.settings["database"]["control_point_dist"],