107 lines
3.6 KiB
Python
Executable File
107 lines
3.6 KiB
Python
Executable File
import numpy as np
|
|
from scipy.interpolate import interp1d
|
|
from .mpc_config import Params
|
|
|
|
P = Params()
|
|
|
|
|
|
def compute_path_from_wp(start_xp, start_yp, step=0.1):
|
|
"""
|
|
Computes a reference path given a set of waypoints
|
|
"""
|
|
final_xp = []
|
|
final_yp = []
|
|
delta = step # [m]
|
|
for idx in range(len(start_xp) - 1):
|
|
section_len = np.sum(
|
|
np.sqrt(
|
|
np.power(np.diff(start_xp[idx : idx + 2]), 2)
|
|
+ np.power(np.diff(start_yp[idx : idx + 2]), 2)
|
|
)
|
|
)
|
|
interp_range = np.linspace(0, 1, np.floor(section_len / delta).astype(int))
|
|
fx = interp1d(np.linspace(0, 1, 2), start_xp[idx : idx + 2], kind=1)
|
|
fy = interp1d(np.linspace(0, 1, 2), start_yp[idx : idx + 2], kind=1)
|
|
# watch out to duplicate points!
|
|
final_xp = np.append(final_xp, fx(interp_range)[1:])
|
|
final_yp = np.append(final_yp, fy(interp_range)[1:])
|
|
dx = np.append(0, np.diff(final_xp))
|
|
dy = np.append(0, np.diff(final_yp))
|
|
theta = np.arctan2(dy, dx)
|
|
return np.vstack((final_xp, final_yp, theta))
|
|
|
|
|
|
def get_nn_idx(state, path):
|
|
"""
|
|
Computes the index of the waypoint closest to vehicle
|
|
"""
|
|
dx = state[0] - path[0, :]
|
|
dy = state[1] - path[1, :]
|
|
dist = np.hypot(dx, dy)
|
|
nn_idx = np.argmin(dist)
|
|
try:
|
|
v = [
|
|
path[0, nn_idx + 1] - path[0, nn_idx],
|
|
path[1, nn_idx + 1] - path[1, nn_idx],
|
|
]
|
|
v /= np.linalg.norm(v)
|
|
d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]
|
|
if np.dot(d, v) > 0:
|
|
target_idx = nn_idx
|
|
else:
|
|
target_idx = nn_idx + 1
|
|
except IndexError as e:
|
|
target_idx = nn_idx
|
|
return target_idx
|
|
|
|
|
|
def normalize_angle(angle):
|
|
"""
|
|
Normalize an angle to [-pi, pi]
|
|
"""
|
|
while angle > np.pi:
|
|
angle -= 2.0 * np.pi
|
|
while angle < -np.pi:
|
|
angle += 2.0 * np.pi
|
|
return angle
|
|
|
|
|
|
def get_ref_trajectory(state, path, target_v, dl=0.1):
|
|
"""
|
|
For each step in the time horizon
|
|
modified reference in robot frame
|
|
"""
|
|
xref = np.zeros((P.N, P.T + 1))
|
|
dref = np.zeros((1, P.T + 1))
|
|
# sp = np.ones((1,T +1))*target_v #speed profile
|
|
ncourse = path.shape[1]
|
|
ind = get_nn_idx(state, path)
|
|
dx = path[0, ind] - state[0]
|
|
dy = path[1, ind] - state[1]
|
|
xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X
|
|
xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y
|
|
xref[2, 0] = target_v # V
|
|
xref[3, 0] = normalize_angle(path[2, ind] - state[3]) # Theta
|
|
dref[0, 0] = 0.0 # Steer operational point should be 0
|
|
travel = 0.0
|
|
for i in range(1, P.T + 1):
|
|
travel += abs(target_v) * P.DT
|
|
dind = int(round(travel / dl))
|
|
if (ind + dind) < ncourse:
|
|
dx = path[0, ind + dind] - state[0]
|
|
dy = path[1, ind + dind] - state[1]
|
|
xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
|
|
xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
|
|
xref[2, i] = target_v # sp[ind + dind]
|
|
xref[3, i] = normalize_angle(path[2, ind + dind] - state[3])
|
|
dref[0, i] = 0.0
|
|
else:
|
|
dx = path[0, ncourse - 1] - state[0]
|
|
dy = path[1, ncourse - 1] - state[1]
|
|
xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])
|
|
xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])
|
|
xref[2, i] = 0.0 # stop? #sp[ncourse - 1]
|
|
xref[3, i] = normalize_angle(path[2, ncourse - 1] - state[3])
|
|
dref[0, i] = 0.0
|
|
return xref, dref
|