Merge remote-tracking branch 'origin/chris_linear_obstacles' into mai…

…n_nav

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,4 +1,4 @@
-from wpimath.geometry import Pose2d, Translation2d
+from wpimath.geometry import Pose2d, Translation2d, Rotation2d
 
 from utils.mapLookup2d import MapLookup2D
 from utils.signalLogging import log
@@ -22,7 +22,7 @@
 TRANSIENT_OBS_DECAY_PER_LOOP = 0.1
 
 # Obstacles come in two main flavors: Fixed and Transient.
-# Transient obstacles decay and disaapear over time. They are things like other robots, or maybe gamepieces we don't want to drive through.
+# Transient obstacles decay and disappear over time. They are things like other robots, or maybe gamepieces we don't want to drive through.
 # Fixed obstacles are things like field elements or walls with fixed, known positions. They are always present and do not decay over time.
 
 # Fixed Obstacles - Six posts in the middle of the field from 2024
@@ -36,12 +36,19 @@
 ]
 
 # Fixed Obstacles - Outer walls of the field 
-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)
-]
+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)
+
+WALLS = [B_WALL, T_WALL, L_WALL, R_WALL]
+
+# Happy Constants for the goal poses we may want to drive to
+GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0))
+GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(180.0))
 
 # Usually, the path planner assumes we traverse the path at a fixed velocity
 # However, near the goal, we'd like to slow down. This map defines how we ramp down

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 *