Add solver settings, goal weight to traj opt resolver. Delete old unused files

guided_mrmp/conflict_resolvers/db_resolver.py

-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
-

guided_mrmp/conflict_resolvers/local_resolver.py

-"""
-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

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):
         """