mpc_python_learn/mpc_demo/utils.py

34 lines
1.1 KiB
Python
Raw Normal View History

2019-12-17 18:37:32 +08:00
import numpy as np
from scipy.interpolate import interp1d
def compute_path_from_wp(start_xp, start_yp, step = 0.1):
"""
Interpolation range is computed to assure one point every fixed distance step [m].
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
:param start_xp: array_like, list of starting x coordinates
:param start_yp: array_like, list of starting y coordinates
:param step: float, interpolation distance [m] between consecutive waypoints
:returns: array_like, of shape (3,N)
"""
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)))
2020-03-04 20:32:29 +08:00
interp_range = np.linspace(0,1,int(section_len/delta))
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
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)
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
final_xp=np.append(final_xp,fx(interp_range))
final_yp=np.append(final_yp,fy(interp_range))
2020-01-13 20:25:26 +08:00
2019-12-17 18:37:32 +08:00
dx = np.append(0, np.diff(final_xp))
dy = np.append(0, np.diff(final_yp))
theta = np.arctan2(dy, dx)
2020-01-13 20:25:26 +08:00
return np.vstack((final_xp,final_yp,theta))