From bf47424fc531bf3ae3f997ca90543d2e4bc5bbfe Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Wed, 16 Oct 2024 21:17:49 -0500
Subject: [PATCH] make the curved path have exactly N points
---
guided_mrmp/conflict_resolvers/curve_path.py | 24 ++++++++++++++------
1 file changed, 17 insertions(+), 7 deletions(-)
diff --git a/guided_mrmp/conflict_resolvers/curve_path.py b/guided_mrmp/conflict_resolvers/curve_path.py
index e425e71..a20aaa3 100644
--- a/guided_mrmp/conflict_resolvers/curve_path.py
+++ b/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)
--
GitLab