Skip to content
Snippets Groups Projects
Commit 50daca1e authored by rachelmoan's avatar rachelmoan
Browse files

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
parent d01ac6af
No related branches found
No related tags found
No related merge requests found
......@@ -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"],
......
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