refactored telementry, added rotation support

drivetrain/controlStrategies/autoDrive.py

@@ -9,12 +9,9 @@
 
 
 GOAL_PICKUP = Pose2d.fromFeet(40,5,Rotation2d.fromDegrees(0.0))
-GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(0.0))
+GOAL_SPEAKER = Pose2d.fromFeet(3,20,Rotation2d.fromDegrees(180.0))
 SPEED_SCALAR = 0.75
 
-TELEM_LOOKAHEAD_DIST_M = 3.0
-TELEM_LOOKAHEAD_STEPS = 7
-
 class AutoDrive(metaclass=Singleton):
     def __init__(self):
         self._toSpeaker = False
@@ -32,21 +29,8 @@ def getTrajectory(self) -> Trajectory|None:
         return None # TODO
 
     def updateTelemetry(self) -> None:        
-        self._telemTraj = []
-        stepsize = TELEM_LOOKAHEAD_DIST_M/TELEM_LOOKAHEAD_STEPS
-        if(self._rfp.goal is not None):
-            cp = self._curPose
-            for _ in range(0,TELEM_LOOKAHEAD_STEPS):
-                self._telemTraj.append(cp)
-                tmp = self._rfp.getCmd(cp, stepsize)
-                if(tmp.desPose is not None):
-                    cp = tmp.desPose
-                else:
-                    break
+        self._telemTraj = self._rfp.updateTelemetry(self._curPose)
 
-            self._telemTraj.append(self._rfp.goal)
-
-
     def getWaypoints(self) -> list[Pose2d]:
         return self._telemTraj
 
@@ -71,9 +55,11 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand
         # If being asked to auto-align, use the command from the dynamic path planner
         if(self._toPickup or self._toSpeaker):
             olCmd = self._rfp.getCmd(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR)
+            log("AutoDrive FwdRev Cmd", olCmd.velX, "mps")
+            log("AutoDrive Strafe Cmd", olCmd.velY, "mps")
+            log("AutoDrive Rot Cmd", olCmd.velT, "radpers")
             if( olCmd.desPose is not None):
                 retCmd = self._trajCtrl.update2(olCmd.velX, olCmd.velY, olCmd.velT, olCmd.desPose, curPose)
 
-        log("AutoDrive_active", self._rfp.goal is not None)
 
         return retCmd

navigation/repulsorFieldPlanner.py

@@ -1,15 +1,19 @@
 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 utils.mapLookup2d import MapLookup2D
+from utils.signalLogging import log
 
+
+# Utility function for defining the shape of the force an obstacle exerts on the robot
 def _logistic_func(x, L, k, x0):
     """Logistic function."""
     return L / (1 + math.exp(-k * (x - x0)))
 
+# Utility class for representing a force vector
 @dataclass
 class Force:
     x:float=0
@@ -23,6 +27,7 @@ def unitY(self) -> float:
     def mag(self):
         return math.sqrt(self.x**2 + self.y**2)
 
+# Generic and specifc types of obstacles that repel a robot
 class Obstacle:
     def __init__(self, strength:float=1.0, forceIsPositive:bool=True):
         self.strength = strength
@@ -43,7 +48,10 @@ def getDist(self, position:Translation2d)->float:
         return float('inf')
     def getTelemTrans(self)->list[Translation2d]:
         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):
     def __init__(self, location:Translation2d, strength:float=1.0, forceIsPositive:bool=True):
         self.strength = strength
@@ -65,6 +73,10 @@ def getDist(self, position:Translation2d)->float:
     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):
     def __init__(self, y:float, strength:float=1.0, forceIsPositive:bool=True):
         self.strength = strength
@@ -95,8 +107,12 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
     def getDist(self, position: Translation2d) -> float:
         return abs(position.x - self.x)
 
+# Relative strength of how hard the goal pulls the robot toward it
+# Too big and the robot will be pulled through obstacles
+# Too small and the robot will get stuck on obstacles ("local minima")
 GOAL_STRENGTH = 0.04
 
+# 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)),
@@ -113,23 +129,28 @@ def getDist(self, position: Translation2d) -> float:
    VerticalObstacle(x=FIELD_X_M, forceIsPositive=False)
 ]
 
+# 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
-
 GOAL_SLOW_DOWN_MAP = MapLookup2D([
     (9999.0, 1.0),
     (SLOW_DOWN_DISTANCE_M, 1.0),
     (GOAL_MARGIN_M, 0.0),
     (0.0, 0.0)
 ])
 
+# These define how far in advance we attempt to plan for telemetry purposes
+TELEM_LOOKAHEAD_DIST_M = 3.0
+TELEM_LOOKAHEAD_STEPS = 7
 
 class RepulsorFieldPlanner:
     def __init__(self):
-       self.fixedObstacles:list[Obstacle] = []
-       self.fixedObstacles.extend(FIELD_OBSTACLES)
-       self.fixedObstacles.extend(WALLS)
-       self.transientObstcales:list[Obstacle] = []
+        self.fixedObstacles:list[Obstacle] = []
+        self.fixedObstacles.extend(FIELD_OBSTACLES)
+        self.fixedObstacles.extend(WALLS)
+        self.transientObstcales:list[Obstacle] = []
+        self.distToGo:float = 0.0
+        self.goal:Pose2d|None = None
 
     def setGoal(self, goal:Pose2d|None):
         self.goal = goal
@@ -138,7 +159,6 @@ def add_obstcale_observaton(self, pose:Pose2d):
         obstacle = PointObstacle(location=pose.translation(),strength=.7)
         self.transientObstcales.append(obstacle)
 
-
     def getGoalForce(self, curLocation:Translation2d) -> Force:
         if(self.goal is not None):
             displacement = self.goal.translation() - curLocation
@@ -166,6 +186,7 @@ def getObstacleTransList(self) -> list[Translation2d]:
 
         return retArr
 
+
     def atGoal(self, trans:Translation2d)->bool:
         if(self.goal is None):
             return True
@@ -183,40 +204,59 @@ def getForceAtTrans(self, trans:Translation2d)->Force:
         for obstacle in self.transientObstcales:
             repusliveForces.append(obstacle.getForceAtPosition(trans))
 
-        # Calcualte sum of forces
+        # calculate sum of forces
         netForce = goalForce
         for force in repusliveForces:
             netForce += force
 
         return netForce
+
 
     def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand:
         retVal = DrivetrainCommand() # Default, no command
+
         if(self.goal is not None):
             curTrans = curPose.translation()
+            self.distToGo = (curTrans - self.goal.translation()).norm()
 
             if(not self.atGoal(curTrans)):
+                # Only calculate a nonzero command if we have a goal and we're not near it.
 
-                err = curTrans - self.goal.translation()
-                nextTrans = curTrans
                 # Slow down when we're near the goal
-                slowFactor = GOAL_SLOW_DOWN_MAP.lookup(err.norm())
-
+                slowFactor = GOAL_SLOW_DOWN_MAP.lookup(self.distToGo)
 
                 netForce = self.getForceAtTrans(curPose.translation())
 
                 # Calculate a substep in the direction of the force
                 step = Translation2d(stepSize_m*netForce.unitX(), stepSize_m*netForce.unitY()) 
 
                 # Take that substep
-                nextTrans += step
-
+                nextTrans = curTrans + step
 
                 # Assemble velocity commands based on the step we took
                 retVal.velX = (nextTrans - curTrans).X()/0.02 * slowFactor
                 retVal.velY = (nextTrans - curTrans).Y()/0.02 * slowFactor
-                retVal.velT = 0.0 # TODO
-                retVal.desPose = Pose2d(nextTrans, curPose.rotation())
-
-
-        return retVal
+                retVal.velT = 0.0 # Let the closed-loop controller do the work.
+                retVal.desPose = Pose2d(nextTrans, self.goal.rotation())
+        else:
+            self.distToGo = 0.0
+
+        return retVal
+
+    def updateTelemetry(self, curPose:Pose2d) -> list[Pose2d]:        
+        telemTraj = []
+        stepsize = TELEM_LOOKAHEAD_DIST_M/TELEM_LOOKAHEAD_STEPS
+        if(self.goal is not None):
+            cp = curPose
+            for _ in range(0,TELEM_LOOKAHEAD_STEPS):
+                telemTraj.append(cp)
+                tmp = self.getCmd(cp, stepsize)
+                if(tmp.desPose is not None):
+                    cp = tmp.desPose
+                else:
+                    break
+
+        log("PotentialField Num Obstacles", len(self.fixedObstacles) + len(self.transientObstcales))
+        log("PotentialField Path Active", self.goal is not None)
+        log("PotentialField DistToGo", self.distToGo, "m")
+        return telemTraj

robot.py

@@ -62,6 +62,11 @@ def robotPeriodic(self):
 
         self.driveTrain.update()
 
+        self.autodrive.updateTelemetry()
+        self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory())
+        self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints())
+        self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles())
+
         self.stt.end()
 
     #########################################################
@@ -108,10 +113,6 @@ def teleopPeriodic(self):
             self.autodrive._rfp.add_obstcale_observaton(self.driveTrain.poseEst.getCurEstPose().transformBy(Transform2d(-3.0, 0.0, Rotation2d())))
 
         self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup())
-        self.autodrive.updateTelemetry()
-        self.driveTrain.poseEst.telemetry.setWPITrajectory(self.autodrive.getTrajectory())
-        self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints())
-        self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles())
 
         # No trajectory in Teleop
         Trajectory().setCmd(None)