Skip to content
Snippets Groups Projects
Commit b35e8bf3 authored by rachelmoan's avatar rachelmoan
Browse files

improve how we construct bezier curve for initial guess.

parent 0b22b300
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
guided_mrmp/conflict_resolvers/curve_path.py
+ 166
− 154
View file @ b35e8bf3
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)
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment