diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 8172f40..06e4f69 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/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,30 +76,28 @@ 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 @@ -111,9 +105,5 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand #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 \ No newline at end of file diff --git a/navigation/forceGenerators.py b/navigation/forceGenerators.py index 3b5cf7c..bd0ec40 100644 --- a/navigation/forceGenerators.py +++ b/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) diff --git a/navigation/obstacleDetector.py b/navigation/obstacleDetector.py index 26e0c34..6e9e568 100644 --- a/navigation/obstacleDetector.py +++ b/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 """ diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index cf68a25..4fd24b7 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/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,12 +350,10 @@ 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 @@ -345,6 +361,35 @@ def update(self, curCmd:DrivetrainCommand, stepSize_m:float) -> DrivetrainComman 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() diff --git a/potentialFieldPlotter.py b/potentialFieldPlotter.py index f7683f2..6da7ca8 100644 --- a/potentialFieldPlotter.py +++ b/potentialFieldPlotter.py @@ -45,11 +45,11 @@ def draw(): self.objects.append(draw) draw() - def add_arrow(self, start_m: Tuple[float, float], end_m: Tuple[float, float], color: str = "red"): + def add_arrow(self, start_m: Tuple[float, float], end_m: Tuple[float, float], color: str = "red", width: int = 2): def draw(): x1, y1 = self._to_pixels(*start_m) x2, y2 = self._to_pixels(*end_m) - self.canvas.create_line(x1, y1, x2, y2, arrow=tk.LAST, fill=color, width=2) + self.canvas.create_line(x1, y1, x2, y2, fill=color, width=width, arrow=tk.LAST) self.objects.append(draw) draw() @@ -63,11 +63,11 @@ def draw(): self.objects.append(draw) draw() - def add_circle(self, location_m: Tuple[float, float], radius_m:float, color: str = "red"): + def add_circle(self, location_m: Tuple[float, float], radius_m:float, color: str = "red", width: int=3): def draw(): x1, y1 = self._to_pixels(location_m[0]-radius_m, location_m[1]-radius_m) x2, y2 = self._to_pixels(location_m[0]+radius_m, location_m[1]+radius_m) - self.canvas.create_oval(x1,y1,x2,y2,outline=color, width=3) + self.canvas.create_oval(x1,y1,x2,y2,outline=color, width=width) self.objects.append(draw) draw() @@ -83,7 +83,7 @@ def _to_pixels(self, x_m: float, y_m: float) -> Tuple[int, int]: # Create the main Tkinter application def main(): - arrowSpacing_m = 0.25 + arrowSpacing_m = 0.21 root = tk.Tk() root.title("Potential Fields") @@ -91,8 +91,9 @@ def main(): canvas = ScaledCanvas(root, width_m, height_m) # Set up the actual repulsor field planner - rpf = RepulsorFieldPlanner() - rpf.setGoal(GOAL_SPEAKER) + rfp = RepulsorFieldPlanner() + #rfp.setGoal(GOAL_SPEAKER) + rfp.setGoal(GOAL_PICKUP) # Field boundary canvas.add_rectangle(0.0, 0.0, FIELD_X_M, FIELD_Y_M, color="black") @@ -102,13 +103,12 @@ def main(): num_y_samples = int(FIELD_Y_M / arrowSpacing_m + 1) forces: dict[tuple[int, int], Force]= {} - # Add some arrows for x_idx in range(num_x_samples): for y_idx in range(num_y_samples): x_pos = x_idx * arrowSpacing_m y_pos = y_idx * arrowSpacing_m - force = rpf._getForceAtTrans(Translation2d(x_pos, y_pos)) + force = rfp._getForceAtTrans(Translation2d(x_pos, y_pos)) forces[(x_idx,y_idx)] = force @@ -117,15 +117,18 @@ def main(): canvas.add_arrow(start, end, color="green") # Plot Obstacles - for obs in rpf.fixedObstacles: - if(isinstance(obs, PointObstacle)): - canvas.add_circle((obs.location.X(), obs.location.Y()), obs.radius, color="grey") - elif(isinstance(obs,LinearForceGenerator)): - if(isinstance(obs, Lane)): - linecolor="yellow" - else: - linecolor = "grey" - canvas.add_line((obs.start.X(), obs.start.Y()), (obs.end.X(), obs.end.Y()), color=linecolor) + for obs in rfp.fixedObstacles: + if(obs.strength != 0.0): + if(isinstance(obs, PointObstacle)): + canvas.add_circle((obs.location.X(), obs.location.Y()), obs.radius, color="#995555") + elif(isinstance(obs,LinearForceGenerator)): + if(isinstance(obs, Lane)): + linecolor="#11dd33" + canvas.add_arrow((obs.start.X(), obs.start.Y()), (obs.end.X(), obs.end.Y()), color=linecolor, width = 6) + else: + linecolor = "#995555" + canvas.add_line((obs.start.X(), obs.start.Y()), (obs.end.X(), obs.end.Y()), color=linecolor) + # Exhaustively detect local minima DELTA_M = arrowSpacing_m/2.0 @@ -140,17 +143,20 @@ def main(): y_pos = y_idx * arrowSpacing_m # Look for minima in X - forcePrev = rpf._getForceAtTrans(Translation2d(x_pos - DELTA_M, y_pos)) - forceNext = rpf._getForceAtTrans(Translation2d(x_pos + DELTA_M, y_pos)) + forcePrev = rfp._getForceAtTrans(Translation2d(x_pos - DELTA_M, y_pos)) + forceNext = rfp._getForceAtTrans(Translation2d(x_pos + DELTA_M, y_pos)) xHasMinima = (forcePrev.x > 0 and forceNext.x < 0) # Look for minima in Y - forcePrev = rpf._getForceAtTrans(Translation2d(x_pos, y_pos - DELTA_M)) - forceNext = rpf._getForceAtTrans(Translation2d(x_pos, y_pos + DELTA_M)) + forcePrev = rfp._getForceAtTrans(Translation2d(x_pos, y_pos - DELTA_M)) + forceNext = rfp._getForceAtTrans(Translation2d(x_pos, y_pos + DELTA_M)) yHasMinima = (forcePrev.y > 0 and forceNext.y < 0) if(yHasMinima and xHasMinima): - canvas.add_circle((x_pos, y_pos), 0.025) + canvas.add_circle((x_pos, y_pos), 0.1) + + if(rfp.goal is not None): + canvas.add_circle((rfp.goal.x, rfp.goal.y), 0.075, color="cyan", width=10) root.mainloop()