From 4f70555406b4338e106a0337b7c32d7748423d3d Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Thu, 3 Oct 2024 22:36:38 -0500 Subject: [PATCH 1/4] Quick experiment where we take substeps to try to avoid stepping over zero crossings. --- navigation/repulsorFieldPlanner.py | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 53a2065..6c925f3 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -1,6 +1,6 @@ from dataclasses import dataclass import math -from wpimath.geometry import Pose2d, Translation2d +from wpimath.geometry import Pose2d, Translation2d, Transform2d, Rotation2d from navigation.navConstants import FIELD_X_M, FIELD_Y_M from drivetrain.drivetrainCommand import DrivetrainCommand @@ -126,13 +126,21 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: # Slow down when we're near the goal slowFactor = GOAL_SLOW_DOWN_MAP.lookup(self.distToGo) - netForce = self.getForceAtTrans(curPose.translation()) + nextTrans = curTrans - # Calculate a substep in the direction of the force - step = Translation2d(stepSize_m*netForce.unitX(), stepSize_m*netForce.unitY()) + for _ in range(4): + + if (nextTrans - curTrans).norm() >= stepSize_m: + break + + netForce = self.getForceAtTrans(nextTrans) + + # Calculate a substep in the direction of the force + step = Translation2d(stepSize_m*netForce.unitX()*0.5, stepSize_m*netForce.unitY()*0.5) + + # Take that step + nextTrans += step - # Take that substep - nextTrans = curTrans + step # Assemble velocity commands based on the step we took retVal.velX = (nextTrans - curTrans).X()/0.02 * slowFactor From c91a1687f33dfa68882323abee9778e0782356c3 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sat, 5 Oct 2024 21:09:09 -0500 Subject: [PATCH 2/4] Added detection of getting stuck, and led to indicate state --- drivetrain/controlStrategies/autoDrive.py | 23 ++++----- humanInterface/ledControl.py | 38 +++++++++++++++ navigation/repulsorFieldPlanner.py | 58 ++++++++++++++++------- robot.py | 21 +++++--- utils/constants.py | 4 +- vectorPlotter.py | 2 +- 6 files changed, 109 insertions(+), 37 deletions(-) create mode 100644 humanInterface/ledControl.py diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index 638bfc3..edddfc7 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -18,9 +18,8 @@ class AutoDrive(metaclass=Singleton): def __init__(self): self._toSpeaker = False self._toPickup = False - self._rfp = RepulsorFieldPlanner() + self.rfp = RepulsorFieldPlanner() self._trajCtrl = HolonomicDriveController() - self._curPose = Pose2d() self._telemTraj = [] self._obsDet = ObstacleDetector() @@ -32,35 +31,37 @@ def getTrajectory(self) -> Trajectory|None: return None # TODO def updateTelemetry(self) -> None: - self._telemTraj = self._rfp.updateTelemetry(self._curPose) + self._telemTraj = self.rfp.updateTelemetry() def getWaypoints(self) -> list[Pose2d]: return self._telemTraj def getObstacles(self) -> list[Translation2d]: - return self._rfp.getObstacleTransList() + return self.rfp.getObstacleTransList() + + def isRunning(self)->bool: + return self.rfp.goal != None def update(self, cmdIn: DrivetrainCommand, curPose: Pose2d) -> DrivetrainCommand: retCmd = cmdIn # default - no auto driving - self._curPose = curPose for obs in self._obsDet.getObstacles(curPose): - self._rfp.add_obstcale_observaton(obs) + self.rfp.add_obstcale_observaton(obs) - self._rfp.decay_observations() + self.rfp._decay_observations() # Handle command changes if(self._toPickup): - self._rfp.setGoal(transform(GOAL_PICKUP)) + self.rfp.setGoal(transform(GOAL_PICKUP)) elif(self._toSpeaker): - self._rfp.setGoal(transform(GOAL_SPEAKER)) + self.rfp.setGoal(transform(GOAL_SPEAKER)) elif(not self._toSpeaker and not self._toPickup): - self._rfp.setGoal(None) + self.rfp.setGoal(None) # 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) + olCmd = self.rfp.update(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") diff --git a/humanInterface/ledControl.py b/humanInterface/ledControl.py new file mode 100644 index 0000000..c023316 --- /dev/null +++ b/humanInterface/ledControl.py @@ -0,0 +1,38 @@ +from wpilib import PWMMotorController +from utils.constants import LED_STACK_LIGHT_CTRL_PWM +from utils.singleton import Singleton +from wpimath.filter import Debouncer + +BLINK = -1.0 +GREEN = 0.35 +RED = 0.03 +BLUE = 0.85 +OFF = 0.0 + +class LEDControl(metaclass=Singleton): + def __init__(self): + + self._isAutoDrive = False + self._isStuck = False + self.stuckDebounce = Debouncer(0.3, Debouncer.DebounceType.kFalling) + self.ctrlBlock = PWMMotorController("LEDCtrl", LED_STACK_LIGHT_CTRL_PWM) + + def update(self): + + stuckDebounced = self.stuckDebounce.calculate(self._isStuck) + + if(self._isAutoDrive): + if(stuckDebounced): + pwmVal = RED * BLINK + else: + pwmVal = BLUE + else: + pwmVal = GREEN + + self.ctrlBlock.set(pwmVal) + + def setAutoDrive(self, isAutoDrive:bool): + self._isAutoDrive = isAutoDrive + + def setStuck(self, isStuck:bool): + self._isStuck = isStuck \ No newline at end of file diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 53a2065..afb92e9 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -1,5 +1,3 @@ -from dataclasses import dataclass -import math from wpimath.geometry import Pose2d, Translation2d from navigation.navConstants import FIELD_X_M, FIELD_Y_M @@ -42,8 +40,12 @@ ]) # These define how far in advance we attempt to plan for telemetry purposes -TELEM_LOOKAHEAD_DIST_M = 3.0 -TELEM_LOOKAHEAD_STEPS = 7 +LOOKAHEAD_DIST_M = 3.0 +LOOKAHEAD_STEPS = 7 +LOOKAHEAD_STEP_SIZE = LOOKAHEAD_DIST_M/LOOKAHEAD_STEPS + +# If the lookahead routine's end poit is within this, we declare ourselves stuck. +STUCK_DIST = LOOKAHEAD_DIST_M/4 class RepulsorFieldPlanner: def __init__(self): @@ -53,6 +55,7 @@ def __init__(self): self.transientObstcales:list[Obstacle] = [] self.distToGo:float = 0.0 self.goal:Pose2d|None = None + self.lookaheadTraj:list[Pose2d] = [] def setGoal(self, goal:Pose2d|None): self.goal = goal @@ -70,7 +73,7 @@ def getGoalForce(self, curLocation:Translation2d) -> Force: # no goal, no force return Force() - def decay_observations(self): + def _decay_observations(self): # Linear decay of each transient obstacle observation for obs in self.transientObstcales: obs.strength -= 0.01 @@ -87,6 +90,15 @@ def getObstacleTransList(self) -> list[Translation2d]: return retArr + def isStuck(self) -> bool: + 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 def atGoal(self, trans:Translation2d)->bool: if(self.goal is None): @@ -94,7 +106,7 @@ def atGoal(self, trans:Translation2d)->bool: else: return (self.goal.translation() - trans).norm() < GOAL_MARGIN_M - def getForceAtTrans(self, trans:Translation2d)->Force: + def _getForceAtTrans(self, trans:Translation2d)->Force: goalForce = self.getGoalForce(trans) @@ -112,8 +124,12 @@ def getForceAtTrans(self, trans:Translation2d)->Force: return netForce + def update(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: + curCmd = self._getCmd(curPose, stepSize_m) + self._doLookahead(curPose) + return curCmd - def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: + def _getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: retVal = DrivetrainCommand() # Default, no command if(self.goal is not None): @@ -126,7 +142,7 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: # Slow down when we're near the goal slowFactor = GOAL_SLOW_DOWN_MAP.lookup(self.distToGo) - netForce = self.getForceAtTrans(curPose.translation()) + netForce = self._getForceAtTrans(curPose.translation()) # Calculate a substep in the direction of the force step = Translation2d(stepSize_m*netForce.unitX(), stepSize_m*netForce.unitY()) @@ -141,23 +157,33 @@ def getCmd(self, curPose:Pose2d, stepSize_m:float) -> DrivetrainCommand: 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 + def _doLookahead(self, curPose): + self.lookaheadTraj = [] if(self.goal is not None): cp = curPose - for _ in range(0,TELEM_LOOKAHEAD_STEPS): - telemTraj.append(cp) - tmp = self.getCmd(cp, stepsize) + self.lookaheadTraj.append(cp) + + for _ in range(0,LOOKAHEAD_STEPS): + tmp = self._getCmd(cp, LOOKAHEAD_STEP_SIZE) if(tmp.desPose is not None): 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 else: + # Weird, no pose given back, give up. break + def updateTelemetry(self) -> list[Pose2d]: 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 + if(self.goal is not None): + return self.lookaheadTraj + else: + return [] diff --git a/robot.py b/robot.py index f80e181..0d2b54b 100644 --- a/robot.py +++ b/robot.py @@ -6,6 +6,7 @@ from drivetrain.drivetrainCommand import DrivetrainCommand from drivetrain.drivetrainControl import DrivetrainControl from humanInterface.driverInterface import DriverInterface +from humanInterface.ledControl import LEDControl from navigation.obstacles import PointObstacle from utils.segmentTimeTracker import SegmentTimeTracker from utils.signalLogging import SignalWrangler @@ -39,6 +40,7 @@ def robotInit(self): self.stt = SegmentTimeTracker() self.dInt = DriverInterface() + self.ledCtrl = LEDControl() self.autoSequencer = AutoSequencer() @@ -68,6 +70,10 @@ def robotPeriodic(self): self.driveTrain.poseEst.telemetry.setCurTrajWaypoints(self.autodrive.getWaypoints()) self.driveTrain.poseEst.telemetry.setCurObstacles(self.autodrive.getObstacles()) + self.ledCtrl.setAutoDrive(self.autodrive.isRunning()) + self.ledCtrl.setStuck(self.autodrive.rfp.isStuck()) + self.ledCtrl.update() + self.stt.end() ######################################################### @@ -111,22 +117,23 @@ def teleopPeriodic(self): # For test purposes, inject a series of obstacles around the current pose ct = self.driveTrain.poseEst.getCurEstPose().translation() tfs = [ - Translation2d(3.0, -0.5), - Translation2d(3.0, 0.0), - Translation2d(-3.0, 0.5), - Translation2d(-3.0, 0.0) + Translation2d(1.7, -0.5), + Translation2d(0.75, -0.75), + Translation2d(1.7, 0.5), + Translation2d(0.75, 0.75), + Translation2d(2.0, 0.0), + Translation2d(0.0, 1.0), + Translation2d(0.0, -1.0), ] for tf in tfs: obs = PointObstacle(location=(ct+tf), strength=0.7) - self.autodrive._rfp.add_obstcale_observaton(obs) + self.autodrive.rfp.add_obstcale_observaton(obs) self.autodrive.setRequest(self.dInt.getNavToSpeaker(), self.dInt.getNavToPickup()) # No trajectory in Teleop Trajectory().setCmd(None) - - ######################################################### ## Disabled-Specific init and update def disabledPeriodic(self): diff --git a/utils/constants.py b/utils/constants.py index 0338e87..a16f322 100644 --- a/utils/constants.py +++ b/utils/constants.py @@ -64,8 +64,8 @@ # Unused = 5 # Unused = 6 # Unused = 7 -# LED_STRIPS_CTRL_PWM = 8 -LED_BLOCK_CTRL_PWM = 9 +# Unused = 8 +LED_STACK_LIGHT_CTRL_PWM = 9 ####################################################################################### diff --git a/vectorPlotter.py b/vectorPlotter.py index 2dea382..823fea6 100644 --- a/vectorPlotter.py +++ b/vectorPlotter.py @@ -204,7 +204,7 @@ def clear_vectors(self): YCount = 0.1 while YCount < 8.1: while XCount < 16.4: - force = rpp.getForceAtTrans(Translation2d(XCount, YCount)) + force = rpp._getForceAtTrans(Translation2d(XCount, YCount)) plotter.add_vector(XCount, YCount, force.x, force.y) XCount += .2 YCount += .2 From 4fe0d9368d96f65089eed8ca9e24f6cedd5847d5 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Sat, 5 Oct 2024 21:34:56 -0500 Subject: [PATCH 3/4] reduced lookahead to reduce load --- navigation/repulsorFieldPlanner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/navigation/repulsorFieldPlanner.py b/navigation/repulsorFieldPlanner.py index 93a4051..b71ea33 100644 --- a/navigation/repulsorFieldPlanner.py +++ b/navigation/repulsorFieldPlanner.py @@ -40,8 +40,8 @@ ]) # These define how far in advance we attempt to plan for telemetry purposes -LOOKAHEAD_DIST_M = 3.0 -LOOKAHEAD_STEPS = 7 +LOOKAHEAD_DIST_M = 1.5 +LOOKAHEAD_STEPS = 4 LOOKAHEAD_STEP_SIZE = LOOKAHEAD_DIST_M/LOOKAHEAD_STEPS # If the lookahead routine's end poit is within this, we declare ourselves stuck. From cdde313fae15eb8bf2a4c4fe3bc7e7304faa9a60 Mon Sep 17 00:00:00 2001 From: Chris Gerth Date: Fri, 11 Oct 2024 08:17:03 -0500 Subject: [PATCH 4/4] Revised planner mechanism to use all desired poses, no feedback into actual pose --- drivetrain/controlStrategies/autoDrive.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/drivetrain/controlStrategies/autoDrive.py b/drivetrain/controlStrategies/autoDrive.py index edddfc7..2a9a9c4 100644 --- a/drivetrain/controlStrategies/autoDrive.py +++ b/drivetrain/controlStrategies/autoDrive.py @@ -22,6 +22,7 @@ def __init__(self): self._trajCtrl = HolonomicDriveController() self._telemTraj = [] self._obsDet = ObstacleDetector() + self._prevCmd:DrivetrainCommand|None = None def setRequest(self, toSpeaker, toPickup) -> None: self._toSpeaker = toSpeaker @@ -61,12 +62,22 @@ 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.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) + + if(self._prevCmd is None or self._prevCmd.desPose is None): + olCmd = self.rfp.update(curPose, MAX_DT_LINEAR_SPEED*0.02*SPEED_SCALAR) + else: + olCmd = self.rfp.update(self._prevCmd.desPose, 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) + + self._prevCmd = retCmd + else: + self._prevCmd = None return retCmd \ No newline at end of file