From b7e34814332a49f623ad0d457fca479bc5f56f5c Mon Sep 17 00:00:00 2001 From: Chris Date: Fri, 11 Oct 2024 14:31:16 -0500 Subject: [PATCH] comments, renames, cleanup --- drivetrain/controlStrategies/autoDrive.py | 7 +- .../{obstacles.py => forceGenerators.py} | 79 ++++++++++++++----- navigation/navConstants.py | 13 +++ navigation/navForce.py | 12 ++- navigation/obstacleDetector.py | 10 ++- navigation/repulsorFieldPlanner.py | 15 ++-- robot.py | 4 +- vectorPlotter.py | 3 +- wrappers/wrapperedObstaclePhotonCamera.py | 26 ++++-- 9 files changed, 121 insertions(+), 48 deletions(-) rename navigation/{obstacles.py => forceGenerators.py} (63%) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 0dc0668..0ddc528 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -5,7 +5,8 @@ from navigation.obstacleDetector import ObstacleDetector from utils.signalLogging import log from utils.singleton import Singleton -from navigation.repulsorFieldPlanner import GOAL_PICKUP, GOAL_SPEAKER, RepulsorFieldPlanner +from navigation.repulsorFieldPlanner import RepulsorFieldPlanner +from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED from utils.allianceTransformUtils import transform @@ -47,9 +48,9 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand retCmd = cmdIn # default - no auto driving for obs in self._obsDet.getObstacles(curPose): - self.rfp.add_obstcale_observaton(obs) + self.rfp.addObstacleObservation(obs) - self.rfp._decay_observations() + self.rfp._decayObservations() # Handle command changes if(self._toPickup): diff --git a/navigation/obstacles.py b/navigation/forceGenerators.py similarity index 63% rename from navigation/obstacles.py rename to navigation/forceGenerators.py index 578b674..38a03a7 100644 --- a/navigation/obstacles.py +++ b/navigation/forceGenerators.py @@ -1,10 +1,13 @@ import math from wpimath.geometry import Translation2d -from navigation.navForce import Force, _logistic_func +from navigation.navForce import Force, logisticFunc -# Generic and specifc types of obstacles that repel a robot -class Obstacle: +class ForceGenerator: + """ + Generic, common class for all objects that generate forces for pathplanning on the field. + Usually you should be using a specific type of force object, not this generic one + """ def __init__(self, strength:float=1.0, radius:float = 0.25): self.strength = strength self.forceIsPositive = True @@ -18,27 +21,47 @@ def __init__(self, strength:float=1.0, radius:float = 0.25): self.fieldSteepness = 3.5 def setForceInverted(self, isInverted)->None: + """ + Invert the force direction. + """ self.forceIsPositive = not isInverted def getForceAtPosition(self, position:Translation2d)->Force: + """ + Abstract implementation that assumes the object has no force. + Specific object types should override this + """ return Force() def _distToForceMag(self, dist:float)->float: + """ + Common method for converting a distance from the force object into a force strength + We're using the logistic function here, which (while not physically plaussable) has some + nice properties about getting bigger near the obstacle, while further objects do not have much + (or any) influence on the robot. + """ dist = abs(dist) - #Sigmoid shape - forceMag = _logistic_func(-1.0 * dist, self.strength, self.fieldSteepness, -self.radius) + forceMag = logisticFunc(-1.0 * dist, self.strength, self.fieldSteepness, -self.radius) if(not self.forceIsPositive): forceMag *= -1.0 return forceMag def getDist(self, position:Translation2d)->float: + """ + Returns the distance (in meters) between the object and a given position + """ return float('inf') def getTelemTrans(self)->list[Translation2d]: + """ + return all x/y positions associated with this field force generating object + """ return [] -# 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): +class PointObstacle(ForceGenerator): + """ + 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 + """ def __init__(self, location:Translation2d, strength:float=1.0, radius:float = 0.3): self.location = location super().__init__(strength,radius) @@ -59,10 +82,13 @@ def getTelemTrans(self) -> list[Translation2d]: return [self.location] -# Linear obstacles are infinite lines at a specific coordinate -# They push the robot away along a perpendicular direction -# with the specific direction determined by forceIsPositive -class HorizontalObstacle(Obstacle): +class HorizontalObstacle(ForceGenerator): + """ + Linear obstacles are infinite lines at a specific coordinate + They push the robot away along a perpendicular direction + with the specific direction determined by the force inversion. + The Horizontal Obstacle exists parallel to the X axis, at a specific Y coordinate, and pushes parallel to the Y axis. + """ def __init__(self, y:float, strength:float=1.0, radius:float = 0.5): self.y=y super().__init__(strength,radius) @@ -77,7 +103,13 @@ def getDist(self, position: Translation2d) -> float: def getTelemTrans(self) -> list[Translation2d]: return [] -class VerticalObstacle(Obstacle): +class VerticalObstacle(ForceGenerator): + """ + Linear obstacles are infinite lines at a specific coordinate + They push the robot away along a perpendicular direction + with the specific direction determined by the force inversion. + The Vertical Obstacle exists parallel to the Y axis, at a specific X coordinate, and pushes parallel to the X axis. + """ def __init__(self, x:float, strength:float=1.0, radius:float = 0.5): self.x=x super().__init__(strength,radius) @@ -88,8 +120,10 @@ def getForceAtPosition(self, position: Translation2d) -> Force: def getDist(self, position: Translation2d) -> float: 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): +class LinearForceGenerator(ForceGenerator): + """ + A linear force generator creates forces based on the relative position of the robot to a specific line segment + """ def __init__(self, start:Translation2d, end:Translation2d, strength:float=0.75, radius:float = 0.4): self.start = start self.end = end @@ -123,9 +157,11 @@ def _shortestTransToSegment(self, point: Translation2d) -> Translation2d: 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): - +class Lane(LinearForceGenerator): + """ + A lane is an attractor - it creates a field force that pulls robots toward and along it, "ejecting" them out the far end. + It helps as a "hint" when the robot needs to navigate in a specific way around obstacles, which is not necessarily straight toward the goal. + """ def getForceAtPosition(self, position: Translation2d) -> Force: toSeg = self._shortestTransToSegment(position) toSegUnit = toSeg/toSeg.norm() @@ -143,9 +179,12 @@ def getForceAtPosition(self, position: Translation2d) -> Force: return Force(unitX*forceMag, unitY*forceMag) -# Field formation that pushes the robot uniformly away from the line -class Wall(LinearObstacle): +class Wall(LinearForceGenerator): + """ + Walls obstacles are finite lines between specific coordinates + They push the robot away along a perpendicular direction. + """ def getForceAtPosition(self, position: Translation2d) -> Force: toSeg = self._shortestTransToSegment(position) toSegUnit = toSeg/toSeg.norm() diff --git a/navigation/navConstants.py b/navigation/navConstants.py index 928992b..1f29c1f 100644 --- a/navigation/navConstants.py +++ b/navigation/navConstants.py @@ -1,3 +1,16 @@ +from wpimath.geometry import Pose2d, Rotation2d +""" +Constants related to navigation +""" + +""" +"Official" field dimensions +""" FIELD_X_M = 16.54 FIELD_Y_M = 8.21 + + +# 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)) diff --git a/navigation/navForce.py b/navigation/navForce.py index 73965c6..dcbec1e 100644 --- a/navigation/navForce.py +++ b/navigation/navForce.py @@ -5,13 +5,19 @@ import math -def _logistic_func(x, L, k, x0): - """Logistic function.""" +def logisticFunc(x, L, k, x0): + """ + Implements the Logistic function. + https://en.wikipedia.org/wiki/Logistic_function + This function is nice due to being exactly 0 at one side, 1.0 at the other, and a smooth transition between the two + """ return L / (1 + math.exp(-k * (x - x0))) -# Utility class for representing a force vector @dataclass class Force: + """ + Simple class to represent a force in a 2d plane + """ x:float=0 y:float=0 def __add__(self, other): diff --git a/navigation/obstacleDetector.py b/navigation/obstacleDetector.py index 87f0a81..ea69270 100644 --- a/navigation/obstacleDetector.py +++ b/navigation/obstacleDetector.py @@ -2,13 +2,19 @@ from wrappers.wrapperedObstaclePhotonCamera import WrapperedObstaclePhotonCamera from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM from wpimath.geometry import Pose2d -from navigation.obstacles import Obstacle, PointObstacle +from navigation.forceGenerators import ForceGenerator, PointObstacle class ObstacleDetector(): + """ + Class to use a PhotonVision-equipped coprocoessor and camera to deduce the location of obstacles on the field and report them + """ def __init__(self): self.frontCam = WrapperedObstaclePhotonCamera("FRONT_CAM", ROBOT_TO_FRONT_CAM) - def getObstacles(self, curPose:Pose2d) -> list['Obstacle']: + def getObstacles(self, curPose:Pose2d) -> list['ForceGenerator']: + """ + Returns the currently observed obstacles + """ retList = [] self.frontCam.update() diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index b007a84..2475521 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -6,7 +6,7 @@ from drivetrain.drivetrainCommand import DrivetrainCommand from navigation.navForce import Force -from navigation.obstacles import HorizontalObstacle, Obstacle, PointObstacle, VerticalObstacle +from navigation.forceGenerators import HorizontalObstacle, ForceGenerator, PointObstacle, VerticalObstacle from navigation.navConstants import FIELD_X_M, FIELD_Y_M # Relative strength of how hard the goal pulls the robot toward it @@ -19,7 +19,7 @@ # Rate at which transient obstacles decay in strength # Bigger numbers here make transient obstacles disappear faster -TRANSIENT_OBS_DECAY_PER_LOOP = 0.1 +TRANSIENT_OBS_DECAY_PER_LOOP = 0.01 # Obstacles come in two main flavors: Fixed and Transient. # Transient obstacles decay and disappear over time. They are things like other robots, or maybe gamepieces we don't want to drive through. @@ -46,9 +46,6 @@ 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 @@ -89,10 +86,10 @@ class RepulsorFieldPlanner: Finally, the planner will recommend a step of fixed size, in the direction the net force pushes on. """ def __init__(self): - self.fixedObstacles:list[Obstacle] = [] + self.fixedObstacles:list[ForceGenerator] = [] self.fixedObstacles.extend(FIELD_OBSTACLES_2024) self.fixedObstacles.extend(WALLS) - self.transientObstcales:list[Obstacle] = [] + self.transientObstcales:list[ForceGenerator] = [] self.distToGo:float = 0.0 self.goal:Pose2d|None = None self.lookaheadTraj:list[Pose2d] = [] @@ -105,7 +102,7 @@ def setGoal(self, goal:Pose2d|None): """ self.goal = goal - def add_obstcale_observaton(self, obs:Obstacle): + def addObstacleObservation(self, obs:ForceGenerator): """ Call this to report a new, transient obstacle observed on the field. """ @@ -127,7 +124,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force: # no goal, no force return Force() - def _decay_observations(self): + def _decayObservations(self): """ Transient obstacles decay in strength over time. This function decays the obstacle's strength, and removes obstacles which have zero strength. diff --git a/robot.py b/robot.py index 0d2b54b..caffc5f 100644 --- a/robot.py +++ b/robot.py @@ -7,7 +7,7 @@ from drivetrain.drivetrainControl import DrivetrainControl from humanInterface.driverInterface import DriverInterface from humanInterface.ledControl import LEDControl -from navigation.obstacles import PointObstacle +from navigation.forceGenerators import PointObstacle from utils.segmentTimeTracker import SegmentTimeTracker from utils.signalLogging import SignalWrangler from utils.calibration import CalibrationWrangler @@ -127,7 +127,7 @@ def teleopPeriodic(self): ] for tf in tfs: obs = PointObstacle(location=(ct+tf), strength=0.7) - self.autodrive.rfp.add_obstcale_observaton(obs) + self.autodrive.rfp.addObstacleObservation(obs) self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup()) diff --git a/vectorPlotter.py b/vectorPlotter.py index f36223c..5738ecf 100644 --- a/vectorPlotter.py +++ b/vectorPlotter.py @@ -2,7 +2,8 @@ import math import matplotlib.pyplot as plt import numpy as np -from navigation.repulsorFieldPlanner import RepulsorFieldPlanner, GOAL_PICKUP, GOAL_SPEAKER +from navigation.repulsorFieldPlanner import RepulsorFieldPlanner +from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER from wpimath.geometry import Translation2d import matplotlib.pyplot as plt import numpy as np diff --git a/wrappers/wrapperedObstaclePhotonCamera.py b/wrappers/wrapperedObstaclePhotonCamera.py index e8c7ed5..5406e73 100644 --- a/wrappers/wrapperedObstaclePhotonCamera.py +++ b/wrappers/wrapperedObstaclePhotonCamera.py @@ -15,23 +15,33 @@ def __init__(self, time:float, estFieldPose:Pose2d, trustworthiness=1.0): MIN_AREA=10.0 #idk tune this if we are reacting to small targets -def calculateDistanceToTargetMeters( +def _calculateDistanceToTargetMeters( cameraHeightMeters:float, targetHeightMeters:float, cameraPitchRadians:float, targetPitchRadians:float): + """ + Utility method to calculate the distance to the target from 2d targeting information + using right triangles, the assumption obstacles are on the ground, and the geometry + of where the camera is mounted on the robot. + """ return (targetHeightMeters - cameraHeightMeters) / math.tan(cameraPitchRadians + targetPitchRadians) -def estimateCameraToTargetTranslation(targetDistanceMeters:float, yaw:Rotation2d): +def _estimateCameraToTargetTranslation(targetDistanceMeters:float, yaw:Rotation2d): + """ + Utility to generate a Translation2d based on + """ return Translation2d( yaw.cos() * targetDistanceMeters, yaw.sin() * targetDistanceMeters) -# Wrappers photonvision to: -# 1 - resolve issues with target ambiguity (two possible poses for each observation) -# 2 - Convert pose estimates to the field -# 3 - Handle recording latency of when the image was actually seen class WrapperedObstaclePhotonCamera: + """ + Wrappers photonvision to: + 1 - resolve issues with target ambiguity (two possible poses for each observation) + 2 - Convert pose estimates to the field + 3 - Handle recording latency of when the image was actually seen + """ def __init__(self, camName, robotToCam:Transform3d): setVersionCheckEnabled(False) @@ -77,13 +87,13 @@ def update(self): # Use algorithm described at # https://docs.limelightvision.io/docs/docs-limelight/tutorials/tutorial-estimating-distance # to estimate distance from the camera to the target. - dist = calculateDistanceToTargetMeters( + dist = _calculateDistanceToTargetMeters( self.robotToCam.translation().Z(), 0.05, # Assume the average bumper starts 5cm off the ground self.robotToCam.rotation().Y(), # Pitch is rotation about the Y axis degreesToRadians(target.getPitch()) ) - camToObstacle = estimateCameraToTargetTranslation( + camToObstacle = _estimateCameraToTargetTranslation( dist, Rotation2d.fromDegrees(target.getYaw()) )