Fixing weird points that were showing up when curving a path

guided_mrmp/conflict_resolvers/curve_path.py

 import numpy as np
 import matplotlib.pyplot as plt
-from Utils import Library
+from guided_mrmp.utils import Library
 import sys
 
-from LocalPlanners import TrajOptMultiRobot
+from guided_mrmp.conflict_resolvers import TrajOptMultiRobot
 
 # Function to calculate the Bézier curve points
 def bezier_curve(t, control_points):
@@ -15,9 +15,9 @@ def smooth_path(points, control_point_distance):
     smoothed_curve = []
 
     # Connect the first point to the first control point
-    control_point_start = points[0] + (points[1] - points[0]) * control_point_distance
+    # control_point_start = points[0] + (points[1] - points[0]) * control_point_distance
     smoothed_curve.append(points[0])
-    smoothed_curve.append(control_point_start)
+    # smoothed_curve.append(control_point_start)
 
     # Iterate through each set of three consecutive points
     for i in range(len(points) - 2):
@@ -36,7 +36,7 @@ def smooth_path(points, control_point_distance):
         
         # Construct the Bézier curve for the current set of points
         control_points = [control_point_start, P1, control_point_end]
-        t_values = np.linspace(0, 1, 21)
+        t_values = np.linspace(0, 1, 20)
         print(t_values)
         curve_points = np.array([bezier_curve(t, control_points) for t in t_values])
         
@@ -44,8 +44,8 @@ def smooth_path(points, control_point_distance):
         smoothed_curve.extend(curve_points[1:])
 
     # Connect the last control point to the last point
-    control_point_end = points[-1] - (points[-1] - points[-2]) * control_point_distance
-    smoothed_curve.append(control_point_end)
+    # control_point_end = points[-1] - (points[-1] - points[-2]) * control_point_distance
+    # smoothed_curve.append(control_point_end)
     smoothed_curve.append(points[-1])
 
     # Convert smoothed curve points to a numpy array
@@ -59,8 +59,30 @@ if __name__ == "__main__":
 
     rectangle_obs = np.array([])
 
+    # points1 = np.array([[1,6],
+    #           [1,1],
+    #           [9,1]])
+    
+    # points2 = np.array([[9,1],
+    #           [9,6],
+    #           [1,6]])
+
+    # smoothed_curve1 = smooth_path(points1, 3)
+    # smoothed_curve2 = smooth_path(points2, 3)
+
+    # # Plot the original points and the smoothed curve
+    # plt.plot(points1[:, 0], points1[:, 1], 'bo-', label='original path')
+    # plt.plot(smoothed_curve1[:, 0], smoothed_curve1[:, 1], 'r-', label='curved path')
+    # plt.xlabel('X')
+    # plt.ylabel('Y')
+    # # plt.title('Smoothed Curve using Bézier Curves')
+    # plt.legend()
+    # plt.grid(True)
+    # plt.axis('equal')
+    # plt.show()
+
     # Example points
-    lib = Library("2x3_library")
+    lib = Library("guided_mrmp/database/2x3_library")
     lib.read_library_from_file()
 
     robot_starts = [[0, 0], [0, 2], [1, 2]]
@@ -74,26 +96,28 @@ if __name__ == "__main__":
 
         # Condition to filter out rows equal to [-1, -1]
         points = np.array(points)
+        
         condition = (points != [-1, -1]).any(axis=1)
         points = points[condition]
-        print(points)
+        print(f"points = {points}")
 
         # Parameters
         control_point_distance = 0.3  # Distance of control points from the middle point
 
         smoothed_curve = smooth_path(points, control_point_distance)
-        # print(smoothed_curve)
+        print(f"smoothed_curve = {smoothed_curve}")
 
         # Plot the original points and the smoothed curve
-        # plt.plot(points[:, 0], points[:, 1], 'bo-', label='original path')
-        # plt.plot(smoothed_curve[:, 0], smoothed_curve[:, 1], 'r-', label='curved path')
-        # plt.xlabel('X')
-        # plt.ylabel('Y')
-        # # plt.title('Smoothed Curve using Bézier Curves')
-        # plt.legend()
-        # plt.grid(True)
-        # plt.axis('equal')
-        # plt.show()
+        plt.plot(points[:, 0], points[:, 1], 'bo-', label='original path')
+        plt.plot(smoothed_curve[:, 0], smoothed_curve[:, 1], 'r-', label='curved path')
+        plt.xlabel('X')
+        plt.ylabel('Y')
+        # plt.title('Smoothed Curve using Bézier Curves')
+        plt.legend()
+        plt.grid(True)
+        plt.axis('equal')
+        plt.show()
+
 
     # weights for the cost function
     dist_robots_weight = 10
@@ -107,20 +131,20 @@ if __name__ == "__main__":
     N = 20
 
 
-    # initial guess 
-    print(f"N = {N}")
-    initial_guess = np.zeros((num_robots*2,N+1))
-    print(initial_guess)
-    # for i,(start,goal) in enumerate(zip(robot_starts, robot_goals)):
-    for i in range(0,num_robots*2,2):
-        start=robot_starts[int(i/2)]
-        goal=robot_goals[int(i/2)]
-        initial_guess[i,:] = np.linspace(start[0], goal[0], N+1)
-        initial_guess[i+1,:] = np.linspace(start[1], goal[1], N+1)
-        # initial_guess[i+2,:] = np.linspace(.5, .5, N+1)
-        # initial_guess[i+3,:] = np.linspace(.5, .5, N+1)
+    # # 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*2,3):
+    #     start=robot_starts[int(i/2)]
+    #     goal=robot_goals[int(i/2)]
+    #     initial_guess[i,:] = np.linspace(start[0], goal[0], N+1)
+    #     initial_guess[i+1,:] = np.linspace(start[1], goal[1], 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)
+    # print(initial_guess)
 
     
 
@@ -139,4 +163,4 @@ if __name__ == "__main__":
     pos_vals = np.array(sol.value(pos))
 
     
-    solver.plot_paths(pos_vals)
+    solver.plot_paths(pos_vals, initial_guess)