github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Simplified calc_target_index function in stanley_controller.py

Browse Source 
Used:

- np.hypot to replace np.sqrt(x**2 + y**2) 
- np.argmin to find location of a minimum in a numpy array
This commit is contained in:
Guillaume Jacquenot
2019-12-07 14:40:28 +01:00
committed by GitHub
parent 153cfd0740
commit 22e92ed558
1 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
PathTracking/stanley_controller/stanley_controller.py
View File

@@ -127,20 +127,19 @@ def calc_target_index(state, cx, cy):
:param cy: [float]
:return: (int, float)
"""
# Calc front axle position
# Calc front axle position
fx = state.x + L * np.cos(state.yaw)
fy = state.y + L * np.sin(state.yaw)
# Search nearest point index
dx = [fx - icx for icx in cx]
dy = [fy - icy for icy in cy]
d = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
closest_error = min(d)
target_idx = d.index(closest_error)
d = np.hypot(dx, dy)
target_idx = np.argmin(d)
# Project RMS error onto front axle vector
front_axle_vec = [-np.cos(state.yaw + np.pi / 2),
- np.sin(state.yaw + np.pi / 2)]
-np.sin(state.yaw + np.pi / 2)]
error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec)
return target_idx, error_front_axle