From 97eb02b0761e74f3812f9b2482ef5b470dd16a5f Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Thu, 22 Aug 2024 16:42:05 -0500
Subject: [PATCH] Fixing weird points that were showing up when curving a path
---
guided_mrmp/conflict_resolvers/curve_path.py | 90 +++++++++++++-------
1 file changed, 57 insertions(+), 33 deletions(-)
diff --git a/guided_mrmp/conflict_resolvers/curve_path.py b/guided_mrmp/conflict_resolvers/curve_path.py
index 9fd690d..33fc54a 100644
--- a/guided_mrmp/conflict_resolvers/curve_path.py
+++ b/guided_mrmp/conflict_resolvers/curve_path.py
@@ -1,9 +1,9 @@
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)
--
GitLab