Cleanups to visualization, improved closed-area walls (push in one di…

…rection, not outward), changed strategy for stuck detection to look at history.

drivetrain/controlStrategies/autoDrive.py

@@ -30,12 +30,9 @@ def __init__(self):
         self._plannerDur:float = 0.0
         self.autoSpeakerPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto targeting the speaker last refresh.
         self.autoPickupPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto targeting the speaker last refresh.
-        self.stuckTracker = 0 
-        self.prevPose = Pose2d()
 
         addLog("AutoDrive Proc Time", lambda:(self._plannerDur * 1000.0), "ms")
 
-
     def setRequest(self, toSpeaker, toPickup) -> None:
         self._toSpeaker = toSpeaker
         self._toPickup = toPickup
@@ -45,7 +42,6 @@ def setRequest(self, toSpeaker, toPickup) -> None:
         self.autoPickupPrevEnabled = self._toPickup
         self.autoSpeakerPrevEnabled = self._toSpeaker
 
-
     def updateTelemetry(self) -> None:        
         self._telemTraj = self.rfp.getLookaheadTraj()
 
@@ -80,40 +76,34 @@ 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):
-            if self.stuckTracker < 10: #Only run if the robot isn't stuck
-                #This checks how much we moved, and if we moved less than one cm it increments the counter by one.
-                if math.sqrt(((curPose.X() - self.prevPose.X()) ** 2) + ((curPose.Y() - self.prevPose.Y()) ** 2)) < .01:
-                    self.stuckTracker += 1
-                else:
-                    if self.stuckTracker > 0:
-                        self.stuckTracker -= 1
-
+            # Open Loop - Calculate the new desired pose and velocity to get there from the 
+            # repulsor field path planner
+            if(self._prevCmd is None):
+                initCmd = DrivetrainCommand(0,0,0,curPose) # TODO - init this from current odometry vel
+                self._olCmd = self.rfp.update(initCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
+            else:
+                self._olCmd = self.rfp.update(self._prevCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
+
+            # Add closed loop - use the trajectory controller to add in additional 
+            # velocity if we're currently far away from the desired pose
+            retCmd = self._trajCtrl.update2(self._olCmd.velX, 
+                                            self._olCmd.velY, 
+                                            self._olCmd.velT, 
+                                            self._olCmd.desPose, curPose)    
 
-                # Open Loop - Calculate the new desired pose and velocity to get there from the 
-                # repulsor field path planner
-                if(self._prevCmd is None):
-                    initCmd = DrivetrainCommand(0,0,0,curPose) # TODO - init this from current odometry vel
-                    self._olCmd = self.rfp.update(initCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
-                else:
-                    self._olCmd = self.rfp.update(self._prevCmd, MAX_PATHPLAN_SPEED_MPS*0.02)
-
-                # Add closed loop - use the trajectory controller to add in additional 
-                # velocity if we're currently far away from the desired pose
-                retCmd = self._trajCtrl.update2(self._olCmd.velX, 
-                                                self._olCmd.velY, 
-                                                self._olCmd.velT, 
-                                                self._olCmd.desPose, curPose)    
-                self._prevCmd = retCmd
+            # TODO - test this works the way we want.
+            if(self.rfp.isStuck()):
+                # While stuck, don't translate, but allow the desired pose to keep going
+                retCmd.velX = 0
+                retCmd.velY = 0
+
+            self._prevCmd = retCmd
         else:
             self._prevCmd = None
 
         self._plannerDur = Timer.getFPGATimestamp() - startTime
 
         #Set our curPos as the new old pose
         self.prevPose = curPose
-        #assume that we are either stuck or done if the counter reaches above 10. (sometimes it will get to like 4 when we are accelerating or taking a sharp turn)
-        if self.stuckTracker >= 10:
-            retCmd = cmdIn #set the returned cmd to the cmd that we were originally given.
-            self._prevCmd = None
 
         return retCmd

navigation/forceGenerators.py

@@ -1,5 +1,5 @@
 import math
-from wpimath.geometry import Translation2d
+from wpimath.geometry import Translation2d, Rotation2d
 from utils.constants import FIELD_X_M, FIELD_Y_M
 from navigation.navForce import Force, logisticFunc
 
@@ -205,8 +205,10 @@ def getForceAtPosition(self, position: Translation2d) -> Force:
         dist = max(toSeg.norm(), 1e-6)
         toSegUnit = toSeg/dist
 
-        unitX = toSegUnit.X() * -1.0
-        unitY = toSegUnit.Y() * -1.0
+        perpVec = (self.end - self.start).rotateBy(Rotation2d.fromDegrees(90.0))
+        perpVec /= perpVec.norm()
+        unitX = perpVec.X()
+        unitY = perpVec.Y()
 
         forceMag = self._distToForceMag(dist)
 

navigation/obstacleDetector.py

@@ -2,7 +2,7 @@
 from wrappers.wrapperedObstaclePhotonCamera import WrapperedObstaclePhotonCamera
 from drivetrain.drivetrainPhysical import ROBOT_TO_FRONT_CAM
 from wpimath.geometry import Pose2d, Pose3d
-from navigation.forceGenerators import ForceGenerator, PointObstacle
+from navigation.forceGenerators import PointObstacle
 
 class ObstacleDetector():
     """
@@ -11,7 +11,7 @@ class ObstacleDetector():
     def __init__(self):
         self.frontCam = WrapperedObstaclePhotonCamera("FRONT_CAM", ROBOT_TO_FRONT_CAM)
 
-    def getObstacles(self, curPose:Pose2d) -> list['ForceGenerator']:
+    def getObstacles(self, curPose:Pose2d) -> list['PointObstacle']:
         """
         Returns the currently observed obstacles
         """

navigation/repulsorFieldPlanner.py

@@ -21,7 +21,7 @@
 # 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
+GOAL_STRENGTH = 0.07
 
 # Maximum number of obstacles we will remember. Older obstacles are automatically removed from our memory
 MAX_OBSTACLES = 10
@@ -44,37 +44,58 @@
     PointObstacle(location=Translation2d(11.0, 5.35))
 ]
 
-# Lane Traffic pattern around center
-x1 = 3.0
-x2 = FIELD_X_M - x1
-y1 = 2.0
-y2 = FIELD_Y_M - y1
-
 # Adds asymmetry so robot doesn't get stuck on opposite side of spaceship
 FIELD_LANES_BLUE_2019 = [
-    Lane(Translation2d(x2 - 2.5, y2), Translation2d(x1 + 2.5, y2)),
-    Lane(Translation2d(x2, y2 - 1.5), Translation2d(x2, y1 + 1.5)),
+    Lane(Translation2d(FIELD_X_M - 3.0 - 2.5, FIELD_Y_M - 2.0), Translation2d(3.0 + 2.5, FIELD_Y_M - 2.0)),
+    Lane(Translation2d(FIELD_X_M - 3.0, FIELD_Y_M - 2.0 - 1.5), Translation2d(FIELD_X_M - 3.0, 2.0 + 1.5)),
 ]
 
+# Spaceship Corners
+SP_X1 = 5.6
+SP_X2 = 10.8
+SP_Y1 = 3.5
+SP_Y2 = 4.5
+
+# Endgame Platforms Common
+EG_Y1 = 2.5
+EG_Y2 = 5.7
+EG_WIDTH = 1.5
+
+# Red Endgame Platforms
+RE_X1 = FIELD_X_M - EG_WIDTH
+RE_X2 = FIELD_X_M
+RE_Y1 = EG_Y1
+RE_Y2 = EG_Y2
+
+# Blue Endgame Platforms
+BE_X1 = 0.0
+BE_X2 = EG_WIDTH
+BE_Y1 = EG_Y1
+BE_Y2 = EG_Y2
+
 FIELD_OBSTACLES_2019 = [
-    # Center spaceship
-    Wall(Translation2d(5.6, 3.5), Translation2d(10.8, 3.5)),
-    Wall(Translation2d(5.6, 3.5), Translation2d(5.6, 4.5)),
-    Wall(Translation2d(5.6, 4.5), Translation2d(10.8, 4.5)),
-    Wall(Translation2d(10.8, 3.5), Translation2d(10.8, 4.5)),
+
     # Rockets
-    PointObstacle(location=Translation2d(5.8, 0.0)),
-    PointObstacle(location=Translation2d(10.6, 0.0)),
-    PointObstacle(location=Translation2d(5.8, FIELD_Y_M)),
-    PointObstacle(location=Translation2d(10.6, FIELD_Y_M)),
-    # Blue endgame platform
-    Wall(Translation2d(0.0, 2.5), Translation2d(1.5, 2.5)),
-    Wall(Translation2d(0.0, 5.7), Translation2d(1.5, 5.7)),
-    Wall(Translation2d(1.5, 2.5), Translation2d(1.5, 5.7)),
-    # Red endgame platform
-    Wall(Translation2d(FIELD_X_M, 2.5), Translation2d(FIELD_X_M - 1.5, 2.5)),
-    Wall(Translation2d(FIELD_X_M, 5.7), Translation2d(FIELD_X_M - 1.5, 5.7)),
-    Wall(Translation2d(FIELD_X_M - 1.5, 2.5), Translation2d(FIELD_X_M - 1.5, 5.7)),
+    PointObstacle(location=Translation2d(5.8, 0.0), radius=0.7),
+    PointObstacle(location=Translation2d(10.6, 0.0), radius=0.7),
+    PointObstacle(location=Translation2d(5.8, FIELD_Y_M), radius=0.7),
+    PointObstacle(location=Translation2d(10.6, FIELD_Y_M), radius=0.7),
+
+    # Center spaceship, defined clockwise
+    Wall(Translation2d(SP_X1, SP_Y1), Translation2d(SP_X1, SP_Y2)),
+    Wall(Translation2d(SP_X1, SP_Y2), Translation2d(SP_X2, SP_Y2)),
+    Wall(Translation2d(SP_X2, SP_Y2), Translation2d(SP_X2, SP_Y1)),
+    Wall(Translation2d(SP_X2, SP_Y1), Translation2d(SP_X1, SP_Y1)),
+
+    # Blue endgame platform defined clockwise
+    Wall(Translation2d(BE_X1, BE_Y2), Translation2d(BE_X2, BE_Y2)),
+    Wall(Translation2d(BE_X2, BE_Y2), Translation2d(BE_X2, BE_Y1)),
+    Wall(Translation2d(BE_X2, BE_Y1), Translation2d(BE_X1, BE_Y1)),
+
+    # Red endgame platform defined clockwise
+    Wall(Translation2d(RE_X1, RE_Y1), Translation2d(RE_X1, RE_Y2)),
+    Wall(Translation2d(RE_X1, RE_Y2), Translation2d(RE_X2, RE_Y2)),
+    Wall(Translation2d(RE_X2, RE_Y1), Translation2d(RE_X1, RE_Y1)),
 ]
 
 # Fixed Obstacles - Outer walls of the field 
@@ -99,7 +120,7 @@
 # However, near the goal, we'd like to slow down. This map defines how we ramp down
 # the step-size toward zero as we get closer to the goal. Once we are close enough,
 # we stop taking steps and simply say the desired position is at the goal.
-GOAL_MARGIN_M = 0.2
+GOAL_MARGIN_M = 0.1
 SLOW_DOWN_DISTANCE_M = 1.5
 GOAL_SLOW_DOWN_MAP = MapLookup2D([
     (9999.0, 1.0),
@@ -158,6 +179,10 @@ def __init__(self):
 
         # Keep things slow right when the goal changes
         self.startSlowFactor = 0.0
+
+        # Stuck information
+        self.currentlyStuck = False
+        self.stuckPrevCmdBuffer:deque[Pose2d] = deque(maxlen=5)
 
         #addLog("PotentialField Num Obstacles", lambda: (len(self.fixedObstacles) + len(self.transientObstcales)))
         #addLog("PotentialField Path Active", lambda: (self.goal is not None))
@@ -285,14 +310,7 @@ def isStuck(self) -> bool:
         Based on current lookahead trajectory, see if we expect to make progress in the near future,
         or if we're stuck in ... basically ... the same spot. IE, at a local minima
         """
-        if(len(self.lookaheadTraj) > 3 and self.goal is not None):
-            start = self.lookaheadTraj[0]
-            end = self.lookaheadTraj[-1]
-            dist = (end-start).translation().norm()
-            distToGoal = (end - self.goal).translation().norm()
-            return dist < STUCK_DIST and distToGoal > LOOKAHEAD_DIST_M * 2
-        else:
-            return False
+        return self.currentlyStuck
 
     def atGoal(self, pose:Pose2d)->bool:
         """
@@ -332,19 +350,46 @@ def _getForceAtTrans(self, trans:Translation2d)->Force:
 
     def update(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand:
 
-        # Update the initial "don't start too fast" factor
-        self.startSlowFactor += 2.0 * Ts
-        self.startSlowFactor = min(self.startSlowFactor, 1.0)
-
         nextCmd = self._getCmd(curCmd, stepSize_m)
 
+        self._updateStuckStatus(nextCmd, stepSize_m)
+
         if(TimedRobot.isSimulation()):
             # Lookahead is for telemetry and debug only, and is very
             # time expensive. We'll do it in simulation, but not on the
             # real robot.
             self._doLookahead(curCmd)
 
         return nextCmd
+
+    def _updateStuckStatus(self, curCmd:DrivetrainCommand, stepSize_m:float) -> bool:
+
+        # Add our new command to the list
+        self.stuckPrevCmdBuffer.append(curCmd.desPose)
+
+        if(self.distToGo < 3.0):
+            # Rough threshold - if we're within a few meters of the goal,
+            # just keep going. Never declare stuck.
+            self.currentlyStuck = False
+        else:
+            # Find the min and max of the X and Y values in our buffer
+            min_x = self.stuckPrevCmdBuffer[0].X()
+            min_y = self.stuckPrevCmdBuffer[0].Y()
+            max_x = self.stuckPrevCmdBuffer[0].X()
+            max_y = self.stuckPrevCmdBuffer[0].Y()
+            for pose in self.stuckPrevCmdBuffer:
+                min_x = min(min_x, pose.X())
+                max_x = max(max_x, pose.X())
+                min_y = min(min_y, pose.Y())
+                max_y = max(max_y, pose.Y())
+            span_x = max_x - min_x
+            span_y = max_y - min_y
+
+            if(span_x < stepSize_m * 2.0 and span_y < stepSize_m * 2.0):
+                # All poses are within a two-step square, we aren't going anywhere
+                self.currentlyStuck = True
+            else:
+                self.currentlyStuck = False
 
     def _getCmd(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainCommand:
         """
@@ -353,6 +398,10 @@ def _getCmd(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainComma
         retVal = DrivetrainCommand() # Default, no command
         curPose = curCmd.desPose
 
+        # Update the initial "don't start too fast" factor
+        self.startSlowFactor += 2.0 * Ts
+        self.startSlowFactor = min(self.startSlowFactor, 1.0)
+
         if(self.goal is not None):
             curTrans = curPose.translation()
             self.distToGo = (curTrans - self.goal.translation()).norm()