From 0f253269bb916e0bfc316444cac77feba13bae3c Mon Sep 17 00:00:00 2001 From: Oryx Desktop Computer Date: Tue, 29 Oct 2024 20:56:15 -0500 Subject: [PATCH 1/5] (NON-FUNCTINAL WIP) Didn't work because I was operating the test to see if the position didn't change in the path planning stage rather than the path executing stage. Hopefully easy fix? --- navigation/repulsorFieldPlanner.py | 34 ++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 659bf32..dfbaad2 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -10,6 +10,8 @@ from navigation.forceGenerators import HorizontalObstacle, ForceGenerator, PointObstacle, VerticalObstacle from utils.constants import FIELD_X_M, FIELD_Y_M +import math + # 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") @@ -34,6 +36,15 @@ PointObstacle(location=Translation2d(11.0, 2.74)), PointObstacle(location=Translation2d(13.27, 4.07)), PointObstacle(location=Translation2d(11.0, 5.35)), + PointObstacle(location=Translation2d(0, 0)), + PointObstacle(location=Translation2d(10.0, 5.35)), + PointObstacle(location=Translation2d(10.5, 6.35)), + PointObstacle(location=Translation2d(10.5, 5.35)), + PointObstacle(location=Translation2d(10.0, 5.35)), + PointObstacle(location=Translation2d(10.0, 5.85)), + PointObstacle(location=Translation2d(11.0, 5.85)), + PointObstacle(location=Translation2d(11.0, 6.35)), + PointObstacle(location=Translation2d(10.0, 6.35)), ] # Fixed Obstacles - Outer walls of the field @@ -293,19 +304,42 @@ def _doLookahead(self, curPose): if(self.goal is not None): cp = curPose self.lookaheadTraj.append(cp) + self.stuckStrikes = 0 + """ + + Current Solution for detecting if we are stuck doesn't work. I think we can fix it by moving it to the function that calls this one and having that function test if the average change in position is bellow a threshhold for too many attempts + + """ for _ in range(0,LOOKAHEAD_STEPS): tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE) if(tmp.desPose is not None): + """ + Check to see if the change in position between the current position and the position of the goal of the command from the lookahead is below a certain percentage of the step size + if they are similar increment a "Strike" counter + + """ + if math.sqrt(((cp.X() - tmp.desPose.X() )** 2) + ((cp.Y() - tmp.desPose.Y())** 2) ) <= LOOKAHEAD_STEP_SIZE * .5: #If the total change in position is less than five percent of the lookahead step size + self.stuckStrikes += 1 + cp = tmp.desPose self.lookaheadTraj.append(cp) if((cp - self.goal).translation().norm() < LOOKAHEAD_STEP_SIZE): # At the goal, no need to keep looking ahead break + + """ + If we get 3 strikes (Or some arbitrary number of strikes or a percentage of the LOOKAHEAD_STEPS value) then we're 'out' and well say that its safe to assume that we are stuck + """ + + if self.stuckStrikes >= 3: + break + else: # Weird, no pose given back, give up. break + print(self.stuckStrikes) def getLookaheadTraj(self) -> list[Pose2d]: """ From f282ef7322297a8666e415182083aa02e9b4f385 Mon Sep 17 00:00:00 2001 From: Oryx Desktop Computer Date: Thu, 7 Nov 2024 20:30:34 -0600 Subject: [PATCH 2/5] Working?? stuck detection. Currently doesn't print or log anything when it gets stuck. Anyone else wanna try and break it before I merge this branch and main? Noticed an issue in sim that I will look into soon. --- drivetrain/controlStrategies/autoDrive.py | 27 ++++++++++++++++++++ drivetrain/drivetrainCommand.py | 2 +- navigation/repulsorFieldPlanner.py | 31 +---------------------- 3 files changed, 29 insertions(+), 31 deletions(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 71078c2..c6dfaa0 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -10,6 +10,7 @@ from navigation.navConstants import GOAL_PICKUP, GOAL_SPEAKER from drivetrain.drivetrainPhysical import MAX_DT_LINEAR_SPEED_MPS from utils.allianceTransformUtils import transform +import math # Maximum speed that we'll attempt to path plan at. Needs to be at least # slightly less than the maximum physical speed, so the robot can "catch up" @@ -27,6 +28,10 @@ def __init__(self): self._olCmd = DrivetrainCommand() self._prevCmd:DrivetrainCommand|None = None self._plannerDur:float = 0.0 + self.autoPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto last refresh. + self.stuckTracker = 0 + self.prevPose = Pose2d() + addLog("AutoDrive Proc Time", lambda:(self._plannerDur * 1000.0), "ms") @@ -34,6 +39,14 @@ def __init__(self): def setRequest(self, toSpeaker, toPickup) -> None: self._toSpeaker = toSpeaker self._toPickup = toPickup + #The following if statement is just logic to enable self.autoPrevEnabled when the driver enables an auto. + if self._toSpeaker == True or self._toPickup == True: + if self.autoPrevEnabled == False: + self.stuckTracker = 0 + self.autoPrevEnabled = True + if self._toSpeaker == False and self._toPickup == False: + self.autoPrevEnabled = False + def updateTelemetry(self) -> None: self._telemTraj = self.rfp.getLookaheadTraj() @@ -48,6 +61,7 @@ def isRunning(self)->bool: return self.rfp.goal != None def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand: + startTime = Timer.getFPGATimestamp() @@ -69,6 +83,13 @@ 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): + #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 or self._prevCmd.desPose is None): @@ -89,4 +110,10 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand 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. + return retCmd \ No newline at end of file diff --git a/drivetrain/drivetrainCommand.py b/drivetrain/drivetrainCommand.py index e49448f..23ac37d 100644 --- a/drivetrain/drivetrainCommand.py +++ b/drivetrain/drivetrainCommand.py @@ -12,4 +12,4 @@ class DrivetrainCommand: velX:float = 0.0 # Field X velocity in meters/sec velY:float = 0.0 # Field Y velocity in meters/sec velT:float = 0.0 # Rotational speed in rad/sec - desPose: Pose2d | None = None # Current desired pose of the drivetrain, nor None if no pose is specified. + desPose: Pose2d | None = None # Current desired pose of the drivetrain, or None if no pose is specified. diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index dfbaad2..ceb2e13 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -36,15 +36,7 @@ PointObstacle(location=Translation2d(11.0, 2.74)), PointObstacle(location=Translation2d(13.27, 4.07)), PointObstacle(location=Translation2d(11.0, 5.35)), - PointObstacle(location=Translation2d(0, 0)), - PointObstacle(location=Translation2d(10.0, 5.35)), - PointObstacle(location=Translation2d(10.5, 6.35)), - PointObstacle(location=Translation2d(10.5, 5.35)), - PointObstacle(location=Translation2d(10.0, 5.35)), - PointObstacle(location=Translation2d(10.0, 5.85)), - PointObstacle(location=Translation2d(11.0, 5.85)), - PointObstacle(location=Translation2d(11.0, 6.35)), - PointObstacle(location=Translation2d(10.0, 6.35)), + PointObstacle(location=Translation2d(0, 0)) ] # Fixed Obstacles - Outer walls of the field @@ -304,23 +296,10 @@ def _doLookahead(self, curPose): if(self.goal is not None): cp = curPose self.lookaheadTraj.append(cp) - self.stuckStrikes = 0 - - """ - - Current Solution for detecting if we are stuck doesn't work. I think we can fix it by moving it to the function that calls this one and having that function test if the average change in position is bellow a threshhold for too many attempts - """ for _ in range(0,LOOKAHEAD_STEPS): tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE) if(tmp.desPose is not None): - """ - Check to see if the change in position between the current position and the position of the goal of the command from the lookahead is below a certain percentage of the step size - if they are similar increment a "Strike" counter - - """ - if math.sqrt(((cp.X() - tmp.desPose.X() )** 2) + ((cp.Y() - tmp.desPose.Y())** 2) ) <= LOOKAHEAD_STEP_SIZE * .5: #If the total change in position is less than five percent of the lookahead step size - self.stuckStrikes += 1 cp = tmp.desPose self.lookaheadTraj.append(cp) @@ -329,17 +308,9 @@ def _doLookahead(self, curPose): # At the goal, no need to keep looking ahead break - """ - If we get 3 strikes (Or some arbitrary number of strikes or a percentage of the LOOKAHEAD_STEPS value) then we're 'out' and well say that its safe to assume that we are stuck - """ - - if self.stuckStrikes >= 3: - break - else: # Weird, no pose given back, give up. break - print(self.stuckStrikes) def getLookaheadTraj(self) -> list[Pose2d]: """ From 7490fb69973099b12e61f616736d5d6a100409c4 Mon Sep 17 00:00:00 2001 From: Oryx Desktop Computer Date: Thu, 7 Nov 2024 20:46:47 -0600 Subject: [PATCH 3/5] Fixed previous issue that caused the preplanner origin to be separated from the robot position. Might be a good idea to test for more edge cases. --- drivetrain/controlStrategies/autoDrive.py | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index c6dfaa0..8ab8c20 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -28,10 +28,10 @@ def __init__(self): self._olCmd = DrivetrainCommand() self._prevCmd:DrivetrainCommand|None = None self._plannerDur:float = 0.0 - self.autoPrevEnabled = False #This name might be a wee bit confusing. It just keeps track if we were in auto last refresh. + 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") @@ -40,12 +40,10 @@ def setRequest(self, toSpeaker, toPickup) -> None: self._toSpeaker = toSpeaker self._toPickup = toPickup #The following if statement is just logic to enable self.autoPrevEnabled when the driver enables an auto. - if self._toSpeaker == True or self._toPickup == True: - if self.autoPrevEnabled == False: - self.stuckTracker = 0 - self.autoPrevEnabled = True - if self._toSpeaker == False and self._toPickup == False: - self.autoPrevEnabled = False + if self.autoSpeakerPrevEnabled != self._toSpeaker or self.autoPickupPrevEnabled != self._toPickup: + self.stuckTracker = 0 + self.autoPickupPrevEnabled = self._toPickup + self.autoSpeakerPrevEnabled = self._toSpeaker def updateTelemetry(self) -> None: From f1d73b09e1a9b930695761bdcffce1cd7c399f13 Mon Sep 17 00:00:00 2001 From: Oryx Desktop Computer Date: Thu, 14 Nov 2024 18:59:40 -0600 Subject: [PATCH 4/5] Fixed issue caused by slight change in logic between the old and new version. Current behavior will just ignore the calculated command and just use the controller's instead. It still does the calculations for auto driving when its stuck but it just ignores the result. This can be changed later. Auto movement wont restart after being stuck until the auto button is pressed again. --- drivetrain/controlStrategies/autoDrive.py | 1 + 1 file changed, 1 insertion(+) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 6bc1268..724bcc5 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -113,5 +113,6 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand #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.stuckTracker += 1 return retCmd \ No newline at end of file From 6c8766411b53b64a89329adf60fdd6ffe248b117 Mon Sep 17 00:00:00 2001 From: Oryx Desktop Computer Date: Thu, 14 Nov 2024 19:13:59 -0600 Subject: [PATCH 5/5] Fixed an issue where the robot wouldn't recalculate it's position when re-enabling auto drive. Also corrected the code so it doesn't constantly calculate autodrive after assuming its stuck just to ignore the calculations. --- drivetrain/controlStrategies/autoDrive.py | 49 ++++++++++++----------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 724bcc5..8172f40 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -80,29 +80,30 @@ 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): - - #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) - self._prevCmd = retCmd + 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) + self._prevCmd = retCmd else: self._prevCmd = None @@ -113,6 +114,6 @@ def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand #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.stuckTracker += 1 + self._prevCmd = None return retCmd \ No newline at end of file