AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-15 03:07:56 -05:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #84 from AlexisTM/optimize_calc_nearest_index

Browse Source 
Optimize calc_nearest_index
This commit is contained in:
Atsushi Sakai
2018-07-12 21:00:36 +09:00
committed by GitHub
parent 8652a4b7d1 8f6a30b924
commit dec1131388
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]
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)
ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x
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]
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)
ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x
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)]]
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
dyl = cy[ind] - state.y
angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl))
if angle < 0:
min_d *= -1
mind *= -1
return ind, min_d
return ind, mind
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]
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)
ind = d.index(mind)
mind = math.sqrt(mind)
dxl = cx[ind] - state.x
dyl = cy[ind] - state.y