diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index d4b2dd0..2d14680 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -29,10 +29,17 @@ 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() @@ -40,7 +47,7 @@ def getForceAtPosition(self, position:Translation2d)->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)