diff --git a/guided_mrmp/conflict_resolvers/curve_path.py b/guided_mrmp/conflict_resolvers/curve_path.py
index a20aaa34638f41adaf0c05e47dba146f1ed99591..12373761b66875dd855937198f6f6848320853dc 100644
--- a/guided_mrmp/conflict_resolvers/curve_path.py
+++ b/guided_mrmp/conflict_resolvers/curve_path.py
@@ -1,176 +1,188 @@
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)