131 lines
4.1 KiB
Python
Executable File
131 lines
4.1 KiB
Python
Executable File
import numpy as np
|
|
from scipy.interpolate import interp1d
|
|
|
|
|
|
def compute_path_from_wp(start_xp, start_yp, step=0.1):
|
|
"""
|
|
|
|
Args:
|
|
start_xp (array-like): 1D array of x-positions
|
|
start_yp (array-like): 1D array of y-positions
|
|
step (float): intepolation step
|
|
|
|
Returns:
|
|
ndarray of shape (3,N) representing the path as x,y,heading
|
|
"""
|
|
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):
|
|
"""
|
|
Finds the index of the closest element
|
|
|
|
Args:
|
|
state (array-like): 1D array whose first two elements are x-pos and y-pos
|
|
path (ndarray): 2D array of shape (2,N) of x,y points
|
|
|
|
Returns:
|
|
int: the index of the closest element
|
|
"""
|
|
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 get_ref_trajectory(state, path, target_v, T, DT):
|
|
"""
|
|
|
|
Args:
|
|
state (array-like): state of the vehicle in global frame
|
|
path (ndarray): 2D array representing the path as x,y,heading points in global frame
|
|
target_v (float): desired speed
|
|
T (float): control horizon duration
|
|
DT (float): control horizon time-step
|
|
|
|
Returns:
|
|
ndarray: 2D array representing state space trajectory [x_k, y_k, v_k, theta_k] w.r.t ego frame.
|
|
Interpolated according to the time-step and the desired velocity
|
|
"""
|
|
K = int(T / DT)
|
|
|
|
xref = np.zeros((4, K))
|
|
ind = get_nn_idx(state, path)
|
|
|
|
cdist = np.append(
|
|
[0.0], np.cumsum(np.hypot(np.diff(path[0, :].T), np.diff(path[1, :]).T))
|
|
)
|
|
cdist = np.clip(cdist, cdist[0], cdist[-1])
|
|
|
|
start_dist = cdist[ind]
|
|
interp_points = [d * DT * target_v + start_dist for d in range(1, K + 1)]
|
|
xref[0, :] = np.interp(interp_points, cdist, path[0, :])
|
|
xref[1, :] = np.interp(interp_points, cdist, path[1, :])
|
|
xref[2, :] = target_v
|
|
xref[3, :] = np.interp(interp_points, cdist, path[2, :])
|
|
|
|
# points where the vehicle is at the end of trajectory
|
|
xref_cdist = np.interp(interp_points, cdist, cdist)
|
|
stop_idx = np.where(xref_cdist == cdist[-1])
|
|
xref[2, stop_idx] = 0.0
|
|
|
|
# transform in ego frame
|
|
dx = xref[0, :] - state[0]
|
|
dy = xref[1, :] - state[1]
|
|
xref[0, :] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) # X
|
|
xref[1, :] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) # Y
|
|
xref[3, :] = path[2, ind] - state[3] # Theta
|
|
|
|
def fix_angle_reference(angle_ref, angle_init):
|
|
"""
|
|
Removes jumps greater than 2PI to smooth the heading
|
|
|
|
Args:
|
|
angle_ref (array-like):
|
|
angle_init (float):
|
|
|
|
Returns:
|
|
array-like:
|
|
"""
|
|
diff_angle = angle_ref - angle_init
|
|
diff_angle = np.unwrap(diff_angle)
|
|
return angle_init + diff_angle
|
|
|
|
xref[3, :] = (xref[3, :] + np.pi) % (2.0 * np.pi) - np.pi
|
|
xref[3, :] = fix_angle_reference(xref[3, :], xref[3, 0])
|
|
|
|
return xref
|