From 2143bc88d9272d2afb9cc15d8d27133cbe9d4e69 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Tue, 10 Jul 2018 21:23:33 +0900 Subject: [PATCH] update README --- .../Eta3SplinePath/eta3_spline_path.py | 145 ++++++++++-------- README.md | 2 +- 2 files changed, 86 insertions(+), 61 deletions(-) diff --git a/PathPlanning/Eta3SplinePath/eta3_spline_path.py b/PathPlanning/Eta3SplinePath/eta3_spline_path.py index 2731b9f4..54ea33c7 100644 --- a/PathPlanning/Eta3SplinePath/eta3_spline_path.py +++ b/PathPlanning/Eta3SplinePath/eta3_spline_path.py @@ -14,6 +14,8 @@ 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 @@ -21,11 +23,13 @@ class eta3_path(object): 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)) + 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:]): + for r, s in zip(segments[:-1], segments[1:]): assert(np.array_equal(r.end_pose, s.start_pose)) self.segments = segments """ @@ -36,10 +40,11 @@ class eta3_path(object): 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 + segment_idx = len(self.segments) - 1 u = 1. else: segment_idx = int(np.floor(u)) @@ -59,11 +64,12 @@ class eta3_path_segment(object): 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 + self.end_pose = end_pose # if no eta is passed, initialize it to array of zeros if not eta: eta = np.zeros((6,)) @@ -89,55 +95,68 @@ class eta3_path_segment(object): 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 + 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 + 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 + 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 + + (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 + + (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 + 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 + 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])) @@ -148,42 +167,47 @@ 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] + 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)) - + 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)) + 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)) + 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)) + 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)) + 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) @@ -191,7 +215,7 @@ def main(): # 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): + for i, u in enumerate(ui): pos[:, i] = path.calc_path_point(u) # plot the path @@ -202,5 +226,6 @@ def main(): plt.title('Path') plt.show() + if __name__ == '__main__': main() diff --git a/README.md b/README.md index a8baf806..e90e3b10 100644 --- a/README.md +++ b/README.md @@ -72,7 +72,7 @@ Python codes for robotics algorithm. # What is this? -This is a collection of Python implementation of robotics algorithms, especially for autonomous navigation. +This is a Python code collection of robotics algorithms, especially for autonomous navigation. Features: