Added support and one-off testing for obstacles shaped like lines, an…

…d "lanes" that pull and funnel a robot along themselves

drivetrain/controlStrategies/autoDrive.py

@@ -1,17 +1,16 @@
-from wpimath.geometry import Pose2d, Rotation2d, Translation2d
+from wpimath.geometry import Pose2d, Translation2d
 from wpimath.trajectory import Trajectory
 from drivetrain.controlStrategies.holonomicDriveController import HolonomicDriveController
 from drivetrain.drivetrainCommand import DrivetrainCommand
 from navigation.obstacleDetector import ObstacleDetector
 from utils.signalLogging import log
 from utils.singleton import Singleton
-from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
+from navigation.repulsorFieldPlanner import GOAL_PICKUP, GOAL_SPEAKER, RepulsorFieldPlanner
 from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED
 from utils.allianceTransformUtils import transform
 
 
-GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0))
-GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(180.0))
+
 SPEED_SCALAR = 0.75
 
 class AutoDrive(metaclass=Singleton):

navigation/obstacles.py

@@ -5,9 +5,9 @@
 
 # Generic and specifc types of obstacles that repel a robot
 class Obstacle:
-    def __init__(self, strength:float=1.0, forceIsPositive:bool=True, radius:float = 0.25):
+    def __init__(self, strength:float=1.0, radius:float = 0.25):
         self.strength = strength
-        self.forceIsPositive = forceIsPositive
+        self.forceIsPositive = True
 
         # 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
@@ -17,6 +17,9 @@ def __init__(self, strength:float=1.0, forceIsPositive:bool=True, radius:float =
         # Parameter to logistic funciton for how steeply the force transitions from "none" to "lots" about the radius
         self.fieldSteepness = 3.5
 
+    def setForceInverted(self, isInverted)->None:
+        self.forceIsPositive = not isInverted
+
     def getForceAtPosition(self, position:Translation2d)->Force:
         return Force()
 
@@ -36,9 +39,9 @@ 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, radius:float = 0.3):
+    def __init__(self, location:Translation2d, strength:float=1.0, radius:float = 0.3):
         self.location = location
-        super().__init__(strength, forceIsPositive,radius)
+        super().__init__(strength,radius)
 
     def getForceAtPosition(self, position: Translation2d) -> Force:
         deltaX =  self.location.x - position.x
@@ -48,8 +51,10 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
         unitY = deltaY/dist
         forceMag = self._distToForceMag(dist)
         return Force(-1.0*unitX*forceMag, -1.0*unitY*forceMag)
+
     def getDist(self, position:Translation2d)->float:
         return (position - self.location).norm()
+
     def getTelemTrans(self) -> list[Translation2d]:
         return [self.location]
 
@@ -58,9 +63,9 @@ 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, radius:float = 0.5):
+    def __init__(self, y:float, strength:float=1.0, radius:float = 0.5):
         self.y=y
-        super().__init__(strength, forceIsPositive,radius)
+        super().__init__(strength,radius)
 
 
     def getForceAtPosition(self, position: Translation2d) -> Force:
@@ -73,12 +78,84 @@ def getTelemTrans(self) -> list[Translation2d]:
         return []
 
 class VerticalObstacle(Obstacle):
-    def __init__(self, x:float, strength:float=1.0, forceIsPositive:bool=True, radius:float = 0.5):
+    def __init__(self, x:float, strength:float=1.0, radius:float = 0.5):
         self.x=x
-        super().__init__(strength, forceIsPositive,radius)
+        super().__init__(strength,radius)
 
     def getForceAtPosition(self, position: Translation2d) -> Force:
         return Force(self._distToForceMag(self.x - position.X()), 0)
 
     def getDist(self, position: Translation2d) -> float:
-        return abs(position.x - self.x)
+        return abs(position.x - self.x)
+
+# Describes a field force that exists along a line segment with a start and end point
+class LinearObstacle(Obstacle):
+    def __init__(self, start:Translation2d, end:Translation2d, strength:float=0.75, radius:float = 0.4):
+        self.start = start
+        self.end = end
+        super().__init__(strength,radius)
+
+    def _shortestTransToSegment(self, point: Translation2d) -> Translation2d:
+        # Vector from start to end of the segment
+        segment_vector_x = self.end.X() - self.start.X()
+        segment_vector_y = self.end.Y() - self.start.Y()
+
+        # Segment length squared (to avoid a square root operation)
+        segment_length_squared = segment_vector_x ** 2 + segment_vector_y ** 2
+
+        # Vector from start of the segment to the point
+        start_to_point_x = point.X() - self.start.X()
+        start_to_point_y = point.Y() - self.start.Y()
+
+        # Project the point onto the line (infinite line, not the segment)
+        t = (start_to_point_x * segment_vector_x + start_to_point_y * segment_vector_y) / segment_length_squared
+
+        # Clamp t to the range [0, 1] to restrict it to the segment
+        t = max(0, min(1, t))
+
+        # Calculate the closest point on the segment
+        closest_point_x = self.start.X() + t * segment_vector_x
+        closest_point_y = self.start.Y() + t * segment_vector_y
+
+        # Return the shortest vector from the point to the closest point on the segment
+        return Translation2d(closest_point_x - point.X(), closest_point_y - point.Y())
+
+    def getDist(self, position: Translation2d) -> float:
+        return self._shortestTransToSegment(position).norm()
+
+# Field formation that pushes the robot toward and along a line between start/end
+class Lane(LinearObstacle):
+
+    def getForceAtPosition(self, position: Translation2d) -> Force:
+        toSeg = self._shortestTransToSegment(position)
+        toSegUnit = toSeg/toSeg.norm()
+
+        alongSeg = (self.end - self.start)
+        alongSegUnit = alongSeg/alongSeg.norm()
+
+        forceDir = alongSegUnit * 0.75 + toSegUnit * 0.25
+        forceDirUnit = forceDir/forceDir.norm()
+        unitX = forceDirUnit.X()
+        unitY = forceDirUnit.Y()
+
+        dist = toSeg.norm()
+        forceMag = self._distToForceMag(dist)
+
+        return Force(unitX*forceMag, unitY*forceMag)
+
+# Field formation that pushes the robot uniformly away from the line
+class Wall(LinearObstacle):
+
+    def getForceAtPosition(self, position: Translation2d) -> Force:
+        toSeg = self._shortestTransToSegment(position)
+        toSegUnit = toSeg/toSeg.norm()
+
+        unitX = toSegUnit.X() * -1.0
+        unitY = toSegUnit.Y() * -1.0
+
+        dist = toSeg.norm()
+        forceMag = self._distToForceMag(dist)
+
+        return Force(unitX*forceMag, unitY*forceMag)
+
+

navigation/repulsorFieldPlanner.py

@@ -1,11 +1,11 @@
 from dataclasses import dataclass
 import math
-from wpimath.geometry import Pose2d, Translation2d
+from wpimath.geometry import Pose2d, Translation2d, Rotation2d
 from navigation.navConstants import FIELD_X_M, FIELD_Y_M
 
 from drivetrain.drivetrainCommand import DrivetrainCommand
 from navigation.navForce import Force
-from navigation.obstacles import HorizontalObstacle, Obstacle, PointObstacle, VerticalObstacle
+from navigation.obstacles import HorizontalObstacle, Obstacle, VerticalObstacle, Wall, Lane
 from utils.mapLookup2d import MapLookup2D
 from utils.signalLogging import log
 
@@ -14,23 +14,34 @@
 # Too small and the robot will get stuck on obstacles ("local minima")
 GOAL_STRENGTH = 0.04
 
+GOAL_PICKUP = Pose2d.fromFeet(50,8,Rotation2d.fromDegrees(0.0))
+GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(180.0))
+
 # The Fixed obstacles are everything fixed on the field, plus the walls
 FIELD_OBSTACLES = [
-    PointObstacle(location=Translation2d(5.56, 2.74)),
-    PointObstacle(location=Translation2d(3.45, 4.07)),
-    PointObstacle(location=Translation2d(5.56, 5.35)),
-    PointObstacle(location=Translation2d(11.0, 2.74)),
-    PointObstacle(location=Translation2d(13.27, 4.07)),
-    PointObstacle(location=Translation2d(11.0, 5.35)),
+    Wall(Translation2d(FIELD_X_M*0.0, FIELD_Y_M*1.0), Translation2d(FIELD_X_M*0.25, FIELD_Y_M*0.5)),
+    Wall(Translation2d(FIELD_X_M*0.5, FIELD_Y_M*0.5), Translation2d(FIELD_X_M*1.0, FIELD_Y_M*0.0)),
+]
+
+INVERTING_LANES = [
+    Lane(Translation2d(6, 3), Translation2d(7,5)),
 ]
 
-WALLS = [
-   HorizontalObstacle(y=0.0, forceIsPositive=True),
-   HorizontalObstacle(y=FIELD_Y_M, forceIsPositive=False),
-   VerticalObstacle(x=0.0, forceIsPositive=True),
-   VerticalObstacle(x=FIELD_X_M, forceIsPositive=False)
+NONIVERTING_LANES = [
+    Lane(Translation2d(12, 1), Translation2d(6,3)),
+    Lane(Translation2d(2.5, 7), Translation2d(5,5)),
 ]
 
+B_WALL = HorizontalObstacle(y=0.0)
+T_WALL = HorizontalObstacle(y=FIELD_Y_M)
+T_WALL.setForceInverted(True)
+
+L_WALL = VerticalObstacle(x=0.0)
+R_WALL = VerticalObstacle(x=FIELD_X_M)
+R_WALL.setForceInverted(True)
+
+OUTTER_WALLS = [B_WALL, T_WALL, L_WALL, R_WALL]
+
 # This map controls how the commanded velocity slows down as we approach the goal
 GOAL_MARGIN_M = 0.05
 SLOW_DOWN_DISTANCE_M = 1.5
@@ -49,14 +60,20 @@ class RepulsorFieldPlanner:
     def __init__(self):
         self.fixedObstacles:list[Obstacle] = []
         self.fixedObstacles.extend(FIELD_OBSTACLES)
-        self.fixedObstacles.extend(WALLS)
+        self.fixedObstacles.extend(INVERTING_LANES)
+        self.fixedObstacles.extend(NONIVERTING_LANES)
+        self.fixedObstacles.extend(OUTTER_WALLS)
         self.transientObstcales:list[Obstacle] = []
         self.distToGo:float = 0.0
         self.goal:Pose2d|None = None
 
     def setGoal(self, goal:Pose2d|None):
         self.goal = goal
 
+        # Reverse lane direction for going toward speaker
+        for lane in INVERTING_LANES:
+            lane.setForceInverted(goal == GOAL_SPEAKER)
+
     def add_obstcale_observaton(self, obs:Obstacle):
         self.transientObstcales.append(obs)
 

vectorPlotter.py

@@ -2,9 +2,8 @@
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-from drivetrain.controlStrategies.autoDrive import GOAL_PICKUP
-from navigation.repulsorFieldPlanner import RepulsorFieldPlanner
-from wpimath.geometry import Pose2d, Translation2d, Rotation2d
+from navigation.repulsorFieldPlanner import RepulsorFieldPlanner, GOAL_PICKUP, GOAL_SPEAKER
+from wpimath.geometry import Translation2d
 import matplotlib.pyplot as plt
 import numpy as np
 from navigation.navConstants import *