visualize potential field

Author: 0aqz0

PathPlanning/PotentialField/PotentialField.py

@@ -45,9 +45,9 @@ def plan(self, start, target):
 
     def calculate_attractive_potential(self, pos, target):
         if pos.dist(target) <= self.da:
-            return 0.5 * self.ka * pos.dist(target) ** 2
+            return self.ka * pos.dist(target) ** 2
         else:
-            return self.ka * (self.da * pos.dist(target) - 0.5 * self.da ** 2)
+            return self.ka * (2 * self.da * pos.dist(target) - self.da ** 2)
 
     def calculate_repulsive_potential(self, pos):
         pr = 0
@@ -56,4 +56,19 @@ def calculate_repulsive_potential(self, pos):
                 return float('inf')
             if obs.dist(pos) <= self.dr:
                 pr = pr + 0.5 * self.kr * (1/obs.dist(pos) - 1/self.dr) ** 2
-        return pr
+        return pr
+
+    def calculate_potential_force(self, pos, target):
+        # attractive force(simple version)
+        attractive_force = Vector(pos.x - target.x, pos.y - target.y) * (-self.ka)
+        # repulsive force
+        repulsive_force = Vector(0, 0)
+        for obs in self.map.obstacles:
+            if obs.dist(pos) == 0:
+                repulsive_force += Vector(float('inf'), float('inf'))
+            elif obs.dist(pos) <= self.dr:
+                repulsive_force += Vector(pos.x - obs.pos.x, pos.y - obs.pos.y) * self.kr * (1/obs.dist(pos) - 1/self.dr) * ((1/obs.dist(pos))**2) * (1/obs.dist(pos))
+        # sum up
+        potential_force = attractive_force + repulsive_force
+
+        return potential_force

PathPlanning/PotentialField/run.py

@@ -2,19 +2,46 @@
 from PathPlanning.utils import *
 
 def main():
-    map = Map(left=0, right=640, top=480, down=0)
+    map = Map(left=0, right=640, top=480, down=0, refresh=False)
     map.add_obstacle(CircleObstacle(pos=Point(200, 200), radius=50))
-    map.add_obstacle(CircleObstacle(pos=Point(350, 250), radius=30))
-    map.add_obstacle(CircleObstacle(pos=Point(450, 400), radius=80))
-    potentialFieldPlanner = PotentialFieldPlanner(map=map, iterations=1e4, step_size=2, ka=1, kr=1e4, da=10, dr=100)
-    start = Point(0, 0)
-    end = Point(640, 480)
+    map.add_obstacle(CircleObstacle(pos=Point(400, 300), radius=40))
+    # map.add_obstacle(CircleObstacle(pos=Point(450, 400), radius=80))
+    potentialFieldPlanner = PotentialFieldPlanner(map=map, iterations=1e4, step_size=2, ka=1, kr=1e7, da=10, dr=50)
+    start = Point(100, 100)
+    end = Point(500, 250)
+    potentialFieldPlanner.plan(start=start, target=end)
+    map.add_geometry(type='point', pos=start.tuple(), size=10, color=(100, 0, 0))
+    map.add_geometry(type='point', pos=end.tuple(), size=10, color=(0, 100, 0))
+    for i in range(len(potentialFieldPlanner.finalPath)-1):
+        map.add_geometry(type='line', start=potentialFieldPlanner.finalPath[i].tuple(), end=potentialFieldPlanner.finalPath[i+1].tuple(), color=(0, 100, 0))
+    # draw potential force
+    for x in range(0, map.right+1, 10):
+        for y in range(0, map.top+1, 10):
+            force = potentialFieldPlanner.calculate_potential_force(Point(x, y), end)
+            force_start = Point(x, y)
+            force_end = force_start + Polar2Vector(5, force.dir())
+            # map.add_geometry(type='point', pos=(x,y), size=3, color=(0, 0, 100))
+            map.add_geometry(type='line', start=force_start.tuple(), end=force_end.tuple(), width=2, color=(120,50,80))
+    # draw potential line
+    potential_lines = [i for i in range(0, 20000, 1000)]
+    for x in range(0, map.right+1, 1):
+        for y in range(0, map.top+1, 1):
+            valid = True
+            for obs in map.obstacles:
+                if obs.dist(Point(x, y)) <= 32:
+                    valid = False
+
+            if valid:
+                offset = 20
+            else:
+                offset = 100
+            potential = potentialFieldPlanner.calculate_attractive_potential(Point(x, y), end) + potentialFieldPlanner.calculate_repulsive_potential(Point(x, y))
+            # print(potential, potentialFieldPlanner.calculate_attractive_potential(Point(x, y), end))
+            for p in potential_lines:
+                if math.fabs(potential-p) < offset:
+                    map.add_geometry(type='point', pos=(x,y), size=1, color=(0, 0, 100))
+
     while map.is_open:
-        potentialFieldPlanner.plan(start=start, target=end)
-        map.add_geometry(type='point', pos=start.tuple(), size=30, color=(100, 0, 0))
-        map.add_geometry(type='point', pos=end.tuple(), size=30, color=(0, 100, 0))
-        for i in range(len(potentialFieldPlanner.finalPath)-1):
-            map.add_geometry(type='line', start=potentialFieldPlanner.finalPath[i].tuple(), end=potentialFieldPlanner.finalPath[i+1].tuple(), color=(0, 100, 0))
         map.render()
 
 if __name__ == '__main__':

PathPlanning/geometry.py

@@ -29,12 +29,18 @@ def __init__(self, x, y):
         self.x = x
         self.y = y
 
+    def dir(self):
+        return math.atan2(self.y, self.x)
+
     def mod(self):
         return math.sqrt(pow(self.x, 2) + pow(self.y, 2))
 
     def __mul__(self, other):
         return Vector(other*self.x, other*self.y)
 
+    def __add__(self, other):
+        return Vector(self.x+other.x, self.y+other.y)
+
 
 def Polar2Vector(dist, theta):
     return Vector(dist*math.cos(theta), dist*math.sin(theta))

PathPlanning/utils.py

@@ -87,13 +87,14 @@ def check_collision(self, other, avoidDist):
 
 
 class Map(Viewer):
-    def __init__(self, top, down, left, right):
+    def __init__(self, top, down, left, right, refresh=True):
         self.top = max(top, down)
         self.down = min(top, down)
         self.left = min(left, right)
         self.right = max(left, right)
         self.length = math.fabs(left - right)
         self.width = math.fabs(top - down)
+        self.refresh = refresh
         Viewer.__init__(self, width=self.length, height=self.width)
         self.obstacles = []
 
@@ -115,16 +116,17 @@ def draw(self):
         # self.draw_line(start=(self.left, self.top), end=(self.right, self.top), lineWidth=5)
         # self.draw_line(start=(self.right, self.top), end=(self.right, self.down), lineWidth=5)
         # self.draw_line(start=(self.right, self.down), end=(self.left, self.down), lineWidth=5)
+        # draw geoms
+        for geom in self.geoms:
+            geom.render()
         # draw obstacles
         for obs in self.obstacles:
             if obs.type() == 'circle':
                 self.draw_circle(pos=obs.pos.tuple(), radius=obs.radius)
             elif obs.type() == 'rectangle':
-                self.draw_polygon(points=obs.vertex(), )
-        # draw geoms
-        for geom in self.geoms:
-            geom.render()
-        self.geoms = []
+                self.draw_polygon(points=obs.vertex(), filled=True)
+        if self.refresh:
+            self.geoms = []
 
 
 class PathPlanner(object):