Skip to content

Update potential fields with different radius #142

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
brunoeducsantos wants to merge 2 commits into from
Closed

Update potential fields with different radius #142

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b0f3559
Update potential fields with different radius
brunoeducsantos Dec 7, 2018
c398dc2
update potential field example
brunoeducsantos Dec 7, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 9 additions & 9 deletions PathPlanning/PotentialFieldPlanning/potential_field_planning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
show_animation = True


def calc_potential_field(gx, gy, ox, oy, reso, rr):
def calc_potential_field(gx, gy, ox, oy, reso, rr,radius):
minx = min(ox) - AREA_WIDTH / 2.0
miny = min(oy) - AREA_WIDTH / 2.0
maxx = max(ox) + AREA_WIDTH / 2.0
Expand All @@ -37,7 +37,7 @@ def calc_potential_field(gx, gy, ox, oy, reso, rr):
for iy in range(yw):
y = iy * reso + miny
ug = calc_attractive_potential(x, y, gx, gy)
uo = calc_repulsive_potential(x, y, ox, oy, rr)
uo = calc_repulsive_potential(x, y, ox, oy, rr,radius)
uf = ug + uo
pmap[ix][iy] = uf

Expand All @@ -48,7 +48,7 @@ def calc_attractive_potential(x, y, gx, gy):
return 0.5 * KP * np.hypot(x - gx, y - gy)


def calc_repulsive_potential(x, y, ox, oy, rr):
def calc_repulsive_potential(x, y, ox, oy, rr,radius):
# search nearest obstacle
minid = -1
dmin = float("inf")
Expand All @@ -61,11 +61,11 @@ def calc_repulsive_potential(x, y, ox, oy, rr):
# calc repulsive potential
dq = np.hypot(x - ox[minid], y - oy[minid])

if dq <= rr:
if dq <= (rr+radius[minid]):
if dq <= 0.1:
dq = 0.1

return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
return 0.5 * ETA * (1.0 / dq - 1.0 / (rr+radius[minid])) ** 2
else:
return 0.0

Expand All @@ -84,10 +84,10 @@ def get_motion_model():
return motion


def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr,radius):

# 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,radius)

# search path
d = np.hypot(sx - gx, sy - gy)
Expand Down Expand Up @@ -148,7 +148,7 @@ def main():
gy = 30.0 # goal y position [m]
grid_size = 0.5 # potential grid size [m]
robot_radius = 5.0 # robot radius [m]

radius = [3.,3.,3.,3.,3.]
ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m]
oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m]

Expand All @@ -158,7 +158,7 @@ def main():

# path generation
rx, ry = potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
sx, sy, gx, gy, ox, oy, grid_size, robot_radius,radius)

print(rx)
print(ry)
Expand Down