Fix the weird oscillations in trajectory plotting. Was caused by a rounding error.

guided_mrmp/planners/db_guided_mrmp.py

@@ -18,32 +18,8 @@ from guided_mrmp.controllers.utils import get_ref_trajectory, compute_path_from_
 
 from guided_mrmp.controllers.mpc import MPC
 
-from guided_mrmp.utils.library import Library
-
-def initialize_libraries(library_fnames=["guided_mrmp/database/2x3_library","guided_mrmp/database/3x3_library","guided_mrmp/database/5x2_library"]):
-    """
-    Load the 2x3, 3x3, and 2x5 libraries. Store them in self.lib-x- 
-    Inputs: 
-        library_fnames - the folder containing the library files
-    """
-    # Create each of the libraries
-    print(f"Loading libraries. This usually takes about 10 seconds...")
-    lib_2x3 = Library(library_fnames[0])
-    lib_2x3.read_library_from_file()
-    
-    lib_3x3 = Library(library_fnames[1])
-    lib_3x3.read_library_from_file()
-
-    
-    lib_2x5 = Library(library_fnames[2])
-    lib_2x5.read_library_from_file()
-
-    return [lib_2x3, lib_3x3, lib_2x5]
-
-
-
 class GuidedMRMP:
-    def __init__(self, env, robots, dynamics_models, T, DT, settings):
+    def __init__(self, env, robots, dynamics_models, T, DT, libs,settings):
         """
         inputs:
             - robots (list): list of Robot class objects
@@ -79,7 +55,7 @@ class GuidedMRMP:
 
         self.scaling_factor = self.settings['simulator']['scaling_factor']
 
-        self.libs = initialize_libraries()
+        self.libs = libs
 
     def ego_to_global(self, robot, mpc_out):
         """
@@ -219,6 +195,8 @@ class GuidedMRMP:
             curr_state = np.array([0, 0, 0])
             x_mpc, u_mpc = mpc.step(curr_state, target_traj, np.array(r.control))
 
+            
+
             self.add_vis_target_traj(screen, self.scaling_factor, r, x_mpc)
             
             # only the first one is used to advance the simulation
@@ -228,6 +206,12 @@ class GuidedMRMP:
 
             # print(f"control = {u_mpc}")
             next_trajs.append(self.ego_to_global(r, x_mpc))
+
+        # use matplotlib to plot the trajectories
+        # for r, traj in zip(self.robots, next_trajs):
+        #     plt.plot(traj[0], traj[1], label=r.label)
+        # plt.legend()
+        # plt.show()
         return next_controls, next_trajs
 
     def advance(self, screen, state, time, dt=0.1):
@@ -241,11 +225,14 @@ class GuidedMRMP:
         """
 
         # get the next control for each robot
-        next_desired_controls, trajectories = self.get_next_controls_and_trajectories(screen)
+        if self.settings['model_predictive_controller']['parallelize']:
+            next_desired_controls, trajectories = self.get_next_controls_and_trajectories_parallel(screen)
+        else:
+            next_desired_controls, trajectories = self.get_next_controls_and_trajectories(screen)
 
         # find all the conflicts at the next time horizon
-        conflicts = self.find_all_conflicts(trajectories, dt)
-        conflicts = []
+        # conflicts = self.find_all_conflicts(trajectories, dt)
+        conflicts=[]
 
         if len(conflicts) == 0:
             # no conflicts, return the desired controls
@@ -262,7 +249,7 @@ class GuidedMRMP:
         # resolve the conflicts using the database
         # resolver = TrajOptDBResolver(10, 60, conflicts, self.robots)
         rad = self.robots[0].radius
-        resolver = TrajOptDBResolver(rad*2, 5, conflicts, self.robots, trajectories, dt, rad,self.env,1,1,1,self.libs[0],self.libs[1],self.libs[2]) 
+        resolver = TrajOptDBResolver(rad*2, 5, conflicts, self.robots, trajectories, dt, rad,self.env,10,10,5,self.libs[0],self.libs[1],self.libs[2]) 
         next_desired_controls, trajs = resolver.get_local_controls(next_desired_controls)
 
         self.add_vis_conflict_free_traj(screen, self.scaling_factor, trajs)
@@ -283,7 +270,7 @@ class GuidedMRMP:
 
         # return the valid controls
         for r, next_control in zip(self.robots, next_desired_controls):
-            r.control = [next_control[0][-1], next_control[1][-1]]
+            r.control = next_control
         
         return True, next_desired_controls
 
@@ -323,14 +310,14 @@ class GuidedMRMP:
         """
         Add the visualization to the screen.
         """
+        # scaling_factor = 1
         traj = self.ego_to_global(robot, traj)
         for i in range(len(traj[0])-1):
-            x = int(traj[0,i])
-            y = int(traj[1,i])
-            next_x = int(traj[0,i+1])
-            next_y = int(traj[1,i+1])
-            pygame.draw.line(screen, (255,0,0), (x*scaling_factor,y*scaling_factor), 
-                             (next_x*scaling_factor,next_y*scaling_factor), 2)
+            x = traj[0,i]*scaling_factor
+            y = traj[1,i]*scaling_factor
+            next_x = traj[0,i+1]*scaling_factor
+            next_y = traj[1,i+1]*scaling_factor
+            pygame.draw.line(screen, (255,0,0), (x,y), (next_x,next_y), 3)
 
     def add_vis_conflict_free_traj(self,screen, scaling_factor, trajs):
         """

guided_mrmp/simulator.py

@@ -37,24 +37,24 @@ class Simulator:
         """
         conflict_detected, controls = self.policy.advance(screen,self.state, self.time, dt)
 
-        if not conflict_detected:
-            for i in range(self.num_robots):
-                new_state = self.dynamics_models[i].next_state(self.state[i], controls[i], dt)
-                self.robots[i].current_position = new_state
-                self.state[i] = new_state
-            self.time += dt
+        # if not conflict_detected:
+        for i in range(self.num_robots):
+            new_state = self.dynamics_models[i].next_state(self.state[i], controls[i], dt)
+            self.robots[i].current_position = new_state
+            self.state[i] = new_state
+        self.time += dt
 
-        if conflict_detected:
-            print("Executing full trajectory")
-            for i in range(self.num_robots):
-                controls_list = controls[i]
-                for j in range(len(controls_list)):
-                    next_control = [controls_list[0][j], controls_list[1][j]]
-                    print(f"next control = {next_control}")
-                    new_state = self.dynamics_models[i].next_state(self.state[i], next_control, dt)
-                    self.robots[i].current_position = new_state
-                    self.state[i] = new_state
-            self.time += dt
+        # if conflict_detected:
+        #     print("Executing full trajectory")
+        #     for i in range(self.num_robots):
+        #         controls_list = controls[i]
+        #         for j in range(len(controls_list)):
+        #             next_control = [controls_list[0][j], controls_list[1][j]]
+        #             print(f"next control = {next_control}")
+        #             new_state = self.dynamics_models[i].next_state(self.state[i], next_control, dt)
+        #             self.robots[i].current_position = new_state
+        #             self.state[i] = new_state
+        #     self.time += dt
 
     def draw_environment(self, screen):
         screen.fill((255,255,255))