Skip to content

Commit 2143bc8

Browse files
AtsushiSakaiAtsushiSakai
committed
update README
1 parent c659b8f commit 2143bc8

File tree

2 files changed

+86
-61
lines changed

2 files changed

+86
-61
lines changed

‎PathPlanning/Eta3SplinePath/eta3_spline_path.py‎

Lines changed: 85 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -14,18 +14,22 @@
1414
import matplotlib.pyplot as plt
1515

1616
# NOTE: *_pose is a 3-array: 0 - x coord, 1 - y coord, 2 - orientation angle \theta
17+
18+
1719
class eta3_path(object):
1820
"""
1921
eta3_path
2022
2123
input
2224
segments: list of `eta3_path_segment` instances definining a continuous path
2325
"""
26+
2427
def __init__(self, segments):
2528
# ensure input has the correct form
26-
assert(isinstance(segments, list) and isinstance(segments[0], eta3_path_segment))
29+
assert(isinstance(segments, list) and isinstance(
30+
segments[0], eta3_path_segment))
2731
# ensure that each segment begins from the previous segment's end (continuity)
28-
for r,s in zip(segments[:-1], segments[1:]):
32+
for r, s in zip(segments[:-1], segments[1:]):
2933
assert(np.array_equal(r.end_pose, s.start_pose))
3034
self.segments = segments
3135
"""
@@ -36,10 +40,11 @@ def __init__(self, segments):
3640
returns
3741
2d (x,y) position vector
3842
"""
43+
3944
def calc_path_point(self, u):
4045
assert(u >= 0 and u <= len(self.segments))
4146
if np.isclose(u, len(self.segments)):
42-
segment_idx = len(self.segments)-1
47+
segment_idx = len(self.segments) - 1
4348
u = 1.
4449
else:
4550
segment_idx = int(np.floor(u))
@@ -59,11 +64,12 @@ class eta3_path_segment(object):
5964
eta - shaping parameters, default=None
6065
kappa - curvature parameters, default=None
6166
"""
67+
6268
def __init__(self, start_pose, end_pose, eta=None, kappa=None):
6369
# make sure inputs are of the correct size
6470
assert(len(start_pose) == 3 and len(start_pose) == len(end_pose))
6571
self.start_pose = start_pose
66-
self.end_pose = end_pose
72+
self.end_pose = end_pose
6773
# if no eta is passed, initialize it to array of zeros
6874
if not eta:
6975
eta = np.zeros((6,))
@@ -89,55 +95,68 @@ def __init__(self, start_pose, end_pose, eta=None, kappa=None):
8995
self.coeffs[0, 1] = eta[0] * ca
9096
self.coeffs[1, 1] = eta[0] * sa
9197
# quadratic (u^2)
92-
self.coeffs[0, 2] = 1./2 * eta[2] * ca - 1./2 * eta[0]**2 * kappa[0] * sa
93-
self.coeffs[1, 2] = 1./2 * eta[2] * sa + 1./2 * eta[0]**2 * kappa[0] * ca
98+
self.coeffs[0, 2] = 1. / 2 * eta[2] * \
99+
ca - 1. / 2 * eta[0]**2 * kappa[0] * sa
100+
self.coeffs[1, 2] = 1. / 2 * eta[2] * \
101+
sa + 1. / 2 * eta[0]**2 * kappa[0] * ca
94102
# cubic (u^3)
95-
self.coeffs[0, 3] = 1./6 * eta[4] * ca - 1./6 * (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa
96-
self.coeffs[1, 3] = 1./6 * eta[4] * sa + 1./6 * (eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca
103+
self.coeffs[0, 3] = 1. / 6 * eta[4] * ca - 1. / 6 * \
104+
(eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa
105+
self.coeffs[1, 3] = 1. / 6 * eta[4] * sa + 1. / 6 * \
106+
(eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca
97107
# quartic (u^4)
98-
self.coeffs[0, 4] = 35. * (end_pose[0] - start_pose[0]) - (20. * eta[0] + 5 * eta[2] + 2./3 * eta[4]) * ca \
99-
+ (5. * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
100-
- (15. * eta[1] - 5./2 * eta[3] + 1./6 * eta[5]) * cb \
101-
- (5./2 * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * sb
102-
self.coeffs[1, 4] = 35. * (end_pose[1] - start_pose[1]) - (20. * eta[0] + 5. * eta[2] + 2./3 * eta[4]) * sa \
103-
- (5. * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
104-
- (15. * eta[1] - 5./2 * eta[3] + 1./6 * eta[5]) * sb \
105-
+ (5./2 * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * cb
108+
self.coeffs[0, 4] = 35. * (end_pose[0] - start_pose[0]) - (20. * eta[0] + 5 * eta[2] + 2. / 3 * eta[4]) * ca \
109+
+ (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
110+
- (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * cb \
111+
- (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] **
112+
3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
113+
self.coeffs[1, 4] = 35. * (end_pose[1] - start_pose[1]) - (20. * eta[0] + 5. * eta[2] + 2. / 3 * eta[4]) * sa \
114+
- (5. * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
115+
- (15. * eta[1] - 5. / 2 * eta[3] + 1. / 6 * eta[5]) * sb \
116+
+ (5. / 2 * eta[1]**2 * kappa[2] - 1. / 6 * eta[1] **
117+
3 * kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
106118
# quintic (u^5)
107119
self.coeffs[0, 5] = -84. * (end_pose[0] - start_pose[0]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * ca \
108120
- (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * sa \
109-
+ (39. * eta[1] - 7. * eta[3] + 1./2 * eta[5]) * cb \
110-
+ (7. * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * sb
121+
+ (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * cb \
122+
+ (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 *
123+
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
111124
self.coeffs[1, 5] = -84. * (end_pose[1] - start_pose[1]) + (45. * eta[0] + 10. * eta[2] + eta[4]) * sa \
112125
+ (10. * eta[0]**2 * kappa[0] + eta[0]**3 * kappa[1] + 3. * eta[0] * eta[2] * kappa[0]) * ca \
113-
+ (39. * eta[1] - 7. * eta[3] + 1./2 * eta[5]) * sb \
114-
- (7. * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * cb
126+
+ (39. * eta[1] - 7. * eta[3] + 1. / 2 * eta[5]) * sb \
127+
- (7. * eta[1]**2 * kappa[2] - 1. / 2 * eta[1]**3 *
128+
kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
115129
# sextic (u^6)
116-
self.coeffs[0, 6] = 70. * (end_pose[0] - start_pose[0]) - (36. * eta[0] + 15./2 * eta[2] + 2./3 * eta[4]) * ca \
117-
+ (15./2 * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
118-
- (34. * eta[1] - 13./2 * eta[3] + 1./2 * eta[5]) * cb \
119-
- (13./2 * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * sb
120-
self.coeffs[1, 6] = 70. * (end_pose[1] - start_pose[1]) - (36. * eta[0] + 15./2 * eta[2] + 2./3 * eta[4]) * sa \
121-
- (15./2 * eta[0]**2 * kappa[0] + 2./3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
122-
- (34. * eta[1] - 13./2 * eta[3] + 1./2 * eta[5]) * sb \
123-
+ (13./2 * eta[1]**2 * kappa[2] - 1./2 * eta[1]**3 * kappa[3] - 3./2 * eta[1] * eta[3] * kappa[2]) * cb
130+
self.coeffs[0, 6] = 70. * (end_pose[0] - start_pose[0]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * ca \
131+
+ (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * sa \
132+
- (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * cb \
133+
- (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] **
134+
3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * sb
135+
self.coeffs[1, 6] = 70. * (end_pose[1] - start_pose[1]) - (36. * eta[0] + 15. / 2 * eta[2] + 2. / 3 * eta[4]) * sa \
136+
- (15. / 2 * eta[0]**2 * kappa[0] + 2. / 3 * eta[0]**3 * kappa[1] + 2. * eta[0] * eta[2] * kappa[0]) * ca \
137+
- (34. * eta[1] - 13. / 2 * eta[3] + 1. / 2 * eta[5]) * sb \
138+
+ (13. / 2 * eta[1]**2 * kappa[2] - 1. / 2 * eta[1] **
139+
3 * kappa[3] - 3. / 2 * eta[1] * eta[3] * kappa[2]) * cb
124140
# septic (u^7)
125-
self.coeffs[0, 7] = -20. * (end_pose[0] - start_pose[0]) + (10. * eta[0] + 2. * eta[2] + 1./6 * eta[4]) * ca \
126-
- (2. * eta[0]**2 * kappa[0] + 1./6 * eta[0]**3 * kappa[1] + 1./2 * eta[0] * eta[2] * kappa[0]) * sa \
127-
+ (10. * eta[1] - 2. * eta[3] + 1./6 * eta[5]) * cb \
128-
+ (2. * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * sb
129-
self.coeffs[1, 7] = -20. * (end_pose[1] - start_pose[1]) + (10. * eta[0] + 2. * eta[2] + 1./6 * eta[4]) * sa \
130-
+ (2. * eta[0]**2 * kappa[0] + 1./6 * eta[0]**3 * kappa[1] + 1./2 * eta[0] * eta[2] * kappa[0]) * ca \
131-
+ (10. * eta[1] - 2. * eta[3] + 1./6 * eta[5]) * sb \
132-
- (2. * eta[1]**2 * kappa[2] - 1./6 * eta[1]**3 * kappa[3] - 1./2 * eta[1] * eta[3] * kappa[2]) * cb
141+
self.coeffs[0, 7] = -20. * (end_pose[0] - start_pose[0]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * ca \
142+
- (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * sa \
143+
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * cb \
144+
+ (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
145+
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * sb
146+
self.coeffs[1, 7] = -20. * (end_pose[1] - start_pose[1]) + (10. * eta[0] + 2. * eta[2] + 1. / 6 * eta[4]) * sa \
147+
+ (2. * eta[0]**2 * kappa[0] + 1. / 6 * eta[0]**3 * kappa[1] + 1. / 2 * eta[0] * eta[2] * kappa[0]) * ca \
148+
+ (10. * eta[1] - 2. * eta[3] + 1. / 6 * eta[5]) * sb \
149+
- (2. * eta[1]**2 * kappa[2] - 1. / 6 * eta[1]**3 *
150+
kappa[3] - 1. / 2 * eta[1] * eta[3] * kappa[2]) * cb
133151
"""
134152
eta3_path_segment::calc_point
135-
153+
136154
input
137155
u - parametric representation of a point along the segment, 0 <= u <= 1
138156
returns
139157
(x,y) of point along the segment
140158
"""
159+
141160
def calc_point(self, u):
142161
assert(u >= 0 and u <= 1)
143162
return self.coeffs.dot(np.array([1, u, u**2, u**3, u**4, u**5, u**6, u**7]))
@@ -148,50 +167,55 @@ def main():
148167
recreate path from reference (see Table 1)
149168
"""
150169
path_segments = []
151-
170+
152171
# segment 1: lane-change curve
153172
start_pose = [0, 0, 0]
154-
end_pose = [4, 1.5, 0]
173+
end_pose = [4, 1.5, 0]
155174
# NOTE: The ordering on kappa is [kappa_A, kappad_A, kappa_B, kappad_B], with kappad_* being the curvature derivative
156-
kappa = [0, 0, 0, 0]
157-
eta = [4.27, 4.27, 0, 0, 0, 0]
158-
path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
159-
175+
kappa = [0, 0, 0, 0]
176+
eta = [4.27, 4.27, 0, 0, 0, 0]
177+
path_segments.append(eta3_path_segment(
178+
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
179+
160180
# segment 2: line segment
161181
start_pose = [4, 1.5, 0]
162-
end_pose = [5.5, 1.5, 0]
163-
kappa = [0, 0, 0, 0]
164-
eta = [0, 0, 0, 0, 0, 0]
165-
path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
182+
end_pose = [5.5, 1.5, 0]
183+
kappa = [0, 0, 0, 0]
184+
eta = [0, 0, 0, 0, 0, 0]
185+
path_segments.append(eta3_path_segment(
186+
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
166187

167188
# segment 3: cubic spiral
168189
start_pose = [5.5, 1.5, 0]
169-
end_pose = [7.4377, 1.8235, 0.6667]
170-
kappa = [0, 0, 1, 1]
171-
eta = [1.88, 1.88, 0, 0, 0, 0]
172-
path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
190+
end_pose = [7.4377, 1.8235, 0.6667]
191+
kappa = [0, 0, 1, 1]
192+
eta = [1.88, 1.88, 0, 0, 0, 0]
193+
path_segments.append(eta3_path_segment(
194+
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
173195

174196
# segment 4: generic twirl arc
175197
start_pose = [7.4377, 1.8235, 0.6667]
176-
end_pose = [7.8, 4.3, 1.8]
177-
kappa = [1, 1, 0.5, 0]
178-
eta = [7, 10, 10, -10, 4, 4]
179-
path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
198+
end_pose = [7.8, 4.3, 1.8]
199+
kappa = [1, 1, 0.5, 0]
200+
eta = [7, 10, 10, -10, 4, 4]
201+
path_segments.append(eta3_path_segment(
202+
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
180203

181204
# segment 5: circular arc
182205
start_pose = [7.8, 4.3, 1.8]
183-
end_pose = [5.4581, 5.8064, 3.3416]
184-
kappa = [0.5, 0, 0.5, 0]
185-
eta = [2.98, 2.98, 0, 0, 0, 0]
186-
path_segments.append(eta3_path_segment(start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
206+
end_pose = [5.4581, 5.8064, 3.3416]
207+
kappa = [0.5, 0, 0.5, 0]
208+
eta = [2.98, 2.98, 0, 0, 0, 0]
209+
path_segments.append(eta3_path_segment(
210+
start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa))
187211

188212
# construct the whole path
189213
path = eta3_path(path_segments)
190214

191215
# interpolate at several points along the path
192216
ui = np.linspace(0, len(path_segments), 1001)
193217
pos = np.empty((2, ui.size))
194-
for i,u in enumerate(ui):
218+
for i, u in enumerate(ui):
195219
pos[:, i] = path.calc_path_point(u)
196220

197221
# plot the path
@@ -202,5 +226,6 @@ def main():
202226
plt.title('Path')
203227
plt.show()
204228

229+
205230
if __name__ == '__main__':
206231
main()

‎README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,7 @@ Python codes for robotics algorithm.
7272

7373
# What is this?
7474

75-
This is a collection of Python implementation of robotics algorithms, especially for autonomous navigation.
75+
This is a Python code collection of robotics algorithms, especially for autonomous navigation.
7676

7777
Features:
7878

0 commit comments

Comments
 (0)