diff --git a/guided_mrmp/conflict_resolvers/db_resolver.py b/guided_mrmp/conflict_resolvers/db_resolver.py
deleted file mode 100644
index 2c128292f07ea31a47b6988f6fde66eb2c161d79..0000000000000000000000000000000000000000
--- a/guided_mrmp/conflict_resolvers/db_resolver.py
+++ /dev/null
@@ -1,36 +0,0 @@
-from guided_mrmp.conflict_resolvers.local_resolver import LocalResolver
-from guided_mrmp.conflict_resolvers.subproblems import SubproblemPlacer
-from .query import QueryDatabase
-
-class DBResolver(LocalResolver):
- def __init__(self, conflicts, all_robots, dt, grid):
- """
- inputs:
- - conflicts (list): list of all conflicts
- - all_robots (list): list of all robots
- - dt (float): time step
- """
- super().__init__(conflicts, all_robots, dt)
- self.grid = grid # obstacle map
-
- def convert_solution_to_local_controls(self, solution):
- raise NotImplementedError
-
-
- def get_local_controls(self):
- controls = []
- for c in self.conflicts:
- # create a subproblem for each conflict
- sp = SubproblemPlacer(self.all_robots, self.grid, c)
- subproblem = sp.place_subproblem()
-
- # solve the subproblem
- query_db = QueryDatabase()
- solution = query_db.query(subproblem)
-
- # get the local controls
- controls = self.convert_solution_to_local_controls(solution)
-
- controls.append(controls)
- return controls
-
diff --git a/guided_mrmp/conflict_resolvers/local_resolver.py b/guided_mrmp/conflict_resolvers/local_resolver.py
deleted file mode 100644
index df8552937fda53d85c2fef780f50b1f5ce9ca0ea..0000000000000000000000000000000000000000
--- a/guided_mrmp/conflict_resolvers/local_resolver.py
+++ /dev/null
@@ -1,28 +0,0 @@
-"""
-ConflictResolver is a local database conflict resolution strategy that resolves
-conflicts in the continuous space by creating a discrete local subprblem
-and solving that subproblem via a query to an exhaustive database of solutions
-"""
-
-import matplotlib.pyplot as plt
-
-class LocalResolver:
-
- def __init__(self, conflicts, all_robots, dt):
- """
- inputs:
- - conflicts (list): list of all conflicts
- - all_robots (list): list of all robots
- - dt (float): time step
- """
- self.conflicts = conflicts
- self.all_robots = all_robots
- self.dt = dt
-
-
- def get_local_controls():
- """
- This function will return the local controls for the robots in conflict.
- It should be implemented by the child classes.
- """
- raise NotImplementedError
diff --git a/guided_mrmp/conflict_resolvers/traj_opt_resolver.py b/guided_mrmp/conflict_resolvers/traj_opt_resolver.py
index aab7dac795fa891bd9d1f32662bc2a301c4f4e8d..99045538d05a77f6f45ac80f03ef222c109e440f 100644
--- a/guided_mrmp/conflict_resolvers/traj_opt_resolver.py
+++ b/guided_mrmp/conflict_resolvers/traj_opt_resolver.py
@@ -9,7 +9,7 @@ class TrajOptResolver():
A class that resolves conflicts using trajectoy optimization.
"""
def __init__(self, num_robots, robot_radius, starts, goals, circle_obstacles, rectangle_obstacles,
- rob_dist_weight, obs_dist_weight, control_weight, time_weight):
+ rob_dist_weight, obs_dist_weight, control_weight, time_weight, goal_weight):
self.num_robots = num_robots
self.starts = starts
self.goals = goals
@@ -20,6 +20,7 @@ class TrajOptResolver():
self.control_weight =control_weight
self.time_weight = time_weight
self.robot_radius = MX(robot_radius)
+ self.goal_weight = goal_weight
def dist(self, robot_position, circle):
"""
@@ -85,8 +86,9 @@ class TrajOptResolver():
for k in range(N):
for c in range(circle_obs.shape[0]):
circle = circle_obs[c, :]
- d = self.dist(pos[2*r : 2*(r+1), k], circle)
- dist_to_other_obstacles += self.apply_quadratic_barrier(2*(self.robot_radius + circle[2]), d, 5)
+ d = sumsqr(pos[2*r : 2*(r+1), k] - transpose(circle[:2])) - 2*self.robot_radius - circle[2]
+ dist_to_other_obstacles += self.apply_quadratic_barrier(3*(self.robot_radius + circle[2]), d, 10)
+
# ------ Robot dist cost ------ #
dist_to_other_robots = 0
@@ -95,7 +97,7 @@ class TrajOptResolver():
for r2 in range(self.num_robots):
if r1 != r2:
d = sumsqr(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k])
- dist_to_other_robots += self.apply_quadratic_barrier(2*self.robot_radius, d, 1)
+ dist_to_other_robots += self.apply_quadratic_barrier(6*self.robot_radius, d, 6)
# ---- dynamics constraints ---- #
@@ -120,12 +122,21 @@ class TrajOptResolver():
for k in range(N-1):
heading_diff_penalty += sumsqr(fmod(heading[:,k+1] - heading[:,k] + pi, 2*pi) - pi)
+
+ # ------ Distance to goal penalty ------ #
+ dist_to_goal = 0
+ for r in range(self.num_robots):
+ # calculate the distance to the goal in the final control interval
+ #
+ dist_to_goal += sumsqr(pos[2*r : 2*(r+1), -1] - self.goals[r])
# ------ cost function ------ #
+
opti.minimize(self.rob_dist_weight*dist_to_other_robots
+ self.obs_dist_weight*dist_to_other_obstacles
+ self.time_weight*T
- + self.control_weight*heading_diff_penalty)
+ + self.control_weight*heading_diff_penalty
+ + 5*dist_to_goal)
# ------ control constraints ------ #
@@ -135,8 +146,8 @@ class TrajOptResolver():
opti.subject_to(sumsqr(omega[r,k]) <= 0.2**2)
# ------ bound x, y, and time ------ #
- opti.subject_to(opti.bounded(x_range[0],x,x_range[1]))
- opti.subject_to(opti.bounded(y_range[0],y,y_range[1]))
+ opti.subject_to(opti.bounded(x_range[0]+self.robot_radius,x,x_range[1]-self.robot_radius))
+ opti.subject_to(opti.bounded(y_range[0]+self.robot_radius,y,y_range[1]-self.robot_radius))
opti.subject_to(opti.bounded(0,T,100))
# ------ initial conditions ------ #
@@ -144,16 +155,16 @@ class TrajOptResolver():
opti.subject_to(heading[r, 0]==self.starts[r][2])
opti.subject_to(pos[2*r : 2*(r+1), 0]==self.starts[r][0:2])
- opti.subject_to(pos[2*r : 2*(r+1), -1]==self.goals[r])
+ # opti.subject_to(sumsqr(pos[2*r : 2*(r+1), -1] - self.goals[r]) < 1**2)
- return {'opti':opti, 'X':X, 'T':T}
+ return {'opti':opti, 'X':X, 'U':U, 'T':T}
def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
opt = Optimizer(problem)
results = opt.solve_optimization_problem(initial_guesses, solver_options)
return results
- def solve(self, N, x_range, y_range, initial_guesses):
+ def solve(self, N, x_range, y_range, initial_guesses=None, solver_options=None):
"""
Setup and solve a multi-robot traj opt problem
@@ -163,10 +174,14 @@ class TrajOptResolver():
- y_range (tuple):
"""
problem = self.problem_setup(N, x_range, y_range)
- results = self.solve_optimization_problem(problem, initial_guesses)
+ results = self.solve_optimization_problem(problem, initial_guesses, solver_options)
+
+ if results['status'] == 'failed':
+ return None, None, None, None, None, None, None
X = results['X']
sol = results['solution']
+ U = results['U']
# Extract the values that we want from the optimizer's solution
pos = X[:self.num_robots*2,:]
@@ -174,7 +189,10 @@ class TrajOptResolver():
y_vals = pos[1::2,:]
theta_vals = X[self.num_robots*2:,:]
- return sol,pos, x_vals, y_vals, theta_vals
+ vels = U[0::2,:]
+ omegas = U[1::2,:]
+
+ return sol,pos, vels, omegas, x_vals, y_vals, theta_vals
def get_local_controls(self, controls):
"""