mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-12 23:58:04 -05:00
cc3fd0c55e
* switched to using utils.angle_mod() * switched to using utils.angle_mod() * renamed mod2pi to pi_2_pi * Removed linting errors * switched to using utils.angle_mod() * switched to using utils.angle_mod() * renamed mod2pi to pi_2_pi * Removed linting errors * annotation changes and round precision * Reverted to mod2pi --------- Co-authored-by: Videh Patel <videh.patel@fluxauto.xyz>
96 lines
2.1 KiB
Python
96 lines
2.1 KiB
Python
import math
|
|
import numpy as np
|
|
from scipy.interpolate import interp1d
|
|
from utils.angle import angle_mod
|
|
|
|
# motion parameter
|
|
L = 1.0 # wheel base
|
|
ds = 0.1 # course distance
|
|
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):
|
|
return angle_mod(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]
|
|
|
|
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 = interp1d(tk, kk, kind="quadratic")
|
|
kp = [fkp(ti) for ti in t]
|
|
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]
|
|
|
|
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 = interp1d(tk, kk, kind="quadratic")
|
|
kp = [fkp(ti) for ti in t]
|
|
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
|