github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-11 11:57:42 -05:00
Code Issues Packages Projects Releases Wiki Activity

Optimize calc_nearest_index

Browse Source
This commit is contained in:
Alexis Paques
2018-07-12 13:26:18 +02:00
parent 8652a4b7d1
commit 8f6a30b924
4 changed files with 16 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
View File

@@ -142,12 +142,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
dx = [state.x - icx for icx in cx] dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy] dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
mind = min(d) mind = min(d)
ind = d.index(mind) ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x dxl = cx[ind] - state.x
dyl = cy[ind] - state.y dyl = cy[ind] - state.y

4
PathTracking/lqr_steer_control/lqr_steer_control.py
View File

@@ -140,12 +140,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
dx = [state.x - icx for icx in cx] dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy] dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
mind = min(d) mind = min(d)
ind = d.index(mind) ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x dxl = cx[ind] - state.x
dyl = cy[ind] - state.y dyl = cy[ind] - state.y

12
PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
View File

@@ -188,20 +188,22 @@ def calc_nearest_index(state, cx, cy, cyaw, pind):
dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]] dx = [state.x - icx for icx in cx[pind:(pind + N_IND_SEARCH)]]
dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]] dy = [state.y - icy for icy in cy[pind:(pind + N_IND_SEARCH)]]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
min_d = min(d) mind = min(d)
ind = d.index(min_d) + pind ind = d.index(mind) + pind
mind = math.sqrt(mind)
dxl = cx[ind] - state.x dxl = cx[ind] - state.x
dyl = cy[ind] - state.y dyl = cy[ind] - state.y
angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl))
if angle < 0: if angle < 0:
min_d *= -1 mind *= -1
return ind, min_d return ind, mind
def predict_motion(x0, oa, od, xref): def predict_motion(x0, oa, od, xref):

4
PathTracking/rear_wheel_feedback/rear_wheel_feedback.py
View File

@@ -83,12 +83,14 @@ def calc_nearest_index(state, cx, cy, cyaw):
dx = [state.x - icx for icx in cx] dx = [state.x - icx for icx in cx]
dy = [state.y - icy for icy in cy] dy = [state.y - icy for icy in cy]
d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)]
mind = min(d) mind = min(d)
ind = d.index(mind) ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x dxl = cx[ind] - state.x
dyl = cy[ind] - state.y dyl = cy[ind] - state.y