make the curved path have exactly N points

guided_mrmp/conflict_resolvers/curve_path.py

@@ -3,14 +3,14 @@ import matplotlib.pyplot as plt
 from guided_mrmp.utils import Library
 import sys
 
-from guided_mrmp.conflict_resolvers import TrajOptMultiRobot
-
+# from guided_mrmp.conflict_resolvers import TrajOptMultiRobot
+# 
 # Function to calculate the Bézier curve points
 def bezier_curve(t, control_points):
     P0, P1, P2 = control_points
     return (1 - t)**2 * P0 + 2 * (1 - t) * t * P1 + t**2 * P2
 
-def smooth_path(points, control_point_distance):
+def smooth_path(points, control_point_distance, N=40):
     # List to store the points along the smoothed curve
     smoothed_curve = []
 
@@ -27,8 +27,12 @@ def smooth_path(points, control_point_distance):
         P2 = points[i + 2]
         
         # Calculate the tangent directions at the start and end points
-        tangent_start = (P1 - P0) / np.linalg.norm(P1 - P0)
-        tangent_end = (P2 - P1) / np.linalg.norm(P2 - P1)
+        if np.linalg.norm(P1 - P0) == 0:
+            tangent_start = np.array([0, 0])
+        else: tangent_start = (P1 - P0) / np.linalg.norm(P1 - P0)
+        if np.linalg.norm(P2 - P1) == 0:
+            tangent_end = np.array([0, 0])
+        else: tangent_end = (P2 - P1) / np.linalg.norm(P2 - P1)
         
         # Calculate the control points
         control_point_start = P1 - tangent_start * control_point_distance
@@ -36,13 +40,19 @@ 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, 20)
-        print(t_values)
+        t_values = np.linspace(0, 1, 10)
+        # print(t_values)
         curve_points = np.array([bezier_curve(t, control_points) for t in t_values])
         
         # Append the points along the curve to the smoothed curve list
         smoothed_curve.extend(curve_points[1:])
 
+        smoothed_curve = np.array(smoothed_curve)
+        t_original = np.linspace(0, 1, len(smoothed_curve))
+        t_resampled = np.linspace(0, 1, N)
+        smoothed_curve = np.array([np.interp(t_resampled, t_original, smoothed_curve[:, i]) for i in range(smoothed_curve.shape[1])]).T
+        smoothed_curve = smoothed_curve.tolist()
+
     # 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)