From 009a98d0403bc04eadce74c5e463536f0247b4b5 Mon Sep 17 00:00:00 2001 From: jwdinius Date: Sun, 8 Jul 2018 15:11:46 -0700 Subject: [PATCH] Add \eta^3 spline path to PathPlanning --- .../Eta3SplinePath/eta3_spline_path.py | 206 ++++++++++++++++++ 1 file changed, 206 insertions(+) create mode 100644 PathPlanning/Eta3SplinePath/eta3_spline_path.py diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py new file mode 100644 index 00000000..2731b9f4 --- /dev/null +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -0,0 +1,206 @@ +""" + +\eta^3 polynomials planner + +author: Joe Dinius, Ph.D (https://jwdinius.github.io) + +Ref: + +- [\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots](https://ieeexplore.ieee.org/document/4339545/) + +""" + +import numpy as np +import matplotlib.pyplot as plt + +# NOTE: *_pose is a 3-array: 0 - x coord, 1 - y coord, 2 - orientation angle \theta +class eta3_path(object): + """ + eta3_path + + input + segments: list of `eta3_path_segment` instances definining a continuous path + """ + def __init__(self, segments): + # ensure input has the correct form + assert(isinstance(segments, list) and isinstance(segments[0], eta3_path_segment)) + # ensure that each segment begins from the previous segment's end (continuity) + for r,s in zip(segments[:-1], segments[1:]): + assert(np.array_equal(r.end_pose, s.start_pose)) + self.segments = segments + """ + eta3_path::calc_path_point + + input + normalized interpolation point along path object, 0 <= u <= len(self.segments) + returns + 2d (x,y) position vector + """ + def calc_path_point(self, u): + assert(u >= 0 and u <= len(self.segments)) + if np.isclose(u, len(self.segments)): + segment_idx = len(self.segments)-1 + u = 1. + else: + segment_idx = int(np.floor(u)) + u -= segment_idx + return self.segments[segment_idx].calc_point(u) + + +class eta3_path_segment(object): + """ + eta3_path_segment - constructs an eta^3 path segment based on desired shaping, eta, and curvature vector, kappa. + If either, or both, of eta and kappa are not set during initialization, they will + default to zeros. + + input + start_pose - starting pose array (x, y, \theta) + end_pose - ending pose array (x, y, \theta) + eta - shaping parameters, default=None + kappa - curvature parameters, default=None + """ + def __init__(self, start_pose, end_pose, eta=None, kappa=None): + # make sure inputs are of the correct size + assert(len(start_pose) == 3 and len(start_pose) == len(end_pose)) + self.start_pose = start_pose + self.end_pose = end_pose + # if no eta is passed, initialize it to array of zeros + if not eta: + eta = np.zeros((6,)) + else: + # make sure that eta has correct size + assert(len(eta) == 6) + # if no kappa is passed, initialize to array of zeros + if not kappa: + kappa = np.zeros((4,)) + else: + assert(len(kappa) == 4) + # set up angle cosines and sines for simpler computations below + ca = np.cos(start_pose[2]) + sa = np.sin(start_pose[2]) + cb = np.cos(end_pose[2]) + sb = np.sin(end_pose[2]) + # 2 dimensions (x,y) x 8 coefficients per dimension + self.coeffs = np.empty((2, 8)) + # constant terms (u^0) + self.coeffs[0, 0] = start_pose[0] + self.coeffs[1, 0] = start_pose[1] + # linear (u^1) + self.coeffs[0, 1] = eta[0] * ca + self.coeffs[1, 1] = eta[0] * sa + # quadratic (u^2) + self.coeffs[0, 2] = 1./2 * eta[2] * ca - 1./2 * eta[0]**2 * kappa[0] * sa + self.coeffs[1, 2] = 1./2 * eta[2] * sa + 1./2 * eta[0]**2 * kappa[0] * ca + # cubic (u^3) + self.coeffs[0, 3] = 1./6 * eta[4] * ca - 1./6 * (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa + self.coeffs[1, 3] = 1./6 * eta[4] * sa + 1./6 * (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca + # quartic (u^4) + self.coeffs[0, 4] = 35. * (end_pose[0] - start_pose[0]) - (20. * eta[0] + 5 * eta[2] + 2./3 * eta[4]) * ca \ + + (5. * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \ + - (15. * eta[1] - 5./2 * eta[3] + 1./6 * eta[5]) * cb \ + - (5./2 * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * sb + self.coeffs[1, 4] = 35. * (end_pose[1] - start_pose[1]) - (20. * eta[0] + 5. * eta[2] + 2./3 * eta[4]) * sa \ + - (5. * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \ + - (15. * eta[1] - 5./2 * eta[3] + 1./6 * eta[5]) * sb \ + + (5./2 * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * cb + # quintic (u^5) + self.coeffs[0, 5] = -84. * (end_pose[0] - start_pose[0]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * ca \ + - (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa \ + + (39. * eta[1] - 7. * eta[3] + 1./2 * eta[5]) * cb \ + + (7. * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * sb + self.coeffs[1, 5] = -84. * (end_pose[1] - start_pose[1]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * sa \ + + (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca \ + + (39. * eta[1] - 7. * eta[3] + 1./2 * eta[5]) * sb \ + - (7. * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * cb + # sextic (u^6) + self.coeffs[0, 6] = 70. * (end_pose[0] - start_pose[0]) - (36. * eta[0] + 15./2 * eta[2] + 2./3 * eta[4]) * ca \ + + (15./2 * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \ + - (34. * eta[1] - 13./2 * eta[3] + 1./2 * eta[5]) * cb \ + - (13./2 * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * sb + self.coeffs[1, 6] = 70. * (end_pose[1] - start_pose[1]) - (36. * eta[0] + 15./2 * eta[2] + 2./3 * eta[4]) * sa \ + - (15./2 * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \ + - (34. * eta[1] - 13./2 * eta[3] + 1./2 * eta[5]) * sb \ + + (13./2 * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * cb + # septic (u^7) + self.coeffs[0, 7] = -20. * (end_pose[0] - start_pose[0]) + (10. * eta[0] + 2. * eta[2] + 1./6 * eta[4]) * ca \ + - (2. * eta[0]**2 * kappa[0] + 1./6 * eta[0]**3 * kappa[1] + 1./2 * eta[0] * eta[2] * kappa[0]) * sa \ + + (10. * eta[1] - 2. * eta[3] + 1./6 * eta[5]) * cb \ + + (2. * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * sb + self.coeffs[1, 7] = -20. * (end_pose[1] - start_pose[1]) + (10. * eta[0] + 2. * eta[2] + 1./6 * eta[4]) * sa \ + + (2. * eta[0]**2 * kappa[0] + 1./6 * eta[0]**3 * kappa[1] + 1./2 * eta[0] * eta[2] * kappa[0]) * ca \ + + (10. * eta[1] - 2. * eta[3] + 1./6 * eta[5]) * sb \ + - (2. * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * cb + """ + eta3_path_segment::calc_point + + input + u - parametric representation of a point along the segment, 0 <= u <= 1 + returns + (x,y) of point along the segment + """ + def calc_point(self, u): + assert(u >= 0 and u <= 1) + return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7])) + + +def main(): + """ + recreate path from reference (see Table 1) + """ + path_segments = [] + + # segment 1: lane-change curve + start_pose = [0, 0, 0] + end_pose = [4, 1.5, 0] + # NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative + kappa = [0, 0, 0, 0] + eta = [4.27, 4.27, 0, 0, 0, 0] + path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) + + # segment 2: line segment + start_pose = [4, 1.5, 0] + end_pose = [5.5, 1.5, 0] + kappa = [0, 0, 0, 0] + eta = [0, 0, 0, 0, 0, 0] + path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) + + # segment 3: cubic spiral + start_pose = [5.5, 1.5, 0] + end_pose = [7.4377, 1.8235, 0.6667] + kappa = [0, 0, 1, 1] + eta = [1.88, 1.88, 0, 0, 0, 0] + path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) + + # segment 4: generic twirl arc + start_pose = [7.4377, 1.8235, 0.6667] + end_pose = [7.8, 4.3, 1.8] + kappa = [1, 1, 0.5, 0] + eta = [7, 10, 10, -10, 4, 4] + path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) + + # segment 5: circular arc + start_pose = [7.8, 4.3, 1.8] + end_pose = [5.4581, 5.8064, 3.3416] + kappa = [0.5, 0, 0.5, 0] + eta = [2.98, 2.98, 0, 0, 0, 0] + path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) + + # construct the whole path + path = eta3_path(path_segments) + + # interpolate at several points along the path + ui = np.linspace(0, len(path_segments), 1001) + pos = np.empty((2, ui.size)) + for i,u in enumerate(ui): + pos[:, i] = path.calc_path_point(u) + + # plot the path + plt.figure('Path from Reference') + plt.plot(pos[0, :], pos[1, :]) + plt.xlabel('x') + plt.ylabel('y') + plt.title('Path') + plt.show() + +if __name__ == '__main__': + main()