mpc_python_learn/mpc_pybullet_demo/mpcpy/utils.py

108 lines
3.6 KiB
Python
Raw Normal View History

2019-12-17 18:37:32 +08:00
import numpy as np
from scipy.interpolate import interp1d
2021-07-08 19:54:48 +08:00
from .mpc_config import Params
P = Params()
def compute_path_from_wp(start_xp, start_yp, step=0.1):
2019-12-17 18:37:32 +08:00
"""
Computes a reference path given a set of waypoints
2019-12-17 18:37:32 +08:00
"""
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))
2020-01-13 20:25:26 +08:00
def get_nn_idx(state, path):
2020-07-01 00:21:27 +08:00
"""
Computes the index of the waypoint closest to vehicle
2020-07-01 00:21:27 +08:00
"""
dx = state[0] - path[0, :]
dy = state[1] - path[1, :]
dist = np.hypot(dx, dy)
2020-07-01 00:21:27 +08:00
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],
]
2020-07-01 00:21:27 +08:00
v /= np.linalg.norm(v)
d = [path[0, nn_idx] - state[0], path[1, nn_idx] - state[1]]
if np.dot(d, v) > 0:
2020-07-01 00:21:27 +08:00
target_idx = nn_idx
else:
target_idx = nn_idx + 1
2020-07-01 00:21:27 +08:00
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):
2020-07-01 00:21:27 +08:00
"""
For each step in the time horizon
modified reference in robot frame
2020-07-01 00:21:27 +08:00
"""
K = int(P.T / P.DT)
xref = np.zeros((P.N, K + 1))
dref = np.zeros((1, K + 1))
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
dl = np.hypot(path[0, 1] - path[0, 0], path[1, 1] - path[1, 0])
for i in range(1, K + 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