AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 21:58:46 -05:00
Code Issues Packages Projects Releases Wiki Activity

Potential field - potential range and ocillations (#345)

Browse Source 
* Potential cover start and goal as well

* Oscillation detection and fix potential look if outside range

* Formatting

* cleaner and flexible oscillation detection

Co-authored-by: Adi Vardi <adidasv111@gmail.com>
This commit is contained in:
adi-vardi-enway
2020-07-04 11:44:35 +08:00
committed by GitHub
parent fa1585d880
commit effddd2f6b
1 changed files with 33 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
PathPlanning/PotentialFieldPlanning/potential_field_planning.py
View File

@@ -9,6 +9,7 @@ https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
"""
from collections import deque
import numpy as np
import matplotlib.pyplot as plt
@@ -16,15 +17,17 @@ import matplotlib.pyplot as plt
KP = 5.0 # attractive potential gain
ETA = 100.0 # repulsive potential gain
AREA_WIDTH = 30.0 # potential area width [m]
# the number of previous positions used to check oscillations
OSCILLATIONS_DETECTION_LENGTH = 3
show_animation = True
def calc_potential_field(gx, gy, ox, oy, reso, rr):
minx = min(ox) - AREA_WIDTH / 2.0
miny = min(oy) - AREA_WIDTH / 2.0
maxx = max(ox) + AREA_WIDTH / 2.0
maxy = max(oy) + AREA_WIDTH / 2.0
def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
xw = int(round((maxx - minx) / reso))
yw = int(round((maxy - miny) / reso))
@@ -84,10 +87,26 @@ def get_motion_model():
return motion
def oscillations_detection(previous_ids, ix, iy):
previous_ids.append((ix, iy))
if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
previous_ids.popleft()
# check if contains any duplicates by copying into a set
previous_ids_set = set()
for index in previous_ids:
if index in previous_ids_set:
return True
else:
previous_ids_set.add(index)
return False
def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
# calc potential field
pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr)
pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)
# search path
d = np.hypot(sx - gx, sy - gy)
@@ -106,14 +125,17 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
rx, ry = [sx], [sy]
motion = get_motion_model()
previous_ids = deque()
while d >= reso:
minp = float("inf")
minix, miniy = -1, -1
for i, _ in enumerate(motion):
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
if inx >= len(pmap) or iny >= len(pmap[0]):
if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
p = float("inf") # outside area
print("outside potential!")
else:
p = pmap[inx][iny]
if minp > p:
@@ -128,6 +150,10 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
rx.append(xp)
ry.append(yp)
if (oscillations_detection(previous_ids, ix, iy)):
print("Oscillation detected at ({},{})!".format(ix, iy))
break
if show_animation:
plt.plot(ix, iy, ".r")
plt.pause(0.01)