diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 2a9a9c4..0dc0668 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/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): diff --git a/navigation/obstacles.py b/navigation/obstacles.py index ceb9c5e..578b674 100644 --- a/navigation/obstacles.py +++ b/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) \ No newline at end of file + 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) + + diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index c91ba68..b007a84 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/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 diff --git a/vectorPlotter.py b/vectorPlotter.py index 823fea6..f36223c 100644 --- a/vectorPlotter.py +++ b/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 *