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

update pure_pursuit code

Browse Source
This commit is contained in:
Atsushi Sakai
2017-12-23 18:48:41 -08:00
parent ae117215b1
commit edaa12a09c
5 changed files with 68 additions and 616 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
PathTracking/pure_pursuit/animation.gif Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.5 MiB

307
PathTracking/pure_pursuit/pure_pursuit.py
View File

@@ -1,6 +1,4 @@
#! /usr/bin/python
# -*- coding: utf-8 -*-
u"""
"""
Path tracking simulation with pure pursuit steering control and PID speed control.
@@ -10,12 +8,34 @@ author: Atsushi Sakai
import numpy as np
import math
import matplotlib.pyplot as plt
import unicycle_model
k = 0.1 # look forward gain
Lfc = 1.0 # look-ahead distance
Kp = 1.0 # speed propotional gain
Lf = 1.0 # look-ahead distance
# animation = True
animation = False
dt = 0.1 # [s]
L = 2.9 # [m] Tread of vehicle
show_animation = True
class State:
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a, delta):
state.x = state.x + state.v * math.cos(state.yaw) * dt
state.y = state.y + state.v * math.sin(state.yaw) * dt
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
state.v = state.v + a * dt
return state
def PIDControl(target, current):
@@ -31,7 +51,6 @@ def pure_pursuit_control(state, cx, cy, pind):
if pind >= ind:
ind = pind
# print(pind, ind)
if ind < len(cx):
tx = cx[ind]
ty = cy[ind]
@@ -44,26 +63,26 @@ def pure_pursuit_control(state, cx, cy, pind):
if state.v < 0: # back
alpha = math.pi - alpha
# if alpha > 0:
# alpha = math.pi - alpha
# else:
# alpha = math.pi + alpha
delta = math.atan2(2.0 * unicycle_model.L * math.sin(alpha) / Lf, 1.0)
Lf = k * state.v + Lfc
delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)
return delta, ind
def calc_target_index(state, cx, cy):
# search nearest point index
dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
ind = d.index(min(d))
L = 0.0
Lf = k * state.v + Lfc
# search look ahead target point index
while Lf > L and (ind + 1) < len(cx):
dx = cx[ind + 1] - cx[ind]
dy = cx[ind + 1] - cx[ind]
@@ -73,176 +92,17 @@ def calc_target_index(state, cx, cy):
return ind
def closed_loop_prediction(cx, cy, cyaw, speed_profile, goal):
T = 500.0 # max simulation time
goal_dis = 0.3
stop_speed = 0.05
state = unicycle_model.State(x=-0.0, y=-0.0, yaw=0.0, v=0.0)
# lastIndex = len(cx) - 1
time = 0.0
x = [state.x]
y = [state.y]
yaw = [state.yaw]
v = [state.v]
t = [0.0]
target_ind = calc_target_index(state, cx, cy)
while T >= time:
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
ai = PIDControl(speed_profile[target_ind], state.v)
state = unicycle_model.update(state, ai, di)
if abs(state.v) <= stop_speed:
target_ind += 1
time = time + unicycle_model.dt
# check goal
dx = state.x - goal[0]
dy = state.y - goal[1]
if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis:
print("Goal")
break
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
if target_ind % 20 == 0 and animation:
plt.cla()
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("speed:" + str(round(state.v, 2)) +
"tind:" + str(target_ind))
plt.pause(0.0001)
return t, x, y, yaw, v
def set_stop_point(target_speed, cx, cy, cyaw):
speed_profile = [target_speed] * len(cx)
forward = True
d = []
# Set stop point
for i in range(len(cx) - 1):
dx = cx[i + 1] - cx[i]
dy = cy[i + 1] - cy[i]
d.append(math.sqrt(dx ** 2.0 + dy ** 2.0))
iyaw = cyaw[i]
move_direction = math.atan2(dy, dx)
is_back = abs(move_direction - iyaw) >= math.pi / 2.0
if dx == 0.0 and dy == 0.0:
continue
if is_back:
speed_profile[i] = - target_speed
else:
speed_profile[i] = target_speed
if is_back and forward:
speed_profile[i] = 0.0
forward = False
# plt.plot(cx[i], cy[i], "xb")
# print(iyaw, move_direction, dx, dy)
elif not is_back and not forward:
speed_profile[i] = 0.0
forward = True
# plt.plot(cx[i], cy[i], "xb")
# print(iyaw, move_direction, dx, dy)
speed_profile[0] = 0.0
speed_profile[-1] = 0.0
d.append(d[-1])
return speed_profile, d
def calc_speed_profile(cx, cy, cyaw, target_speed, a):
speed_profile, d = set_stop_point(target_speed, cx, cy, cyaw)
nsp = len(speed_profile)
# plt.plot(speed_profile, "xb")
# forward integration
for i in range(nsp - 1):
if speed_profile[i + 1] >= 0: # forward
tspeed = speed_profile[i] + a * d[i]
if tspeed <= speed_profile[i + 1]:
speed_profile[i + 1] = tspeed
else:
tspeed = speed_profile[i] - a * d[i]
if tspeed >= speed_profile[i + 1]:
speed_profile[i + 1] = tspeed
# plt.plot(speed_profile, "ok")
# back integration
for i in range(nsp - 1):
if speed_profile[- i - 1] >= 0: # forward
tspeed = speed_profile[-i] + a * d[-i]
if tspeed <= speed_profile[-i - 1]:
speed_profile[-i - 1] = tspeed
else:
tspeed = speed_profile[-i] - a * d[-i]
if tspeed >= speed_profile[-i - 1]:
speed_profile[-i - 1] = tspeed
# flg, ax = plt.subplots(1)
# plt.plot(speed_profile, "-r")
# plt.show()
return speed_profile
def extend_path(cx, cy, cyaw):
dl = 0.1
dl_list = [dl] * (int(Lf / dl) + 0)
move_direction = math.atan2(cy[-1] - cy[-2], cx[-1] - cx[-2])
is_back = abs(move_direction - cyaw[-1]) >= math.pi / 2.0
for idl in dl_list:
if is_back:
idl *= -1
cx = np.append(cx, cx[-1] + idl * math.cos(cyaw[-1]))
cy = np.append(cy, cy[-1] + idl * math.sin(cyaw[-1]))
cyaw = np.append(cyaw, cyaw[-1])
return cx, cy, cyaw
def main():
# target course
import numpy as np
cx = np.arange(0, 50, 0.1)
cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]
target_speed = 10.0 / 3.6
target_speed = 10.0 / 3.6 # [m/s]
T = 15.0 # max simulation time
T = 100.0 # max simulation time
state = unicycle_model.State(x=-0.0, y=-3.0, yaw=0.0, v=0.0)
# state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=-30.0 / 3.6)
# state = unicycle_model.State(x=10.0, y=5.0, yaw=0.0, v=-30.0 / 3.6)
# state = unicycle_model.State(
# x=3.0, y=5.0, yaw=math.radians(-40.0), v=-10.0 / 3.6)
# state = unicycle_model.State(
# x=3.0, y=5.0, yaw=math.radians(40.0), v=50.0 / 3.6)
# initial state
state = State(x=-0.0, y=-3.0, yaw=0.0, v=0.0)
lastIndex = len(cx) - 1
time = 0.0
@@ -256,9 +116,9 @@ def main():
while T >= time and lastIndex > target_ind:
ai = PIDControl(target_speed, state.v)
di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
state = unicycle_model.update(state, ai, di)
state = update(state, ai, di)
time = time + unicycle_model.dt
time = time + dt
x.append(state.x)
y.append(state.y)
@@ -266,69 +126,36 @@ def main():
v.append(state.v)
t.append(time)
# plt.cla()
# plt.plot(cx, cy, ".r", label="course")
# plt.plot(x, y, "-b", label="trajectory")
# plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
# plt.axis("equal")
# plt.grid(True)
# plt.pause(0.1)
# input()
if show_animation:
plt.cla()
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
plt.pause(0.001)
flg, ax = plt.subplots(1)
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
# Test
assert lastIndex >= target_ind, "Cannot goal"
flg, ax = plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
if show_animation:
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
def main2():
import pandas as pd
data = pd.read_csv("rrt_course.csv")
cx = np.array(data["x"])
cy = np.array(data["y"])
cyaw = np.array(data["yaw"])
target_speed = 10.0 / 3.6
a = 0.1
goal = [cx[-1], cy[-1]]
cx, cy, cyaw = extend_path(cx, cy, cyaw)
speed_profile = calc_speed_profile(cx, cy, cyaw, target_speed, a)
t, x, y, yaw, v = closed_loop_prediction(cx, cy, cyaw, speed_profile, goal)
flg, ax = plt.subplots(1)
plt.plot(cx, cy, ".r", label="course")
plt.plot(x, y, "-b", label="trajectory")
plt.plot(goal[0], goal[1], "xg", label="goal")
plt.legend()
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
flg, ax = plt.subplots(1)
plt.plot(t, [iv * 3.6 for iv in v], "-r")
plt.xlabel("Time[s]")
plt.ylabel("Speed[km/h]")
plt.grid(True)
plt.show()
if __name__ == '__main__':
print("Pure pursuit path tracking simulation start")
# main()
main2()
main()

304
PathTracking/pure_pursuit/rrt_course.csv
View File

@@ -1,304 +0,0 @@
x,y,yaw
0,0,0
0,0,0
0.099993333,0.000999967,0.02
0.199946671,0.003999467,0.04
0.299820032,0.0089973,0.06
0.39957347,0.015991468,0.08
0.499167083,0.024979174,0.1
0.598561036,0.035956821,0.12
0.697715573,0.048920019,0.14
0.796591033,0.063863583,0.16
0.895147867,0.080781536,0.18
0.993346654,0.099667111,0.2
1.091148115,0.120512753,0.22
1.188513132,0.143310126,0.24
1.285402759,0.168050109,0.26
1.381778243,0.194722808,0.28
1.477601033,0.223317554,0.3
1.572832803,0.25382291,0.32
1.667690714,0.285476899,0.322200686
1.7625448,0.317142375,0.322200686
1.857398886,0.348807852,0.322200686
1.952252972,0.380473329,0.322200686
2.047107058,0.412138805,0.322200686
2.141961143,0.443804282,0.322200686
2.236815229,0.475469758,0.322200686
2.331669315,0.507135235,0.322200686
2.426523401,0.538800712,0.322200686
2.521377487,0.570466188,0.322200686
2.616231572,0.602131665,0.322200686
2.711085658,0.633797141,0.322200686
2.805939744,0.665462618,0.322200686
2.90079383,0.697128095,0.322200686
2.995647916,0.728793571,0.322200686
3.090502001,0.760459048,0.322200686
3.185356087,0.792124524,0.322200686
3.280210173,0.823790001,0.322200686
3.375064259,0.855455477,0.322200686
3.469918345,0.887120954,0.322200686
3.56477243,0.918786431,0.322200686
3.659626516,0.950451907,0.322200686
3.754480602,0.982117384,0.322200686
3.849334688,1.01378286,0.322200686
3.944188773,1.045448337,0.322200686
4.039042859,1.077113814,0.322200686
4.133896945,1.10877929,0.322200686
4.228751031,1.140444767,0.322200686
4.323605117,1.172110243,0.322200686
4.418459202,1.20377572,0.322200686
4.513313288,1.235441197,0.322200686
4.608167374,1.267106673,0.322200686
4.70302146,1.29877215,0.322200686
4.797875546,1.330437626,0.322200686
4.892729631,1.362103103,0.322200686
4.987583717,1.39376858,0.322200686
5.082437803,1.425434056,0.322200686
5.177291889,1.457099533,0.322200686
5.272145975,1.488765009,0.322200686
5.36700006,1.520430486,0.322200686
5.461854146,1.552095962,0.322200686
5.556708232,1.583761439,0.322200686
5.651562318,1.615426916,0.322200686
5.746416404,1.647092392,0.322200686
5.841270489,1.678757869,0.322200686
5.936124575,1.710423345,0.322200686
6.030978724,1.742088632,0.321917939
6.126151812,1.77277658,0.301917939
6.221919583,1.801555056,0.281917939
6.318243732,1.828412548,0.261917939
6.415085732,1.853338314,0.241917939
6.512406845,1.876322385,0.221917939
6.610168146,1.897355566,0.201917939
6.708330531,1.916429446,0.181917939
6.806854736,1.933536393,0.161917939
6.905701353,1.948669567,0.141917939
7.004830845,1.961822914,0.121917939
7.104203561,1.972991172,0.101917939
7.203779754,1.982169875,0.081917939
7.303519593,1.989355351,0.061917939
7.403383185,1.994544727,0.041917939
7.503330586,1.997735926,0.021917939
7.603321818,1.998927671,0.001917939
7.703316885,1.998119488,-0.018082061
7.803275792,1.995311697,-0.038082061
7.903158555,1.990505424,-0.058082061
8.002925223,1.98370259,-0.078082061
8.102535892,1.974905915,-0.098082061
8.201950716,1.96411892,-0.118082061
8.301129933,1.951345918,-0.138082061
8.400033872,1.936592018,-0.158082061
8.498622973,1.919863122,-0.178082061
8.5968578,1.901165921,-0.198082061
8.694699062,1.880507894,-0.218082061
8.792107624,1.857897304,-0.238082061
8.835526197,1.847154409,-0.247027664
8.835526197,1.847154409,-0.247027664
8.932728598,1.823673356,-0.227027664
9.03038115,1.802140918,-0.207027664
9.128444791,1.782565707,-0.187027664
9.226880299,1.764955554,-0.167027664
9.3256483,1.749317501,-0.147027664
9.424709288,1.735657804,-0.127027664
9.52402364,1.723981926,-0.107027664
9.623551632,1.714294538,-0.087027664
9.723253454,1.706599515,-0.067027664
9.823089226,1.700899934,-0.047027664
9.923019016,1.697198075,-0.027027664
10.02300285,1.695495419,-0.007027664
10.12300074,1.695792648,0.012972336
10.22297269,1.698089641,0.032972336
10.32287871,1.70238548,0.052972336
10.42267884,1.708678448,0.072972336
10.52233315,1.716966026,0.092972336
10.6218018,1.727244901,0.112972336
10.72104499,1.73951096,0.132972336
10.82002302,1.753759297,0.152972336
10.91869632,1.769984214,0.172972336
11.0170254,1.78817922,0.192972336
11.11497094,1.808337037,0.212972336
11.21249376,1.830449604,0.232972336
11.30955486,1.854508074,0.252972336
11.40611541,1.880502826,0.272972336
11.50213679,1.908423461,0.292972336
11.59758058,1.938258811,0.312972336
11.69240863,1.969996944,0.332972336
11.78658298,2.003625163,0.352972336
11.88006599,2.039130019,0.372972336
11.97307064,2.07587289,0.369773015
12.06666676,2.111078482,0.349773015
12.16094822,2.144405236,0.329773015
12.25587732,2.175839821,0.309773015
12.35141608,2.205369664,0.289773015
12.44752629,2.232982953,0.269773015
12.54416951,2.258668644,0.249773015
12.64130708,2.282416463,0.229773015
12.73890015,2.30421691,0.209773015
12.83690968,2.324061265,0.189773015
12.93529647,2.341941592,0.169773015
13.03402117,2.357850739,0.149773015
13.13304428,2.371782342,0.129773015
13.23232621,2.383730828,0.109773015
13.33182723,2.393691418,0.089773015
13.43150755,2.401660128,0.069773015
13.5313273,2.407633772,0.049773015
13.63124655,2.411609958,0.029773015
13.73122533,2.413587098,0.009773015
13.83122366,2.4135644,-0.010226985
13.93120154,2.411541873,-0.030226985
14.03111898,2.407520326,-0.050226985
14.130936,2.401501369,-0.070226985
14.2306127,2.393487407,-0.090226985
14.33010919,2.383481647,-0.110226985
14.42938568,2.371488091,-0.130226985
14.52840246,2.357511537,-0.150226985
14.62711992,2.341557574,-0.170226985
14.72549858,2.323632584,-0.190226985
14.8234991,2.303743737,-0.210226985
14.92108226,2.281898988,-0.230226985
15.01820904,2.258107074,-0.250226985
15.11484059,2.232377513,-0.270226985
15.21093825,2.204720595,-0.290226985
15.3064636,2.175147384,-0.310226985
15.40137842,2.143669707,-0.330226985
15.49564474,2.110300156,-0.350226985
15.58922487,2.075052077,-0.370226985
15.68208136,2.037939571,-0.390226985
15.77417708,1.99897748,-0.410226985
15.8654752,1.95818139,-0.430226985
15.95593919,1.915567618,-0.450226985
16.04553286,1.87115321,-0.470226985
15.98434311,1.903321256,-0.490226985
15.8965972,1.951282929,-0.510226985
15.80982801,2.000989811,-0.530226985
15.72407024,2.05242202,-0.550226985
15.6393582,2.105558985,-0.570226985
15.55572577,2.16037945,-0.590226985
15.47320641,2.216861488,-0.610226985
15.39183311,2.274982508,-0.630226985
15.31163842,2.334719261,-0.650226985
15.23265444,2.396047854,-0.670226985
15.15491274,2.458943756,-0.690226985
15.07844442,2.52338181,-0.710226985
15.00328006,2.589336241,-0.730226985
14.92944975,2.656780669,-0.750226985
14.85698299,2.725688116,-0.770226985
14.78590879,2.79603102,-0.790226985
14.71625556,2.867781246,-0.810226985
14.64805118,2.940910093,-0.830226985
14.58132291,3.015388313,-0.850226985
14.51609745,3.091186113,-0.870226985
14.4524009,3.168273176,-0.890226985
14.39025872,3.246618669,-0.910226985
14.32969577,3.326191254,-0.930226985
14.27073628,3.406959103,-0.950226985
14.21340384,3.48888991,-0.970226985
14.15772136,3.571950905,-0.990226985
14.10371113,3.656108862,-1.010226985
14.05139475,3.741330122,-1.030226985
14.00079315,3.827580595,-1.050226985
13.95192656,3.914825784,-1.070226985
13.90481453,4.00303079,-1.090226985
13.8594759,4.092160334,-1.110226985
13.81592881,4.182178765,-1.130226985
13.77419068,4.273050077,-1.150226985
13.73427821,4.364737922,-1.170226985
13.69767916,4.453530976,-1.189435296
13.69767916,4.453530976,-1.189435296
13.66139137,4.546712854,-1.209435296
13.62697434,4.640601804,-1.229435296
13.59444185,4.735160271,-1.249435296
13.56380691,4.830350434,-1.269435296
13.53508177,4.926134218,-1.289435296
13.50827793,5.022473309,-1.309435296
13.4834061,5.119329175,-1.329435296
13.46047623,5.216663074,-1.349435296
13.4394975,5.314436073,-1.369435296
13.42047829,5.412609065,-1.389435296
13.40342621,5.511142782,-1.409435296
13.38834809,5.609997812,-1.429435296
13.37524995,5.709134613,-1.449435296
13.36259395,5.808329049,-1.434174994
13.34798454,5.907254433,-1.414174994
13.33139967,6.005867865,-1.394174994
13.31284599,6.104129899,-1.374174994
13.2923309,6.202001233,-1.354174994
13.26986262,6.299442719,-1.334174994
13.24545014,6.396415381,-1.314174994
13.21910321,6.492880432,-1.294174994
13.19083238,6.588799288,-1.274174994
13.16064896,6.684133581,-1.254174994
13.12856501,6.77884518,-1.234174994
13.09459337,6.872896201,-1.214174994
13.05874763,6.966249025,-1.194174994
13.02104213,7.058866313,-1.174174994
12.98149195,7.150711017,-1.154174994
12.94011291,7.241746403,-1.134174994
12.89692155,7.331936056,-1.114174994
12.85193516,7.421243902,-1.094174994
12.80517173,7.50963422,-1.074174994
12.75664997,7.597071654,-1.054174994
12.70638927,7.68352123,-1.034174994
12.65440975,7.76894837,-1.014174994
12.6007322,7.853318904,-0.994174994
12.54537808,7.936599086,-0.974174994
12.48836955,8.018755603,-0.954174994
12.42972939,8.099755594,-0.934174994
12.36948107,8.179566662,-0.914174994
12.30764868,8.258156881,-0.894174994
12.24425696,8.335494818,-0.874174994
12.17933126,8.411549538,-0.854174994
12.11289756,8.486290619,-0.834174994
12.04498241,8.559688168,-0.814174994
11.975613,8.631712825,-0.794174994
11.90481707,8.702335782,-0.774174994
11.83262292,8.771528791,-0.754174994
11.75905945,8.839264175,-0.734174994
11.68415607,8.905514841,-0.714174994
11.60794275,8.970254289,-0.694174994
11.53044997,9.033456626,-0.674174994
11.45170872,9.095096569,-0.654174994
11.37175051,9.155149466,-0.634174994
11.2906073,9.213591294,-0.614174994
11.20831157,9.270398679,-0.594174994
11.12489622,9.325548897,-0.574174994
11.04039463,9.379019891,-0.554174994
10.95484058,9.430790271,-0.534174994
10.86826831,9.48083933,-0.514174994
10.78071244,9.52914705,-0.494174994
10.69220799,9.575694108,-0.474174994
10.60279036,9.620461886,-0.454174994
10.51249531,9.663432478,-0.434174994
10.42135897,9.704588695,-0.414174994
10.32941779,9.743914075,-0.394174994
10.23670854,9.78139289,-0.374174994
10.1432683,9.817010148,-0.354174994
10.04913446,9.850751603,-0.334174994
9.954344654,9.882603758,-0.314174994
9.858936807,9.912553873,-0.294174994
9.762949078,9.940589969,-0.274174994
9.666419862,9.966700831,-0.254174994
9.569387769,9.990876015,-0.234174994
9.47189161,10.01310585,-0.214174994
9.373970382,10.03338145,-0.194174994
9.275663254,10.0516947,-0.174174994
9.232175341,10.05930721,-0.154174994
9.331136169,10.04493984,-0.134174994
9.430364534,10.03255444,-0.114174994
9.529820745,10.02215594,-0.094174994
9.629465022,10.01374852,-0.074174994
9.729257508,10.00733553,-0.054174994
9.829158287,10.00291954,-0.034174994
9.929127401,10.00050232,-0.014174994
10,10,0
10,10,0
10.2,10,0
10,10,0
9.8,10,0
9.6,10,0
9.4,10,0
9.2,10,0
9,10,0
8.8,10,0
8.6,10,0
8.4,10,0
1 x y yaw
2 0 0 0
3 0 0 0
4 0.099993333 0.000999967 0.02
5 0.199946671 0.003999467 0.04
6 0.299820032 0.0089973 0.06
7 0.39957347 0.015991468 0.08
8 0.499167083 0.024979174 0.1
9 0.598561036 0.035956821 0.12
10 0.697715573 0.048920019 0.14
11 0.796591033 0.063863583 0.16
12 0.895147867 0.080781536 0.18
13 0.993346654 0.099667111 0.2
14 1.091148115 0.120512753 0.22
15 1.188513132 0.143310126 0.24
16 1.285402759 0.168050109 0.26
17 1.381778243 0.194722808 0.28
18 1.477601033 0.223317554 0.3
19 1.572832803 0.25382291 0.32
20 1.667690714 0.285476899 0.322200686
21 1.7625448 0.317142375 0.322200686
22 1.857398886 0.348807852 0.322200686
23 1.952252972 0.380473329 0.322200686
24 2.047107058 0.412138805 0.322200686
25 2.141961143 0.443804282 0.322200686
26 2.236815229 0.475469758 0.322200686
27 2.331669315 0.507135235 0.322200686
28 2.426523401 0.538800712 0.322200686
29 2.521377487 0.570466188 0.322200686
30 2.616231572 0.602131665 0.322200686
31 2.711085658 0.633797141 0.322200686
32 2.805939744 0.665462618 0.322200686
33 2.90079383 0.697128095 0.322200686
34 2.995647916 0.728793571 0.322200686
35 3.090502001 0.760459048 0.322200686
36 3.185356087 0.792124524 0.322200686
37 3.280210173 0.823790001 0.322200686
38 3.375064259 0.855455477 0.322200686
39 3.469918345 0.887120954 0.322200686
40 3.56477243 0.918786431 0.322200686
41 3.659626516 0.950451907 0.322200686
42 3.754480602 0.982117384 0.322200686
43 3.849334688 1.01378286 0.322200686
44 3.944188773 1.045448337 0.322200686
45 4.039042859 1.077113814 0.322200686
46 4.133896945 1.10877929 0.322200686
47 4.228751031 1.140444767 0.322200686
48 4.323605117 1.172110243 0.322200686
49 4.418459202 1.20377572 0.322200686
50 4.513313288 1.235441197 0.322200686
51 4.608167374 1.267106673 0.322200686
52 4.70302146 1.29877215 0.322200686
53 4.797875546 1.330437626 0.322200686
54 4.892729631 1.362103103 0.322200686
55 4.987583717 1.39376858 0.322200686
56 5.082437803 1.425434056 0.322200686
57 5.177291889 1.457099533 0.322200686
58 5.272145975 1.488765009 0.322200686
59 5.36700006 1.520430486 0.322200686
60 5.461854146 1.552095962 0.322200686
61 5.556708232 1.583761439 0.322200686
62 5.651562318 1.615426916 0.322200686
63 5.746416404 1.647092392 0.322200686
64 5.841270489 1.678757869 0.322200686
65 5.936124575 1.710423345 0.322200686
66 6.030978724 1.742088632 0.321917939
67 6.126151812 1.77277658 0.301917939
68 6.221919583 1.801555056 0.281917939
69 6.318243732 1.828412548 0.261917939
70 6.415085732 1.853338314 0.241917939
71 6.512406845 1.876322385 0.221917939
72 6.610168146 1.897355566 0.201917939
73 6.708330531 1.916429446 0.181917939
74 6.806854736 1.933536393 0.161917939
75 6.905701353 1.948669567 0.141917939
76 7.004830845 1.961822914 0.121917939
77 7.104203561 1.972991172 0.101917939
78 7.203779754 1.982169875 0.081917939
79 7.303519593 1.989355351 0.061917939
80 7.403383185 1.994544727 0.041917939
81 7.503330586 1.997735926 0.021917939
82 7.603321818 1.998927671 0.001917939
83 7.703316885 1.998119488 -0.018082061
84 7.803275792 1.995311697 -0.038082061
85 7.903158555 1.990505424 -0.058082061
86 8.002925223 1.98370259 -0.078082061
87 8.102535892 1.974905915 -0.098082061
88 8.201950716 1.96411892 -0.118082061
89 8.301129933 1.951345918 -0.138082061
90 8.400033872 1.936592018 -0.158082061
91 8.498622973 1.919863122 -0.178082061
92 8.5968578 1.901165921 -0.198082061
93 8.694699062 1.880507894 -0.218082061
94 8.792107624 1.857897304 -0.238082061
95 8.835526197 1.847154409 -0.247027664
96 8.835526197 1.847154409 -0.247027664
97 8.932728598 1.823673356 -0.227027664
98 9.03038115 1.802140918 -0.207027664
99 9.128444791 1.782565707 -0.187027664
100 9.226880299 1.764955554 -0.167027664
101 9.3256483 1.749317501 -0.147027664
102 9.424709288 1.735657804 -0.127027664
103 9.52402364 1.723981926 -0.107027664
104 9.623551632 1.714294538 -0.087027664
105 9.723253454 1.706599515 -0.067027664
106 9.823089226 1.700899934 -0.047027664
107 9.923019016 1.697198075 -0.027027664
108 10.02300285 1.695495419 -0.007027664
109 10.12300074 1.695792648 0.012972336
110 10.22297269 1.698089641 0.032972336
111 10.32287871 1.70238548 0.052972336
112 10.42267884 1.708678448 0.072972336
113 10.52233315 1.716966026 0.092972336
114 10.6218018 1.727244901 0.112972336
115 10.72104499 1.73951096 0.132972336
116 10.82002302 1.753759297 0.152972336
117 10.91869632 1.769984214 0.172972336
118 11.0170254 1.78817922 0.192972336
119 11.11497094 1.808337037 0.212972336
120 11.21249376 1.830449604 0.232972336
121 11.30955486 1.854508074 0.252972336
122 11.40611541 1.880502826 0.272972336
123 11.50213679 1.908423461 0.292972336
124 11.59758058 1.938258811 0.312972336
125 11.69240863 1.969996944 0.332972336
126 11.78658298 2.003625163 0.352972336
127 11.88006599 2.039130019 0.372972336
128 11.97307064 2.07587289 0.369773015
129 12.06666676 2.111078482 0.349773015
130 12.16094822 2.144405236 0.329773015
131 12.25587732 2.175839821 0.309773015
132 12.35141608 2.205369664 0.289773015
133 12.44752629 2.232982953 0.269773015
134 12.54416951 2.258668644 0.249773015
135 12.64130708 2.282416463 0.229773015
136 12.73890015 2.30421691 0.209773015
137 12.83690968 2.324061265 0.189773015
138 12.93529647 2.341941592 0.169773015
139 13.03402117 2.357850739 0.149773015
140 13.13304428 2.371782342 0.129773015
141 13.23232621 2.383730828 0.109773015
142 13.33182723 2.393691418 0.089773015
143 13.43150755 2.401660128 0.069773015
144 13.5313273 2.407633772 0.049773015
145 13.63124655 2.411609958 0.029773015
146 13.73122533 2.413587098 0.009773015
147 13.83122366 2.4135644 -0.010226985
148 13.93120154 2.411541873 -0.030226985
149 14.03111898 2.407520326 -0.050226985
150 14.130936 2.401501369 -0.070226985
151 14.2306127 2.393487407 -0.090226985
152 14.33010919 2.383481647 -0.110226985
153 14.42938568 2.371488091 -0.130226985
154 14.52840246 2.357511537 -0.150226985
155 14.62711992 2.341557574 -0.170226985
156 14.72549858 2.323632584 -0.190226985
157 14.8234991 2.303743737 -0.210226985
158 14.92108226 2.281898988 -0.230226985
159 15.01820904 2.258107074 -0.250226985
160 15.11484059 2.232377513 -0.270226985
161 15.21093825 2.204720595 -0.290226985
162 15.3064636 2.175147384 -0.310226985
163 15.40137842 2.143669707 -0.330226985
164 15.49564474 2.110300156 -0.350226985
165 15.58922487 2.075052077 -0.370226985
166 15.68208136 2.037939571 -0.390226985
167 15.77417708 1.99897748 -0.410226985
168 15.8654752 1.95818139 -0.430226985
169 15.95593919 1.915567618 -0.450226985
170 16.04553286 1.87115321 -0.470226985
171 15.98434311 1.903321256 -0.490226985
172 15.8965972 1.951282929 -0.510226985
173 15.80982801 2.000989811 -0.530226985
174 15.72407024 2.05242202 -0.550226985
175 15.6393582 2.105558985 -0.570226985
176 15.55572577 2.16037945 -0.590226985
177 15.47320641 2.216861488 -0.610226985
178 15.39183311 2.274982508 -0.630226985
179 15.31163842 2.334719261 -0.650226985
180 15.23265444 2.396047854 -0.670226985
181 15.15491274 2.458943756 -0.690226985
182 15.07844442 2.52338181 -0.710226985
183 15.00328006 2.589336241 -0.730226985
184 14.92944975 2.656780669 -0.750226985
185 14.85698299 2.725688116 -0.770226985
186 14.78590879 2.79603102 -0.790226985
187 14.71625556 2.867781246 -0.810226985
188 14.64805118 2.940910093 -0.830226985
189 14.58132291 3.015388313 -0.850226985
190 14.51609745 3.091186113 -0.870226985
191 14.4524009 3.168273176 -0.890226985
192 14.39025872 3.246618669 -0.910226985
193 14.32969577 3.326191254 -0.930226985
194 14.27073628 3.406959103 -0.950226985
195 14.21340384 3.48888991 -0.970226985
196 14.15772136 3.571950905 -0.990226985
197 14.10371113 3.656108862 -1.010226985
198 14.05139475 3.741330122 -1.030226985
199 14.00079315 3.827580595 -1.050226985
200 13.95192656 3.914825784 -1.070226985
201 13.90481453 4.00303079 -1.090226985
202 13.8594759 4.092160334 -1.110226985
203 13.81592881 4.182178765 -1.130226985
204 13.77419068 4.273050077 -1.150226985
205 13.73427821 4.364737922 -1.170226985
206 13.69767916 4.453530976 -1.189435296
207 13.69767916 4.453530976 -1.189435296
208 13.66139137 4.546712854 -1.209435296
209 13.62697434 4.640601804 -1.229435296
210 13.59444185 4.735160271 -1.249435296
211 13.56380691 4.830350434 -1.269435296
212 13.53508177 4.926134218 -1.289435296
213 13.50827793 5.022473309 -1.309435296
214 13.4834061 5.119329175 -1.329435296
215 13.46047623 5.216663074 -1.349435296
216 13.4394975 5.314436073 -1.369435296
217 13.42047829 5.412609065 -1.389435296
218 13.40342621 5.511142782 -1.409435296
219 13.38834809 5.609997812 -1.429435296
220 13.37524995 5.709134613 -1.449435296
221 13.36259395 5.808329049 -1.434174994
222 13.34798454 5.907254433 -1.414174994
223 13.33139967 6.005867865 -1.394174994
224 13.31284599 6.104129899 -1.374174994
225 13.2923309 6.202001233 -1.354174994
226 13.26986262 6.299442719 -1.334174994
227 13.24545014 6.396415381 -1.314174994
228 13.21910321 6.492880432 -1.294174994
229 13.19083238 6.588799288 -1.274174994
230 13.16064896 6.684133581 -1.254174994
231 13.12856501 6.77884518 -1.234174994
232 13.09459337 6.872896201 -1.214174994
233 13.05874763 6.966249025 -1.194174994
234 13.02104213 7.058866313 -1.174174994
235 12.98149195 7.150711017 -1.154174994
236 12.94011291 7.241746403 -1.134174994
237 12.89692155 7.331936056 -1.114174994
238 12.85193516 7.421243902 -1.094174994
239 12.80517173 7.50963422 -1.074174994
240 12.75664997 7.597071654 -1.054174994
241 12.70638927 7.68352123 -1.034174994
242 12.65440975 7.76894837 -1.014174994
243 12.6007322 7.853318904 -0.994174994
244 12.54537808 7.936599086 -0.974174994
245 12.48836955 8.018755603 -0.954174994
246 12.42972939 8.099755594 -0.934174994
247 12.36948107 8.179566662 -0.914174994
248 12.30764868 8.258156881 -0.894174994
249 12.24425696 8.335494818 -0.874174994
250 12.17933126 8.411549538 -0.854174994
251 12.11289756 8.486290619 -0.834174994
252 12.04498241 8.559688168 -0.814174994
253 11.975613 8.631712825 -0.794174994
254 11.90481707 8.702335782 -0.774174994
255 11.83262292 8.771528791 -0.754174994
256 11.75905945 8.839264175 -0.734174994
257 11.68415607 8.905514841 -0.714174994
258 11.60794275 8.970254289 -0.694174994
259 11.53044997 9.033456626 -0.674174994
260 11.45170872 9.095096569 -0.654174994
261 11.37175051 9.155149466 -0.634174994
262 11.2906073 9.213591294 -0.614174994
263 11.20831157 9.270398679 -0.594174994
264 11.12489622 9.325548897 -0.574174994
265 11.04039463 9.379019891 -0.554174994
266 10.95484058 9.430790271 -0.534174994
267 10.86826831 9.48083933 -0.514174994
268 10.78071244 9.52914705 -0.494174994
269 10.69220799 9.575694108 -0.474174994
270 10.60279036 9.620461886 -0.454174994
271 10.51249531 9.663432478 -0.434174994
272 10.42135897 9.704588695 -0.414174994
273 10.32941779 9.743914075 -0.394174994
274 10.23670854 9.78139289 -0.374174994
275 10.1432683 9.817010148 -0.354174994
276 10.04913446 9.850751603 -0.334174994
277 9.954344654 9.882603758 -0.314174994
278 9.858936807 9.912553873 -0.294174994
279 9.762949078 9.940589969 -0.274174994
280 9.666419862 9.966700831 -0.254174994
281 9.569387769 9.990876015 -0.234174994
282 9.47189161 10.01310585 -0.214174994
283 9.373970382 10.03338145 -0.194174994
284 9.275663254 10.0516947 -0.174174994
285 9.232175341 10.05930721 -0.154174994
286 9.331136169 10.04493984 -0.134174994
287 9.430364534 10.03255444 -0.114174994
288 9.529820745 10.02215594 -0.094174994
289 9.629465022 10.01374852 -0.074174994
290 9.729257508 10.00733553 -0.054174994
291 9.829158287 10.00291954 -0.034174994
292 9.929127401 10.00050232 -0.014174994
293 10 10 0
294 10 10 0
295 10.2 10 0
296 10 10 0
297 9.8 10 0
298 9.6 10 0
299 9.4 10 0
300 9.2 10 0
301 9 10 0
302 8.8 10 0
303 8.6 10 0
304 8.4 10 0

68
PathTracking/pure_pursuit/unicycle_model.py
View File

@@ -1,68 +0,0 @@
#! /usr/bin/python
# -*- coding: utf-8 -*-
"""
author Atsushi Sakai
"""
import math
dt = 0.1 # [s]
L = 2.9 # [m]
class State:
def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
self.x = x
self.y = y
self.yaw = yaw
self.v = v
def update(state, a, delta):
state.x = state.x + state.v * math.cos(state.yaw) * dt
state.y = state.y + state.v * math.sin(state.yaw) * dt
state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
state.v = state.v + a * dt
return state
if __name__ == '__main__':
print("start unicycle simulation")
import matplotlib.pyplot as plt
T = 100
a = [1.0] * T
delta = [math.radians(1.0)] * T
# print(delta)
# print(a, delta)
state = State()
x = []
y = []
yaw = []
v = []
for (ai, di) in zip(a, delta):
state = update(state, ai, di)
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
flg, ax = plt.subplots(1)
plt.plot(x, y)
plt.axis("equal")
plt.grid(True)
flg, ax = plt.subplots(1)
plt.plot(v)
plt.grid(True)
plt.show()