mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 04:28:04 -05:00
Add \eta^3 spline path to PathPlanning
This commit is contained in:
jwdinius
206
PathPlanning/Eta3SplinePath/eta3_spline_path.py
Normal file
206
PathPlanning/Eta3SplinePath/eta3_spline_path.py
Normal file
@@ -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()
|
||||
Reference in New Issue
Block a user