WIP - improved architecture of field planner

navigation/repulsorFieldPlanner.py

@@ -29,18 +29,25 @@ def mag(self):
 
 # Generic and specifc types of obstacles that repel a robot
 class Obstacle:
-    def __init__(self, strength:float=1.0, forceIsPositive:bool=True):
+    def __init__(self, strength:float=1.0, forceIsPositive:bool=True, radius:float = 0.25):
         self.strength = strength
         self.forceIsPositive = forceIsPositive
-        self.radius = 0.25
+
+        # Force uses a logistic function to calculate push magnitiude as a fucntion of distance from center.
+        # The radius offsets the center to get a larger or smaller obstacle
+        # The number is in meters, and represents the half-strength point on the logistic function
+        self.radius = radius
+
+        # Parameter to logistic funciton for how steeply the force transitions from "none" to "lots" about the radius
+        self.fieldSteepness = 3.5
 
     def getForceAtPosition(self, position:Translation2d)->Force:
         return Force()
 
     def _distToForceMag(self, dist:float)->float:
         dist = abs(dist)
         #Sigmoid shape
-        forceMag = _logistic_func(-1.0 * dist, self.strength, 4.0, -self.radius)
+        forceMag = _logistic_func(-1.0 * dist, self.strength, self.fieldSteepness, -self.radius)
         if(not self.forceIsPositive):
             forceMag *= -1.0
         return forceMag
@@ -53,13 +60,10 @@ def getTelemTrans(self)->list[Translation2d]:
 # A point obstacle is defined as round, centered at a specific point, with a specific radius
 # It pushes the robot away radially outward from its center
 class PointObstacle(Obstacle):
-    def __init__(self, location:Translation2d, strength:float=1.0, forceIsPositive:bool=True):
-        self.strength = strength
-        self.forceIsPositive = forceIsPositive
+    def __init__(self, location:Translation2d, strength:float=1.0, forceIsPositive:bool=True, radius:float = 0.3):
         self.location = location
-        self.radius = .4
-
-
+        super().__init__(strength, forceIsPositive,radius)
+
     def getForceAtPosition(self, position: Translation2d) -> Force:
         deltaX =  self.location.x - position.x
         deltaY =  self.location.y - position.y
@@ -78,11 +82,10 @@ def getTelemTrans(self) -> list[Translation2d]:
 # They push the robot away along a perpendicular direction
 # with the specific direction determined by forceIsPositive
 class HorizontalObstacle(Obstacle):
-    def __init__(self, y:float, strength:float=1.0, forceIsPositive:bool=True):
-        self.strength = strength
-        self.forceIsPositive = forceIsPositive
+    def __init__(self, y:float, strength:float=1.0, forceIsPositive:bool=True, radius:float = 0.5):
         self.y=y
-        self.radius = 0.5
+        super().__init__(strength, forceIsPositive,radius)
+
 
     def getForceAtPosition(self, position: Translation2d) -> Force:
         return Force(0, self._distToForceMag(self.y - position.Y()))
@@ -94,12 +97,9 @@ def getTelemTrans(self) -> list[Translation2d]:
         return []
 
 class VerticalObstacle(Obstacle):
-    def __init__(self, x:float, strength:float=1.0, forceIsPositive:bool=True):
-        self.strength = strength
-        self.forceIsPositive = forceIsPositive
+    def __init__(self, x:float, strength:float=1.0, forceIsPositive:bool=True, radius:float = 0.5):
         self.x=x
-        self.radius = 0.5
-
+        super().__init__(strength, forceIsPositive,radius)
 
     def getForceAtPosition(self, position: Translation2d) -> Force:
         return Force(self._distToForceMag(self.x - position.X()), 0)