traj_opt testing problems straight from the database

guided_mrmp/conflict_resolvers/traj_opt.py

@@ -2,7 +2,7 @@ import numpy as np
 import matplotlib.pyplot as plt
 from matplotlib.patches import Circle, Rectangle
 from casadi import *
-# from guided_mrmp.conflict_resolvers.curve_path import smooth_path
+from guided_mrmp.conflict_resolvers.curve_path import smooth_path
 
 class TrajOptMultiRobot():
     def __init__(self, num_robots, robot_radius, starts, goals, circle_obstacles, rectangle_obstacles,
@@ -45,9 +45,19 @@ class TrajOptMultiRobot():
     def log_normal_barrier(self, sigma, d, c):
         return c*fmax(0, 2-(d/sigma))**2.5
 
-    def solve(self, num_control_intervals, initial_guess):
+    def problem_setup(self, N, x_range, y_range):
+        """
+        Problem setup for the multi-robot collision resolution traj opt problem
+
+        inputs:
+            - N (int): number of control intervals
+            - x_range (tuple): range of x values
+            - y_range (tuple): range of y values
 
-        N = num_control_intervals
+        outputs:
+            - problem (dict): dictionary containing the optimization problem 
+                              and the decision variables
+        """
         opti = Opti() # Optimization problem
 
         # ---- decision variables --------- #
@@ -57,16 +67,15 @@ class TrajOptMultiRobot():
         y = pos[1::2,:]
         heading = X[self.num_robots*2:,:]           # heading is the last value
 
-        
-        circle_obs = DM(self.circle_obs)            # make the obstacles casadi objects 
-        
         U = opti.variable(self.num_robots*2, N)     # control trajectory (v, omega)
         vel = U[0::2,:]
         omega = U[1::2,:]
         T = opti.variable()                         # final time
 
-
-        # sum up the cost of distance to obstacles
+        # ---- obstacle setup ------------ #
+        circle_obs = DM(self.circle_obs)            # make the obstacles casadi objects 
+        
+        # ------ Obstacle dist cost ------ #
         # TODO:: Include rectangular obstacles
         dist_to_other_obstacles = 0
         for r in range(self.num_robots):
@@ -74,29 +83,22 @@ class TrajOptMultiRobot():
                 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(self.robot_radius + circle[2] + 0.5, d, 1)
-                    # dist_to_other_obstacles += self.log_normal_barrier(5, d, 5)
+                    dist_to_other_obstacles += self.apply_quadratic_barrier(2*(self.robot_radius + circle[2]), d, 5)
 
+        # ------ Robot dist cost ------ #
         dist_to_other_robots = 0
         for k in range(N):
             for r1 in range(self.num_robots):
                 for r2 in range(self.num_robots):
                     if r1 != r2:
-                        # print(f"\n{r1} position1 = {pos[2*r1 : 2*(r1+1), k]}")
-                        # print(f"{r2} position2 = {pos[2*r2 : 2*(r2+1), k]}")
-
-                        # note: using norm 2 here gives an invalid num detected error. 
-                        # Must be the sqrt causing an issue
-                        # d = norm_2(pos[2*r1 : 2*(r1+1), k] - pos[2*r2 : 2*(r2+1), k]) - 2*self.robot_radius
                         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+.5, d, 1)
-                      
-        dt = T/N # length of a control interval
+                        dist_to_other_robots += self.apply_quadratic_barrier(2*self.robot_radius, d, 1)
+
 
-        # print("Initial pos:", pos[:, 0])
-        # print("Initial heading:", heading[:, 0])
+        # ---- dynamics constraints ---- #              
+        dt = T/N # length of a control interval
 
-        pi = [3.14159*2]*self.num_robots
+        pi = [3.14159]*self.num_robots
         pi = np.array(pi)
         pi = DM(pi)
 
@@ -106,62 +108,101 @@ class TrajOptMultiRobot():
             dthetadt = omega[:,k]
             opti.subject_to(x[:,k+1]==x[:,k] + dt*dxdt)
             opti.subject_to(y[:,k+1]==y[:,k] + dt*dydt) 
-            opti.subject_to(heading[:,k+1]==fmod(heading[:,k] + dt*dthetadt, pi))
+            opti.subject_to(heading[:,k+1]==fmod(heading[:,k] + dt*dthetadt, 2*pi))
+
 
+        # ------ Control panalty ------ #
         # Calculate the sum of squared differences between consecutive heading angles
         heading_diff_penalty = 0
         for k in range(N-1):
-            heading_diff_penalty += sumsqr(heading[:,k+1] - heading[:,k])
+            heading_diff_penalty += sumsqr(fmod(heading[:,k+1] - heading[:,k] + pi, 2*pi) - pi)
+
 
+        # ------ cost function ------ #
         opti.minimize(self.rob_dist_weight*dist_to_other_robots 
                     + self.obs_dist_weight*dist_to_other_obstacles 
                     + self.time_weight*T
-                    + 5*heading_diff_penalty)
+                    + self.control_weight*heading_diff_penalty)
 
 
-        # ---- path constraints -----------
+        # ------ control constraints ------ #
         for k in range(N):
             for r in range(self.num_robots):
                 opti.subject_to(sumsqr(vel[r,k]) <= 0.2**2)
                 opti.subject_to(sumsqr(omega[r,k]) <= 0.2**2)
 
-        opti.subject_to(opti.bounded(0,x,10))
-        opti.subject_to(opti.bounded(0,y,10))
-        # opti.subject_to(opti.bounded(-0.05,vel,0.05)) 
-        # opti.subject_to(opti.bounded(-.1,U,.1)) # control is limited
+        # ------ 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(0,T,100))
 
-        # ---- boundary conditions --------
+        # ------ initial conditions ------ #
         for r in range(self.num_robots):
-            # opti.subject_to(vel[r, 0]==0) 
-            opti.subject_to(pos[2*r : 2*(r+1), 0]==self.starts[r])
+            
+            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])
 
-        # --- misc. constraints  --- #
-        opti.subject_to(opti.bounded(0,T,100))
+        return {'opti':opti, 'X':X, 'T':T}
 
-        # --- initial values for solver --- #
-        opti.set_initial(T, 20)
+    def solve_optimization_problem(self, problem, initial_guesses=None, solver_options=None):
+        opti = problem['opti']
         
-        opti.set_initial(X,initial_guess)
-        
-
-
-        # --- solve NLP --- #
-        opti.solver("ipopt") # set numerical backend
-        sol = opti.solve()   # actual solve
+        if initial_guesses:
+            for param, value in initial_guesses.items():
+                print(f"param = {param}")
+                print(f"value = {value}")
+                opti.set_initial(problem[param], value)
+
+        # Set numerical backend, with options if provided
+        if solver_options:
+            opti.solver('ipopt', solver_options)
+        else:
+            opti.solver('ipopt')
+
+        try:
+            sol = opti.solve()   # actual solve
+            status = 'succeeded'
+        except:
+            sol = None
+            status = 'failed'
+
+        results = {
+            'status' : status,
+            'solution' : sol,
+        }
+
+        if sol:
+            for var_name, var in problem.items():
+                if var_name != 'opti':
+                    results[var_name] = sol.value(var)
+
+        return results
+    
+    def solve(self, N, x_range, y_range, initial_guesses):
+        """
+        Setup and solve a multi-robot traj opt problem
 
-        # print(f"pos = {opti.debug.value(pos[2:4,:])}")
+        input: 
+            - N (int): the number of control intervals
+            - x_range (tuple): 
+            - y_range (tuple): 
+        """
+        problem = self.problem_setup(N, x_range, y_range)
+        results = self.solve_optimization_problem(problem, initial_guesses)
 
-        # Extract x and y values
-        x_vals = np.array(sol.value(x))
-        y_vals = np.array(sol.value(y))
+        X = results['X']
+        sol = results['solution']
 
-        # Extract theta values
-        theta_vals = np.array(sol.value(heading))
+        # Extract the values that we want from the optimizer's solution
+        pos = X[:self.num_robots*2,:]               
+        x_vals = pos[0::2,:]                             
+        y_vals = pos[1::2,:]
+        theta_vals = X[self.num_robots*2:,:]
 
         return sol,pos, x_vals, y_vals, theta_vals
 
-    def plot_paths(self, x_opt, initial_guess):
+    def plot_paths(self, x_opt, initial_guess, x_range, y_range):
         fig, ax = plt.subplots()
 
         # Plot obstacles
@@ -171,12 +212,7 @@ class TrajOptMultiRobot():
             # elif len(obstacle) == 4:  # Rectangle
             #     ax.add_patch(Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], color='red'))
 
-        if self.num_robots > 20:
-            colors = plt.cm.hsv(np.linspace(0.2, 1.0, self.num_robots))
-        elif self.num_robots > 10:
-            colors = plt.cm.tab20(np.linspace(0, 1, self.num_robots))
-        else:
-            colors = plt.cm.tab10(np.linspace(0, 1, self.num_robots))
+        colors = plt.cm.Set1(np.linspace(0, 1, self.num_robots))
 
         # Plot robot paths
         for r,color in zip(range(self.num_robots),colors):
@@ -184,31 +220,73 @@ class TrajOptMultiRobot():
             ax.scatter(x_opt[r*2, :], x_opt[r*2+1, :], color=color, s=10 )
             ax.scatter(self.starts[r][0], self.starts[r][1], s=85,color=color)
             ax.scatter(self.goals[r][0], self.goals[r][1], s=85,facecolors='none', edgecolors=color)
-            ax.plot(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, linestyle='--')
-            ax.scatter(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, s=5 )
+            if initial_guess is not None:
+                ax.plot(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, linestyle='--')
+                ax.scatter(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, s=5 )
+
+            plot_roomba(self.starts[r][0], self.starts[r][1], 0, color)
+            # plot_roomba(self.goals[r][0], self.goals[r][1], 0, color)
+
+
+
+        plt.ylim(0, y_range[1])
+        plt.xlim(0,x_range[1])
+        plt.axis("equal")
+        plt.axis("off")
+        
 
-        ax.set_xlabel('X')
-        ax.set_ylabel('Y')
-        ax.legend()
-        ax.set_aspect('equal', 'box')
+        plt.tight_layout()
 
-        plt.ylim(0,10)
-        plt.xlim(0,10)
-        plt.title('Robot Paths')
         plt.grid(False)
         plt.show()
 
+    def plot_paths_db(self, x_opt, initial_guess,x_range, y_range):
+        fig, ax = plt.subplots()
+
+        # Plot obstacles
+        for obstacle in self.circle_obs:
+            # if len(obstacle) == 2:  # Circle
+            ax.add_patch(Circle(obstacle, obstacle[2], color='red'))
+            # elif len(obstacle) == 4:  # Rectangle
+            #     ax.add_patch(Rectangle((obstacle[0], obstacle[1]), obstacle[2], obstacle[3], color='red'))
+
+        colors = plt.cm.Set1(np.linspace(0, 1, self.num_robots))
+
+        # Plot robot paths
+        for r,color in zip(range(self.num_robots),colors):
+            if x_opt is not None:
+                ax.plot(x_opt[r*2, :], x_opt[r*2+1, :], label=f'Robot {r+1}', color=color)
+                ax.scatter(x_opt[r*2, :], x_opt[r*2+1, :], color=color, s=10 )
+            ax.scatter(self.starts[r][0], self.starts[r][1], s=85,color=color)
+            ax.scatter(self.goals[r][0], self.goals[r][1], s=135,facecolors='none', edgecolors=color)
+            if initial_guess is not None:
+                ax.plot(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, linestyle='--')
+                ax.scatter(initial_guess[r*3, :], initial_guess[r*3+1, :], color=color, s=5 )
 
-def plot_sim(x_histories, y_histories, h_histories):
+            if x_opt is not None: plot_roomba(self.starts[r][0], self.starts[r][1], 0, color)
+            # plot_roomba(self.goals[r][0], self.goals[r][1], 0, color)
 
-    if len(x_histories) > 20:
-        colors = plt.cm.hsv(np.linspace(0.2, 1.0, len(x_histories)))
-    elif len(x_histories) > 10:
-        colors = plt.cm.tab20(np.linspace(0, 1, len(x_histories)))
-    else:
-        colors = plt.cm.tab10(np.linspace(0, 1, len(x_histories)))
 
-    
+
+        plt.ylim(0, y_range[1])
+        plt.xlim(0,x_range[1])
+        plt.axis("equal")
+        # plt.axis("off")
+        
+
+        plt.tight_layout()
+
+        plt.grid(False)
+        plt.show()
+
+
+def plot_sim(x_histories, y_histories, h_histories, x_range, y_range):
+
+    x_histories = np.array(x_histories)
+    y_histories = np.array(y_histories)
+    h_histories = np.array(h_histories)
+
+    colors = plt.cm.Set1(np.linspace(0, 1, len(x_histories)))
 
 
     longest_traj = max([len(x) for x in x_histories])
@@ -234,17 +312,23 @@ def plot_sim(x_histories, y_histories, h_histories):
                 plot_roomba(x_history[-1], y_history[-1], h_history[-1], color)
 
         
-        ax = plt.gca()
-        ax.set_xlim([0, 10])
-        ax.set_ylim([0, 10])
+        plt.ylim(0, y_range[1])
+        plt.xlim(0,x_range[1])
+        plt.axis("equal")
+        # plt.axis("off")
+        
+
         plt.tight_layout()
 
+        plt.grid(False)
+
         plt.draw()
+        plt.savefig(f"frames/sim_{i}.png")
         plt.pause(0.2)
     input()
 
 
-def plot_roomba(x, y, yaw, color):
+def plot_roomba(x, y, yaw, color, radius=.5):
     """
 
     Args:
@@ -252,37 +336,114 @@ def plot_roomba(x, y, yaw, color):
         y ():
         yaw ():
     """
-    LENGTH = 0.5  # [m]
-    WIDTH = 0.25  # [m]
-    OFFSET = LENGTH  # [m]
-
     fig = plt.gcf()
     ax = fig.gca()
-    circle = plt.Circle((x, y), .5, color=color, fill=False)
+    circle = plt.Circle((x, y), radius, color=color, fill=False)
     ax.add_patch(circle)
 
     # Plot direction marker
-    dx = 1 * np.cos(yaw)
-    dy = 1 * np.sin(yaw)
-    ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')
+    dx = radius * np.cos(yaw)
+    dy = radius * np.sin(yaw)
+    ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.05, fc='r', ec='r')
+
+def generate_prob_from_db(N, cp_dist=.5):
+    from guided_mrmp.utils import Library
+    import random
+    lib = Library("guided_mrmp/database/5x2_library")
+    lib.read_library_from_file()
+
+    d = lib.key_to_idx
+
+    # get a random key from the library
+    key, idx = random.choice(list(d.items()))
+
+    print(key)
+    print(len(key))
+
+    num_robots = len(key) // 4
+
+    start_nodes = []
+    goal_nodes = []
+
+    for i in range(0, len(key), 4):
+        start = [int(key[i]), int(key[i+1])]
+        goal = [int(key[i+2]), int(key[i+3])]
+        start_heading = np.arctan2(goal[1] - start[1], goal[0] - start[0])
+        start.append(start_heading)
+        start_nodes.append(start)
+        goal_nodes.append(goal)
+
+
+    sol = lib.get_matching_solution(start_nodes, goal_nodes)
+
+    print(f"sol = {sol}")
 
+    # turn this solution into an initial guess 
+    initial_guess = np.zeros((num_robots*3, N+1))
+    for i in range(num_robots):
 
+        print(f"Robot {i+1} solution:")
+        rough_points = np.array(sol[i])
+        points = []
+        for point in rough_points:
+            if point[0] == -1: break
+            points.append(point)
+        
+        points = np.array(points)
+        print(f"points = {points}")
+
+        smoothed_curve = smooth_path(points, cp_dist, N)
+        print(f"smoothed_curve = {smoothed_curve}")
+        initial_guess[i*3, :] = smoothed_curve[:, 0]        # x
+        initial_guess[i*3 + 1, :] = smoothed_curve[:, 1]    # y
+
+        for j in range(N):
+            dx = smoothed_curve[j+1, 0] - smoothed_curve[j, 0]
+            dy = smoothed_curve[j+1, 1] - smoothed_curve[j, 1]
+            initial_guess[i*3 + 2, j] = np.arctan2(dy, dx)
+
+        # initial_guess[i*3 + 2, :] = np.arctan2(np.diff(smoothed_curve[:, 1]), 
+        #                                        np.diff(smoothed_curve[:, 0]))
+
+    
+
+
+    print(sol)
+
+    for i in range(num_robots):
+        print(f"Robot {i+1} initial guess:")
+        print(f"x: {initial_guess[i*3, :]}")
+        print(f"y: {initial_guess[i*3 + 1, :]}")
+        print(f"theta: {initial_guess[i*3 + 2, :]}")
+
+    return start_nodes, goal_nodes, initial_guess
 
 if __name__ == "__main__":
 
+    import os
+    import numpy as np
+    import random
+
+    seed = 1123581
+    seed = 112
+    print(f"***Setting Python Seed {seed}***")
+    os.environ['PYTHONHASHSEED'] = str(seed)
+    np.random.seed(seed)
+    random.seed(seed)
+
     # define obstacles
-    circle_obs = np.array([[5,5,1],
-                           [7,7,1],
-                           [3,3,1]])
+    circle_obs = np.array([[5,3,1]])
     
 
-    circle_obs = np.array([])
+    # circle_obs = np.array([])
 
     rectangle_obs = np.array([])
 
     #  define all the robots' starts and goals
     robot_starts = [[1,6],[9,1],[2,2],[1,3]]
     robot_goals = [[9,1],[1,6],[8,8],[7,3]]
+
+    
     # robot_starts = [[9,5]]
     # robot_goals = [[1,5]]
 
@@ -294,76 +455,38 @@ if __name__ == "__main__":
 
     # other params
     num_robots = 4
-    rob_radius = 0.25
-    N = 20
-
+    rob_radius = .75
+    N = 30
 
-    # ---- straight line initial guess ---- # 
-    print(f"N = {N}")
-    initial_guess = np.zeros((num_robots*3,N+1))
-    print(initial_guess)
-    # for i,(start,goal) in enumerate(zip(robot_starts, robot_goals)):
-    for i in range(0,num_robots*3,3):
-        # print(f"i = {i}")
-        start=robot_starts[int(i/3)]
-        goal=robot_goals[int(i/3)]
-        # print(f"start = {start}")
-        # print(f"goal = {goal}")
-        initial_guess[i,:] = np.linspace(start[0], goal[0], N+1)
-        initial_guess[i+1,:] = np.linspace(start[1], goal[1], N+1)
-        dx = goal[0] - start[0]
-        dy = goal[1] - start[1]
-        initial_guess[i+2,:] = np.arctan2(dy,dx)*np.ones(N+1)
-        # initial_guess[i+2,:] = np.linspace(.5, .5, N+1)
-        # initial_guess[i+3,:] = np.linspace(.5, .5, N+1)
-
-    print(initial_guess)
-
-    # jagged initial guess 
-    # initial_guess = np.array([
-    #     [1, 1, 1, 1, 1, 1, 2, 3, 4, 5, 6, 7, 8, 9, 9, 9, 9, 9, 9, 9, 9],
-    #     [6, 5, 4, 3, 2, 1, 1, 1,1,1,1,1,1,1,1,1,1,1,1,1,1],
-    #     [0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 ],
-    #     [9, 9, 9, 9, 9, 9, 8 ,7, 6, 5, 4, 3, 2, 1, 1, 1, 1, 1, 1, 1, 1],
-    #     [1, 2, 3, 4, 5, 6, 6, 6,6,6,6,6,6,6,6,6,6,6,6,6,6],
-    #     [0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 ]
-    #     ])
-
-    # points1 = np.array([[1,6],
-    #           [1,1],
-    #           [9,1]])
-    
-    # points2 = np.array([[9,1],
-    #           [9,6],
-    #           [1,6]])
-    
-    # points3 = np.array([[2,2],
-    #           [4,4],
-    #           [8,8]])
-    
-    # points4 = np.array([[1,3],
-    #           [3,3],
-    #           [7,3]])
+    robot_starts, robot_goals, initial_guess = generate_prob_from_db(N)
 
-    # smoothed_curve1 = smooth_path(points1, 3)
-    # smoothed_curve2 = smooth_path(points2, 3)
-    # smoothed_curve3 = smooth_path(points3, 3)
-    # smoothed_curve4 = smooth_path(points4, 3)
+    num_robots = len(robot_starts)
 
+    h = 2
+    x_range = (0, 5*h)
+    y_range = (0, 2*h)
+    robot_starts = np.array(robot_starts)
+    robot_goals = np.array(robot_goals)
+    robot_starts = robot_starts*h + .5*h
+    robot_goals = robot_goals*h + .5*h
+    initial_guess = initial_guess*h + .5*h
 
-    # print(f"smoothed_curve = {smoothed_curve}")
+    print(f"robot_starts = {robot_starts}")
+    print(f"robot_goals = {robot_goals}")
 
-    # initial_guess = np.zeros((num_robots*3,N+1))
-    # initial_guess[0,:] = smoothed_curve1[:,0]
-    # initial_guess[1,:] = smoothed_curve1[:,1]
-    # initial_guess[3,:] = smoothed_curve2[:,0]
-    # initial_guess[4,:] = smoothed_curve2[:,1]
-    # initial_guess[6,:] = smoothed_curve3[:,0]
-    # initial_guess[7,:] = smoothed_curve3[:,1]
-    # initial_guess[9,:] = smoothed_curve4[:,0]
-    # initial_guess[10,:] = smoothed_curve4[:,1]
+    # ---- straight line initial guess ---- # 
+    straight_line = False
+    if straight_line:
+        initial_guess = np.zeros((num_robots*3,N+1))
+        for i in range(0,num_robots*3,3):
+            start=robot_starts[int(i/3)]
+            goal=robot_goals[int(i/3)]
+            initial_guess[i,:] = np.linspace(start[0], goal[0], N+1)
+            initial_guess[i+1,:] = np.linspace(start[1], goal[1], N+1)
+            dx = goal[0] - start[0]
+            dy = goal[1] - start[1]
+            initial_guess[i+2,:] = np.arctan2(dy,dx)*np.ones(N+1)
 
-    
 
     solver = TrajOptMultiRobot(num_robots=num_robots, 
                                robot_radius=rob_radius,
@@ -376,13 +499,21 @@ if __name__ == "__main__":
                                control_weight=control_costs_weight,
                                time_weight=time_weight
                                )
-    sol,pos, xs, ys, thetas = solver.solve(N, initial_guess)
+    
+    initial_guesses = {
+        'X': initial_guess,
+        'T': 20
+    }
+
+    solver.plot_paths_db(None, initial_guess, x_range, y_range)
+    sol,pos, xs, ys, thetas = solver.solve(N, x_range, y_range, initial_guesses)
     pos_vals = np.array(sol.value(pos))
 
-    
-    solver.plot_paths(pos_vals, initial_guess)
+    solver.plot_paths_db(None, initial_guess, x_range, y_range)
+    solver.plot_paths_db(pos_vals, None, x_range, y_range)
+    plot_sim(xs, ys, thetas, x_range, y_range)
 
-    print(pos_vals)
+    # print(pos_vals)
 
 
 
@@ -392,6 +523,6 @@ if __name__ == "__main__":
     #     ys.append(pos_vals[r*2+1, :])
     #     thetas.append(pos_vals[num_robots*2 + r, :])
 
-    plot_sim(xs, ys, thetas)
+    # plot_sim(xs, ys, thetas)