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

upgrade numpy to 1.25.0 and fix bugs (#861)

Browse Source
This commit is contained in:
Atsushi Sakai
2023-07-01 15:30:32 +09:00
committed by GitHub
parent 67edaf8b02
commit 4d71470631
18 changed files with 66 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py
View File

@@ -81,7 +81,7 @@ def generate_lookup_table():
if x is not None:
print("find good path")
lookup_table.append(
[x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
[x[-1], y[-1], yaw[-1], float(p[0, 0]), float(p[1, 0]), float(p[2, 0])])
print("finish lookup table generation")

39
PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
View File

@@ -1,10 +1,10 @@
import math
import numpy as np
import scipy.interpolate
from scipy.interpolate import interp1d
# motion parameter
L = 1.0 # wheel base
ds = 0.1 # course distanse
ds = 0.1 # course distance
v = 10.0 / 3.6 # velocity [m/s]
@@ -22,7 +22,6 @@ def pi_2_pi(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
@@ -33,18 +32,20 @@ def update(state, v, delta, dt, L):
def generate_trajectory(s, km, kf, k0):
n = s / ds
time = s / v # [s]
if isinstance(time, type(np.array([]))): time = time[0]
if isinstance(km, type(np.array([]))): km = km[0]
if isinstance(kf, type(np.array([]))): kf = kf[0]
if isinstance(time, type(np.array([]))):
time = time[0]
if isinstance(km, type(np.array([]))):
km = km[0]
if isinstance(kf, type(np.array([]))):
kf = kf[0]
tk = np.array([0.0, time / 2.0, time])
kk = np.array([k0, km, kf])
t = np.arange(0.0, time, time / n)
fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic")
fkp = interp1d(tk, kk, kind="quadratic")
kp = [fkp(ti) for ti in t]
dt = float(time / n)
@@ -64,18 +65,22 @@ def generate_trajectory(s, km, kf, k0):
def generate_last_state(s, km, kf, k0):
n = s / ds
time = s / v # [s]
if isinstance(time, type(np.array([]))): time = time[0]
if isinstance(km, type(np.array([]))): km = km[0]
if isinstance(kf, type(np.array([]))): kf = kf[0]
if isinstance(n, type(np.array([]))):
n = n[0]
if isinstance(time, type(np.array([]))):
time = time[0]
if isinstance(km, type(np.array([]))):
km = km[0]
if isinstance(kf, type(np.array([]))):
kf = kf[0]
tk = np.array([0.0, time / 2.0, time])
kk = np.array([k0, km, kf])
t = np.arange(0.0, time, time / n)
fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic")
fkp = interp1d(tk, kk, kind="quadratic")
kp = [fkp(ti) for ti in t]
dt = time / n

6
PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py
View File

@@ -106,7 +106,7 @@ def show_trajectory(target, xc, yc): # pragma: no cover
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)
xc, yc, yawc = motion_model.generate_trajectory(p[0, 0], p[1, 0], p[2, 0], k0)
dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1)
cost = np.linalg.norm(dc)
@@ -135,7 +135,7 @@ def optimize_trajectory(target, k0, p):
return xc, yc, yawc, p
def test_optimize_trajectory(): # pragma: no cover
def optimize_trajectory_demo(): # pragma: no cover
# target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0))
target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0))
@@ -155,7 +155,7 @@ def test_optimize_trajectory(): # pragma: no cover
def main(): # pragma: no cover
print(__file__ + " start!!")
test_optimize_trajectory()
optimize_trajectory_demo()
if __name__ == '__main__':