github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 04:28:04 -05:00
Code Issues Packages Projects Releases Wiki Activity

update README

Browse Source
This commit is contained in:
Atsushi Sakai
2018-07-10 21:23:33 +09:00
parent c659b8f4eb
commit 2143bc88d9
2 changed files with 86 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
PathPlanning/Eta3SplinePath/eta3_spline_path.py
View File

@@ -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()

2
README.md
View File

@@ -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: