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

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2017-07-18 22:42:13 -07:00
parent d86ba52fc9
commit 46b547d087
6 changed files with 12 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py Normal file
View File

11
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -11,8 +11,9 @@ import motion_model
from matplotrecorder import matplotrecorder
# optimization parameter
maxiter = 100
h = np.matrix([0.1, 0.001, 0.001]).T # parameter sampling distanse
max_iter = 100
h = np.matrix([0.5, 0.02, 0.02]).T # parameter sampling distanse
cost_th = 0.1
matplotrecorder.donothing = True
show_graph = False
@@ -102,14 +103,13 @@ def show_trajectory(target, xc, yc):
def optimize_trajectory(target, k0, p):
for i in range(maxiter):
for i in range(max_iter):
xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0)
dc = np.matrix(calc_diff(target, xc, yc, yawc)).T
# print(dc.T)
cost = np.linalg.norm(dc)
if cost <= 0.05:
if cost <= cost_th:
print("path is ok cost is:" + str(cost))
break
@@ -146,7 +146,6 @@ def test_optimize_trajectory():
show_trajectory(target, x, y)
matplotrecorder.save_movie("animation.gif", 0.1)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")

183
PathPlanning/StateLatticePlanner/model_predictive_trajectory_generator.py
View File

@@ -1,183 +0,0 @@
"""
Model trajectory generator
author: Atsushi Sakai
"""
import numpy as np
import matplotlib.pyplot as plt
import math
import motion_model
from matplotrecorder import matplotrecorder
# optimization parameter
max_iter = 100
h = np.matrix([0.5, 0.02, 0.02]).T # parameter sampling distanse
cost_th = 0.1
matplotrecorder.donothing = True
show_graph = False
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u"""
Plot arrow
"""
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
plt.plot(0, 0)
def calc_diff(target, x, y, yaw):
d = np.array([target.x - x[-1],
target.y - y[-1],
motion_model.pi_2_pi(target.yaw - yaw[-1])])
return d
def calc_J(target, p, h, k0):
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0] + h[0, 0], p[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0] - h[0, 0], p[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0] + h[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0] - h[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] + h[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] - h[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d3 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
J = np.hstack((d1, d2, d3))
return J
def selection_learning_param(dp, p, k0, target):
mincost = float("inf")
mina = 1.0
maxa = 2.0
da = 0.5
for a in np.arange(mina, maxa, da):
tp = p[:, :] + a * dp
xc, yc, yawc = motion_model.generate_last_state(
tp[0], tp[1], tp[2], k0)
dc = np.matrix(calc_diff(target, [xc], [yc], [yawc])).T
cost = np.linalg.norm(dc)
if cost <= mincost and a != 0.0:
mina = a
mincost = cost
# print(mincost, mina)
# input()
return mina
def show_trajectory(target, xc, yc):
plt.clf()
plot_arrow(target.x, target.y, target.yaw)
plt.plot(xc, yc, "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.1)
matplotrecorder.save_frame()
def optimize_trajectory(target, k0, p):
for i in range(max_iter):
xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0)
dc = np.matrix(calc_diff(target, xc, yc, yawc)).T
# print(dc.T)
cost = np.linalg.norm(dc)
if cost <= cost_th:
print("path is ok cost is:" + str(cost))
break
J = calc_J(target, p, h, k0)
try:
dp = - np.linalg.inv(J) * dc
except np.linalg.linalg.LinAlgError:
print("cannot calc path LinAlgError")
xc, yc, yawc, p = None, None, None, None
break
alpha = selection_learning_param(dp, p, k0, target)
p += alpha * np.array(dp)
# print(p.T)
if show_graph:
show_trajectory(target, xc, yc)
else:
xc, yc, yawc, p = None, None, None, None
print("cannot calc path")
return xc, yc, yawc, p
def test_optimize_trajectory():
# target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0))
k0 = 0.0
init_p = np.matrix([6.0, 0.0, 0.0]).T
x, y, yaw, p = optimize_trajectory(target, k0, init_p)
show_trajectory(target, x, y)
matplotrecorder.save_movie("animation.gif", 0.1)
# plt.plot(x, y, "-r")
plot_arrow(target.x, target.y, target.yaw)
plt.axis("equal")
plt.grid(True)
plt.show()
def test_trajectory_generate():
s = 5.0 # [m]
k0 = 0.0
km = math.radians(30.0)
kf = math.radians(-30.0)
# plt.plot(xk, yk, "xr")
# plt.plot(t, kp)
# plt.show()
x, y = motion_model.generate_trajectory(s, km, kf, k0)
plt.plot(x, y, "-r")
plt.axis("equal")
plt.grid(True)
plt.show()
def main():
print(__file__ + " start!!")
# test_trajectory_generate()
test_optimize_trajectory()
if __name__ == '__main__':
main()

82
PathPlanning/StateLatticePlanner/motion_model.py
View File

@@ -1,82 +0,0 @@
import math
import numpy as np
import scipy.interpolate
# motion parameter
L = 1.0 # wheel base
ds = 0.1 # course distanse
v = 10.0 / 3.6 # velocity [m/s]
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 pi_2_pi(angle):
while(angle > math.pi):
angle = angle - 2.0 * math.pi
while(angle < -math.pi):
angle = angle + 2.0 * math.pi
return angle
def update(state, v, delta, dt, L):
state.v = v
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.yaw = pi_2_pi(state.yaw)
return state
def generate_trajectory(s, km, kf, k0):
n = s / ds
time = s / v # [s]
tk = np.array([0.0, time / 2.0, time])
kk = np.array([k0, km, kf])
t = np.arange(0.0, time, time / n)
kp = scipy.interpolate.spline(tk, kk, t, order=2)
dt = float(time / n)
# plt.plot(t, kp)
# plt.show()
state = State()
x, y, yaw = [state.x], [state.y], [state.yaw]
for ikp in kp:
state = update(state, v, ikp, dt, L)
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
return x, y, yaw
def generate_last_state(s, km, kf, k0):
n = s / ds
time = s / v # [s]
tk = np.array([0.0, time / 2.0, time])
kk = np.array([k0, km, kf])
t = np.arange(0.0, time, time / n)
kp = scipy.interpolate.spline(tk, kk, t, order=2)
dt = time / n
# plt.plot(t, kp)
# plt.show()
state = State()
[update(state, v, ikp, dt, L) for ikp in kp]
return state.x, state.y, state.yaw

9
PathPlanning/StateLatticePlanner/state_lattice_planner.py
View File

@@ -3,12 +3,17 @@ State lattice planner with model predictive trajectory generator
author: Atsushi Sakai
"""
import sys
import os
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from matplotlib import pyplot as plt
import numpy as np
import math
import pandas as pd
import model_predictive_trajectory_generator as planner
import motion_model
import ModelPredictiveTrajectoryGenerator.model_predictive_trajectory_generator as planner
import ModelPredictiveTrajectoryGenerator.motion_model as motion_model
def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table):

0
PathPlanning/__init__.py Normal file
View File