improve how we construct bezier curve for initial guess.

guided_mrmp/conflict_resolvers/curve_path.py

 import numpy as np
 import matplotlib.pyplot as plt
-from guided_mrmp.utils import Library
-import sys
-
-# 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, N=40):
-    # List to store the points along the smoothed curve
-    smoothed_curve = []
-
-    # Connect the first point to the first control point
-    # control_point_start = points[0] + (points[1] - points[0]) * control_point_distance
-    smoothed_curve.append(points[0])
-    # smoothed_curve.append(control_point_start)
-
-    # Iterate through each set of three consecutive points
-    for i in range(len(points) - 2):
-        # Extract the three consecutive points
-        P0 = points[i]
-        P1 = points[i + 1]
-        P2 = points[i + 2]
-        
-        # Calculate the tangent directions at the start and end points
-        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
-        control_point_end = P1 + tangent_end * 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, 10)
-        # print(t_values)
-        curve_points = np.array([bezier_curve(t, control_points) for t in t_values])
+import math
+from scipy.ndimage import gaussian_filter1d
+
+def bezier(t, points):
+    """Calculate Bezier curve point for parameter t and given control points."""
+    n = len(points) - 1
+    return sum(
+        (math.comb(n, i) * (1 - t) ** (n - i) * t ** i * points[i] for i in range(n + 1)),
+        np.zeros(2)
+    )
+
+def smooth_path(points, N=100, alpha=0.25, sigma=1.0):
+    smooth_points = []
+    control_points = []
+    i = 0
+    while i < len(points) - 1:
+        if i < len(points) - 3 and is_bend(points[i:i+4]):
+            # Double bend (cubic bezier with two softened control points)
+            p0, p1, p2, p3 = np.array(points[i]), np.array(points[i+1]), np.array(points[i+2]), np.array(points[i+3])
+            cp1 = soften_control_point(p1, alpha, p0, p2)  # Soften the first control point
+            cp2 = soften_control_point(p2, alpha, p1, p3)  # Soften the second control point
+            control_points.extend([cp1, cp2])  # Collect control points for visualization
+            for t in np.linspace(0, 1, 20):
+                smooth_points.append(bezier(t, [p0, cp1, cp2, p3]))
+            i += 3
+        elif i < len(points) - 2 and is_bend(points[i:i+3]):
+            # Single bend (quadratic bezier with one softened control point)
+            p0, p1, p2 = np.array(points[i]), np.array(points[i+1]), np.array(points[i+2])
+            cp = soften_control_point(p1, alpha, p0, p2)  # Use refined softening
+            control_points.append(cp)  # Collect control point for visualization
+            for t in np.linspace(0, 1, 20):
+                smooth_points.append(bezier(t, [p0, cp, p2]))
+            i += 2
+        else:
+            # No bend, interpolate straight line
+            smooth_points.append(points[i])
+            i += 1
+
+    # Ensure start and end points are included
+    smooth_points = [points[0]] + smooth_points + [points[-1]]
+
+    # Apply Gaussian smoothing to soften remaining sharp transitions
+    smooth_points = np.array(smooth_points)
+    smooth_points[:, 0] = gaussian_filter1d(smooth_points[:, 0], sigma=sigma)
+    smooth_points[:, 1] = gaussian_filter1d(smooth_points[:, 1], sigma=sigma)
+
+    # Downsample to N points, preserving start and end points
+    indices = np.linspace(0, len(smooth_points) - 1, N).astype(int)
+    downsampled_points = smooth_points[indices]
+    downsampled_points[0], downsampled_points[-1] = points[0], points[-1]
+
+    return downsampled_points, np.array(control_points)
+
+def soften_control_point(middle_point, alpha, prev_point, next_point):
+    """Move middle point along the bisector away from the 90-degree angle."""
+
+    middle_point = np.array(middle_point, dtype=np.float64)
+    prev_point = np.array(prev_point, dtype=np.float64)
+    next_point = np.array(next_point, dtype=np.float64)
+
+    # Vectors from middle point to adjacent points
+    vec1 = prev_point - middle_point
+    vec2 = next_point - middle_point
+
+    # Normalize the vectors
+    vec1 /= np.linalg.norm(vec1)
+    vec2 /= np.linalg.norm(vec2)
+
+    # Calculate the bisector direction
+    bisector = vec1 + vec2
+    bisector /= np.linalg.norm(bisector)  # Normalize bisector
+
+    # Move middle point along the bisector direction
+    adjusted_point = middle_point + alpha * bisector
+    return adjusted_point
+
+def is_bend(segment):
+    """Check if three or four points form a 90-degree bend."""
+    if len(segment) == 3:
+        return np.cross(segment[1] - segment[0], segment[2] - segment[1]) != 0
+    elif len(segment) == 4:
+        return (np.cross(segment[1] - segment[0], segment[2] - segment[1]) != 0 and
+                np.cross(segment[2] - segment[1], segment[3] - segment[2]) != 0)
+    return False
+
+def calculate_headings(path_points):
+    """
+    Calculate headings for each segment in the path, allowing for reverse movement.
+
+    Parameters:
+        path_points (np.ndarray): Array of (x, y) points representing the smoothed path.
+
+    Returns:
+        headings (list): List of headings (in radians) for each segment in the path.
+    """
+    headings = []
+    prev_heading = None
+
+    for i in range(len(path_points) - 1):
+        # Calculate forward and reverse headings for each segment
+        p1, p2 = path_points[i], path_points[i + 1]
+        forward_heading = np.arctan2(p2[1] - p1[1], p2[0] - p1[0])
+
         
-        # 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()
+        reverse_heading = (forward_heading + np.pi) % (2 * np.pi)
+
+        # Choose direction based on previous heading to minimize angle change
+        if prev_heading is not None:
+            forward_diff = np.abs((forward_heading - prev_heading + np.pi) % (2 * np.pi) - np.pi)
+            reverse_diff = np.abs((reverse_heading - prev_heading + np.pi) % (2 * np.pi) - np.pi)
+
+
 
-    # 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)
-    smoothed_curve.append(points[-1])
+            chosen_heading = forward_heading if forward_diff <= reverse_diff else reverse_heading
+        else:
+            # If it's the first segment, choose forward heading by default
+            chosen_heading = forward_heading
 
-    # Convert smoothed curve points to a numpy array
-    return np.array(smoothed_curve)
 
+        # plot the two points and the heading
+        # import matplotlib.pyplot as plt
+        # plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'ro-')  # Plot the two points
+        # dx = 0.1 * np.cos(forward_heading)
+        # dy = 0.1 * np.sin(forward_heading)
+        # plt.arrow(p1[0], p1[1], dx, dy, head_width=0.01, head_length=0.1, fc='blue', ec='blue')
+        # dx = 0.1 * np.cos(reverse_heading)
+        # dy = 0.1 * np.sin(reverse_heading)
+        # plt.arrow(p1[0], p1[1], dx, dy, head_width=0.01, head_length=0.1, fc='green', ec='green')
+        # plt.show()
+
+        headings.append(chosen_heading)
+        prev_heading = chosen_heading
+
+    return headings
 
 if __name__ == "__main__":
+    # Example points and visualization
+    points = np.array([
+        [0, 0], [0, 1], [1, 1], [2, 1], [2, 2], [2, 3], [2, 2], [2, 1], [2,0]
+    ])
+
+    # points = np.array([
+    #     [0, 0], [0, 1], [1, 1], [1,0], [0,0]
+    # ])
 
-    # define obstacles
-    circle_obs = np.array([])
+    # Generate smooth curve and get control points
+    N = 20
+    smooth_points, control_points = smooth_path(points, N, alpha=-.5, sigma=0.8)
 
-    rectangle_obs = np.array([])
+    print(f"smooth_points = {smooth_points}")
 
-    # points1 = np.array([[1,6],
-    #           [1,1],
-    #           [9,1]])
-    
-    # points2 = np.array([[9,1],
-    #           [9,6],
-    #           [1,6]])
+    # Example usage with a smooth path
+    # path_points, control_points = smooth_path(points, N=100, alpha=0.2, sigma=1.5)
+    headings = calculate_headings(smooth_points)
 
-    # smoothed_curve1 = smooth_path(points1, 3)
-    # smoothed_curve2 = smooth_path(points2, 3)
+    # Displaying the headings
+    for i, heading in enumerate(headings):
+        print(f"Segment {i}: Heading = {np.degrees(heading):.2f} degrees")
 
-    # # 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()
+    # Plotting
+    plt.figure(figsize=(8, 8))
+    plt.plot(smooth_points[:, 0], smooth_points[:, 1], 'b-', label="Bezier Smooth Path")
 
-    # Example points
-    lib = Library("guided_mrmp/database/2x3_library")
-    lib.read_library_from_file()
+    plt.scatter(points[:, 0], points[:, 1], color="purple", marker="x", s=100, label="Control Points")
 
-    robot_starts = [[0, 0], [0, 2], [1, 2]]
-    robot_goals = [[0, 1],[1, 2], [0, 2]]
-    sol = lib.get_matching_solution(robot_starts, robot_goals)
+    # Add circles and headings
+    for i, (x, y) in enumerate(smooth_points):
+        plt.plot(x, y, 'ro')  # Circle at each point
+        if i < len(headings):
+            heading = headings[i]
+            dx = 0.1 * np.cos(heading)
+            dy = 0.1 * np.sin(heading)
+            plt.arrow(x, y, dx, dy, head_width=0.05, head_length=0.1, fc='green', ec='green')
 
-    print(sol)
+    plt.xlabel("X")
+    plt.ylabel("Y")
+    plt.title("Smoothed Path with Control Points and Headings")
+    plt.legend()
+    plt.grid(True)
+    plt.show()
 
-    for points in sol:
 
 
-        # Condition to filter out rows equal to [-1, -1]
-        points = np.array(points)
-        
-        condition = (points != [-1, -1]).any(axis=1)
-        points = points[condition]
-        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(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()
-
-
-    # weights for the cost function
-    dist_robots_weight = 10
-    dist_obstacles_weight = 10
-    control_costs_weight = 1.0
-    time_weight = 5.0
-
-    # other params
-    num_robots = 3
-    rob_radius = 0.25
-    N = 20
 
 
-    # # 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)
-
-    
-
-    # solver = TrajOptMultiRobot(num_robots=num_robots, 
-    #                            robot_radius=rob_radius,
-    #                            starts=robot_starts, 
-    #                            goals=robot_goals, 
-    #                            circle_obstacles=circle_obs, 
-    #                            rectangle_obstacles=rectangle_obs,
-    #                            rob_dist_weight=dist_robots_weight,
-    #                            obs_dist_weight=dist_obstacles_weight,
-    #                            control_weight=control_costs_weight,
-    #                            time_weight=time_weight
-    #                            )
-    # sol,pos = solver.solve(N, initial_guess)
-    # pos_vals = np.array(sol.value(pos))
-
-    
-    # solver.plot_paths(pos_vals, initial_guess)