From b0f68fa7f31060d1c7ad1aadfafb73448c622d31 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu <134637512+Rongrrz@users.noreply.github.com> Date: Sat, 18 Jan 2025 15:53:11 -0800 Subject: [PATCH] Revert "Swerve simple trajectory update (#485)" (#500) This reverts commit 8ae3bcbc2a4512529ca40e598c379f1682a50b61. --- .../drive/SwerveCalculatorNode.java | 12 - .../drive/SwerveKinematicsCalculator.java | 288 ------------------ .../drive/SwervePointKinematics.java | 28 -- .../drive/SwerveSimpleTrajectoryCommand.java | 5 +- .../drive/SwerveSimpleTrajectoryMode.java | 9 - .../subsystems/pose/BasePoseSubsystem.java | 1 - .../trajectory/ProvidesInterpolationData.java | 3 - .../trajectory/SimpleTimeInterpolator.java | 62 +--- .../SwerveSimpleTrajectoryLogic.java | 191 ++---------- .../common/trajectory/XbotSwervePoint.java | 9 - .../drive/SwerveCalculatorTest.java | 125 -------- 11 files changed, 39 insertions(+), 694 deletions(-) delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java delete mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java delete mode 100644 src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java b/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java deleted file mode 100644 index ebae44dd8..000000000 --- a/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java +++ /dev/null @@ -1,12 +0,0 @@ -package xbot.common.subsystems.drive; - - -/** - * @param operationEndingVelocity m/s, velocity we end at - * @param operationAcceleration mls^2, acceleration to go at - * @param operationTime Time in seconds - */ // SwerveCalculatorNode are small little steps to be interpreted and formed together -//to create a path to be used for the SwerveKinematicsCalculator -public record SwerveCalculatorNode(double operationTime, double operationAcceleration, double operationEndingVelocity) { - -} diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java b/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java deleted file mode 100644 index bb75971b2..000000000 --- a/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java +++ /dev/null @@ -1,288 +0,0 @@ -package xbot.common.subsystems.drive; - -import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; -import xbot.common.logging.RobotAssertionManager; - -import java.util.ArrayList; -import java.util.List; - -public class SwerveKinematicsCalculator { - - /* - The SwerveKinematicsCalculator is designed to compute the motion of a swerve drive system from a starting position - and velocity to a goal position and velocity. It takes into account parameters like maximum acceleration, - goal velocity, and maximum velocity to generate a sequence of motion stages, ensuring precise path planning - and execution. - - (Breaks an XbotSwervePoint down into smaller nodes containing time, velocity, and acceleration) - Note: goal velocity may not always be reached, but in such case will be as close as possible. - - Code involving the quadratic formula IS NOT ROBUST (since only this script uses them as of currently) - */ - - final double startPosition; - final double endPosition; - final double acceleration; - final double initialVelocity; - final double goalVelocity; - final double maxVelocity; - final List nodeMap; - RobotAssertionManager assertionManager; - - // Returns the x-intercepts of a quadratic equation - private static List quadraticFormula(double a, double b, double c) { - double squareRootResult = Math.sqrt(Math.pow(b, 2) - (4 * a * c)); - double result1 = (-b + squareRootResult) / (2 * a); - double result2 = (-b - squareRootResult) / (2 * a); - - List results = new ArrayList<>(); - results.add(result1); - results.add(result2); - return results; - } - - public static double calculateTimeToGoalPosition(double acceleration, double initialVelocity, - double initialPosition, double goalPosition) { - double a = 0.5 * acceleration; - double b = initialVelocity; - double c = initialPosition - goalPosition; - if (acceleration == 0) { - return (-c/b); - - } else if (acceleration > 0) { - List result = quadraticFormula(a, b, c); - return Math.max(result.get(0), result.get(1)); - - } else { - List result = quadraticFormula(-a, -b, -c); - return Math.min(result.get(0), result.get(1)); - - } - } - - public SwerveKinematicsCalculator(RobotAssertionManager assertionManager, double startPosition, double endPosition, - SwervePointKinematics kinematics) { - this.assertionManager = assertionManager; - // Gimmicky way to avoid division by zero and to deal with human code-error - if (endPosition - startPosition == 0) { - assertionManager.throwException("Calculator instantiated for same start and end position", new Exception()); - endPosition += 0.01; - } - - this.startPosition = startPosition; - this.endPosition = endPosition; - this.acceleration = kinematics.acceleration; - this.initialVelocity = kinematics.initialVelocity; - this.goalVelocity = kinematics.goalVelocity; - this.maxVelocity = kinematics.maxVelocity; - - // Likely need to validate the above values to prevent bad stuff - nodeMap = generateNodeMap(); - } - - // Based off of given value, initialize the proper path, store notes in a list, and be prepared to output values - // (If velocity gets to negative we are *KINDA* cooked) - public List generateNodeMap() { - List nodeMap = new ArrayList<>(); - double leftoverDistance = endPosition - startPosition; - - double currentVelocity = initialVelocity; - - // Deceleration - if (currentVelocity > goalVelocity) { - // Cruise as much as possible, then decelerate - double decelerateToGoalVelocityTime = (goalVelocity - currentVelocity) / -acceleration; - double distanceNeededToDecelerate = 0.5 * (currentVelocity + goalVelocity) * decelerateToGoalVelocityTime; - - double distanceExcludingDeceleration = leftoverDistance - distanceNeededToDecelerate; - - if (distanceExcludingDeceleration > 0) { - double halfDistance = distanceExcludingDeceleration / 2; - double timeForHalf = calculateTimeToGoalPosition(acceleration, currentVelocity, 0, halfDistance); - double peakVelocity = currentVelocity + acceleration * timeForHalf; - - if (peakVelocity <= maxVelocity) { - // Add two nodes, accelerate to peak, decelerate to goal - nodeMap.add(new SwerveCalculatorNode(timeForHalf, acceleration, peakVelocity)); - nodeMap.add(new SwerveCalculatorNode(timeForHalf, -acceleration, currentVelocity)); - } else { - // Going to peak will not work as it will exceed our maximumVelocity limit - // Go to max, then cruise, then decelerate - double timeFromCurrentToMaxV = (maxVelocity - currentVelocity) / acceleration; // Rename - double initialPosition = (endPosition - startPosition) - leftoverDistance; - double finalPositionAfterAcceleration = 0.5 * (currentVelocity + maxVelocity) * timeFromCurrentToMaxV + initialPosition; - double positionDelta = finalPositionAfterAcceleration - initialPosition; - double cruiseDistance = distanceExcludingDeceleration - (positionDelta * 2); - double cruiseTime = cruiseDistance / maxVelocity; - - nodeMap.add(new SwerveCalculatorNode(timeFromCurrentToMaxV, acceleration, maxVelocity)); - nodeMap.add(new SwerveCalculatorNode(cruiseTime, 0, maxVelocity)); - nodeMap.add(new SwerveCalculatorNode(timeFromCurrentToMaxV, -acceleration, currentVelocity)); - } - - nodeMap.add(new SwerveCalculatorNode(decelerateToGoalVelocityTime, -acceleration, goalVelocity)); - - } else { - // Decelerate as much as possible - double decelerationTime = calculateTimeToGoalPosition(-acceleration, currentVelocity, 0, leftoverDistance); - nodeMap.add(new SwerveCalculatorNode( - decelerationTime, - -acceleration, - currentVelocity - acceleration * decelerationTime - )); - } - return nodeMap; - } - - // Acceleration - if (currentVelocity < goalVelocity) { - double initialToGoalVelocityTime = (goalVelocity - currentVelocity) / acceleration; - double operationDistance = currentVelocity * initialToGoalVelocityTime - + 0.5 * acceleration * Math.pow(initialToGoalVelocityTime, 2); - - if (operationDistance >= leftoverDistance) { - // No matter how much you accelerate you won't reach goal velocity before distance... - // ...so we accelerate til the end! - double time = calculateTimeToGoalPosition(acceleration, currentVelocity, 0, leftoverDistance); - nodeMap.add(new SwerveCalculatorNode( - time, - acceleration, - currentVelocity + acceleration * time - )); - return nodeMap; - } else { - // Accelerate to goal velocity - nodeMap.add(new SwerveCalculatorNode(initialToGoalVelocityTime, acceleration, goalVelocity)); - leftoverDistance -= operationDistance; - currentVelocity = goalVelocity; - } - } - - // STEP 2: If not at max velocity, build an accelerate->cruise>decelerate nodeMap - if (currentVelocity < maxVelocity) { - // Check /\ (vortex) does the job (or maybe its peak exceeds maxVelocity) - // if not then we need to add a cruising part: /---\ (looks like this) - double halfDistance = leftoverDistance / 2; - double timeForHalf = calculateTimeToGoalPosition(acceleration, currentVelocity, 0, halfDistance); - - // Figure out end velocity - double peakVelocity = currentVelocity + acceleration * timeForHalf; - if (peakVelocity <= maxVelocity) { - // Add two nodes, accelerate to peak, decelerate to goal - nodeMap.add(new SwerveCalculatorNode(timeForHalf, acceleration, peakVelocity)); - nodeMap.add(new SwerveCalculatorNode(timeForHalf, -acceleration, goalVelocity)); - } else { - // Going to peak will not work as it will exceed our maximumVelocity limit - // Go to max, then cruise, then decelerate - double timeFromGoalToMaxVelocity = (maxVelocity - currentVelocity) / acceleration; - double initialPosition = (endPosition - startPosition) - leftoverDistance; - double finalPositionAfterAcceleration = 0.5 * (currentVelocity + maxVelocity) - * timeFromGoalToMaxVelocity + initialPosition; - double positionDelta = finalPositionAfterAcceleration - initialPosition; - double cruiseDistance = leftoverDistance - (positionDelta * 2); - double cruiseTime = cruiseDistance / maxVelocity; - - nodeMap.add(new SwerveCalculatorNode(timeFromGoalToMaxVelocity, acceleration, maxVelocity)); - nodeMap.add(new SwerveCalculatorNode(cruiseTime, 0, maxVelocity)); - nodeMap.add(new SwerveCalculatorNode(timeFromGoalToMaxVelocity, -acceleration, goalVelocity)); - } - return nodeMap; - - } else { - // Cruise til the end if we are at both goal & max velocity! - double cruiseTime = leftoverDistance / currentVelocity; - nodeMap.add(new SwerveCalculatorNode( - cruiseTime, - 0, - currentVelocity - )); - return nodeMap; - } - } - - public List getNodeMap() { - return nodeMap; - } - - public double getVelocityAtFinish() { - SwerveCalculatorNode finalNode = nodeMap.get(nodeMap.size() - 1); - return finalNode.operationEndingVelocity(); - } - - public double getTotalOperationTime() { - double time = 0; - for (SwerveCalculatorNode node : this.nodeMap) { - time += node.operationTime(); - } - return time; - } - - public double getDistanceTravelledInMetersAtTime(double time) { - if (time < 0) { - assertionManager.throwException("Invalid time to getDistanceTravelledInMetersAtTime", new Exception()); - return 0; - } - double elapsedTime = 0; - double totalDistance = 0; - double velocity = initialVelocity; - for (SwerveCalculatorNode node : this.nodeMap) { - // Get amount of time elapsed (no exceed time) - // Add distance with node - double operationTime = node.operationTime(); - if ((time - (operationTime + elapsedTime)) >= 0) { - double distanceTravelled = velocity * operationTime - + 0.5 * node.operationAcceleration() * Math.pow(operationTime, 2); - - totalDistance += distanceTravelled; - velocity = node.operationEndingVelocity(); - elapsedTime += node.operationTime(); - } else { - // Find the amount of time we'll be using the node - operationTime = time - elapsedTime; - double distanceTravelled = velocity * operationTime - + 0.5 * node.operationAcceleration() * Math.pow(operationTime, 2); - totalDistance += distanceTravelled; - break; - } - } - return totalDistance; - } - - // Range: 0 - 1 (not in actual percentages!!!) - public double geDistanceTravelledAtCompletionPercentage(double percentage) { - double time = getTotalOperationTime() * percentage; - return getDistanceTravelledInMetersAtTime(time); - } - - public double getTotalDistanceInMeters() { - return Math.abs(endPosition - startPosition); - } - - public double getVelocityAtDistanceTravelled(double distanceTravelled) { - double totalDistanceTravelled = 0; - double currentVelocity = initialVelocity; - for (SwerveCalculatorNode node : this.nodeMap) { - - // Find amount of distance current node travels - double operationDistance = (currentVelocity * node.operationTime()) + (0.5 - * node.operationAcceleration()) * Math.pow(node.operationTime(), 2); - - // Continue until we land on the node we stop in - if (distanceTravelled - (totalDistanceTravelled + operationDistance) >= 0) { - totalDistanceTravelled += operationDistance; - currentVelocity += (node.operationAcceleration() * node.operationTime()); - } else { - // Accelerate the remaining distance - double operationTime = calculateTimeToGoalPosition( - node.operationAcceleration(), - currentVelocity, - totalDistanceTravelled, - distanceTravelled); - currentVelocity += (node.operationAcceleration() * operationTime); - break; - } - } - return currentVelocity; - } -} diff --git a/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java b/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java deleted file mode 100644 index 38ea27d1e..000000000 --- a/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java +++ /dev/null @@ -1,28 +0,0 @@ -package xbot.common.subsystems.drive; - -// Contains a set of kinematics values -public class SwervePointKinematics { - final double acceleration; // Units: m/s^2 - final double initialVelocity; // Units: m/s - final double goalVelocity; // m/s, velocity you want to be when you reach your goal, may not always be fulfilled. - final double maxVelocity; // m/s - - public SwervePointKinematics(double a, double initialVelocity, double goalVelocity, double maxVelocity) { - this.acceleration = a; - this.initialVelocity = initialVelocity; - this.goalVelocity = Math.max(Math.min(maxVelocity, goalVelocity), -maxVelocity); - this.maxVelocity = maxVelocity; - } - - public SwervePointKinematics kinematicsWithNewVi(double newVi) { - return new SwervePointKinematics(acceleration, newVi, goalVelocity, maxVelocity); - } - - public double getAcceleration() { - return this.acceleration; - } - - public double getMaxVelocity() { - return this.maxVelocity; - } -} diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java index 804a748da..6663d3474 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Twist2d; import xbot.common.command.BaseCommand; -import xbot.common.logging.RobotAssertionManager; import xbot.common.math.XYPair; import xbot.common.properties.PropertyFactory; import xbot.common.subsystems.drive.control_logic.HeadingModule; @@ -25,14 +24,14 @@ public class SwerveSimpleTrajectoryCommand extends BaseCommand { @Inject public SwerveSimpleTrajectoryCommand(BaseSwerveDriveSubsystem drive, BasePoseSubsystem pose, PropertyFactory pf, - HeadingModuleFactory headingModuleFactory, RobotAssertionManager assertionManager) { + HeadingModuleFactory headingModuleFactory) { this.drive = drive; this.pose = pose; headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid()); pf.setPrefix(this); this.addRequirements(drive); - logic = new SwerveSimpleTrajectoryLogic(assertionManager); + logic = new SwerveSimpleTrajectoryLogic(); alternativeIsFinishedSupplier = () -> false; } diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java deleted file mode 100644 index d88553a3b..000000000 --- a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java +++ /dev/null @@ -1,9 +0,0 @@ -package xbot.common.subsystems.drive; - -/* Different ways to render the SwerveSimpleTrajectoryCommand's path */ -public enum SwerveSimpleTrajectoryMode { - ConstantVelocity, // We'll use a constant velocity throughout the course of our travel - DurationInSeconds, // Each point will be given a "duration" - KinematicsForIndividualPoints, // Each individual point will have its own kinematics values it'll go at - KinematicsForPointsList // One kinematics values for the entire route -} diff --git a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java index 171737dfb..d9bb24a29 100644 --- a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java +++ b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java @@ -333,7 +333,6 @@ public void periodic() { isNavXReady = true; } updatePose(); - aKitLog.record("Heading", currentHeading.getDegrees()); } @Override diff --git a/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java b/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java index 666eba99d..156d00621 100644 --- a/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java +++ b/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java @@ -2,7 +2,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import xbot.common.subsystems.drive.SwervePointKinematics; public interface ProvidesInterpolationData { public Translation2d getTranslation2d(); @@ -10,6 +9,4 @@ public interface ProvidesInterpolationData { public double getSecondsForSegment(); public Rotation2d getRotation2d(); - - public SwervePointKinematics getKinematics(); } \ No newline at end of file diff --git a/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java b/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java index e8b8c2875..bd98b522c 100644 --- a/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java +++ b/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java @@ -7,10 +7,6 @@ import xbot.common.advantage.AKitLogger; import xbot.common.controls.sensors.XTimer; -import xbot.common.logging.RobotAssertionManager; -import xbot.common.subsystems.drive.SwerveKinematicsCalculator; -import xbot.common.subsystems.drive.SwervePointKinematics; -import xbot.common.subsystems.drive.SwerveSimpleTrajectoryMode; import java.util.List; public class SimpleTimeInterpolator { @@ -21,13 +17,10 @@ public class SimpleTimeInterpolator { int index; double maximumDistanceFromChasePointInMeters = 0.3; - SwerveKinematicsCalculator calculator; - private List keyPoints; Logger log = LogManager.getLogger(SimpleTimeInterpolator.class); AKitLogger aKitLog = new AKitLogger("SimpleTimeInterpolator/"); - RobotAssertionManager assertionManager; public class InterpolationResult { public Translation2d chasePoint; @@ -78,9 +71,7 @@ public InterpolationResult(Translation2d chasePoint, boolean isOnFinalPoint, Rot } } - public SimpleTimeInterpolator(RobotAssertionManager assertionManager) { - this.assertionManager = assertionManager; - } + public SimpleTimeInterpolator() {} public void setKeyPoints(List keyPoints) { this.keyPoints = keyPoints; @@ -96,23 +87,9 @@ public void initialize(ProvidesInterpolationData baseline) { accumulatedProductiveSeconds = 0; previousTimestamp = XTimer.getFPGATimestamp(); index = 0; - aKitLog.record("freeze proc", false); - } - - public SwerveKinematicsCalculator newCalculator(Translation2d targetPointTranslation2d, SwervePointKinematics kinematics) { - return new SwerveKinematicsCalculator( - assertionManager, - 0, - baseline.getTranslation2d().minus(targetPointTranslation2d).getNorm(), - kinematics - ); } - public void resetCalculator() { - calculator = null; - } - - public InterpolationResult calculateTarget(Translation2d currentLocation, SwerveSimpleTrajectoryMode mode) { + public InterpolationResult calculateTarget(Translation2d currentLocation) { double currentTime = XTimer.getFPGATimestamp(); aKitLog.record("CurrentTime", currentTime); @@ -135,14 +112,6 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve return new InterpolationResult(currentLocation, true, targetKeyPoint.getRotation2d()); } - if ((mode == SwerveSimpleTrajectoryMode.KinematicsForIndividualPoints - || mode == SwerveSimpleTrajectoryMode.KinematicsForPointsList) && calculator == null) { - calculator = newCalculator( - targetKeyPoint.getTranslation2d(), - targetKeyPoint.getKinematics() - ); - } - // First, assume we are just going to our target. (This is what all trajectories will eventually // settle to - all this interpolation is for intermediate points.) Translation2d chasePoint = targetKeyPoint.getTranslation2d(); @@ -153,6 +122,8 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve aKitLog.record("accumulatedProductiveSeconds", accumulatedProductiveSeconds); + + // If the fraction is above 1, it's time to set a new baseline point and start LERPing on the next // one. if (lerpFraction >= 1 && index < keyPoints.size()-1) { @@ -166,34 +137,19 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve targetKeyPoint = keyPoints.get(index); log.info("Baseline is now " + baseline.getTranslation2d() + " and target is now " + targetKeyPoint.getTranslation2d()); - - if (mode == SwerveSimpleTrajectoryMode.KinematicsForIndividualPoints || mode == SwerveSimpleTrajectoryMode.KinematicsForPointsList) { - calculator = newCalculator( - targetKeyPoint.getTranslation2d(), - targetKeyPoint.getKinematics() - ); - } } // Most of the time, the fraction will be less than one. // In that case, we want to interpolate between the baseline and the target. - double multiplier = 1; if (lerpFraction < 1) { - if (mode == SwerveSimpleTrajectoryMode.KinematicsForIndividualPoints || mode == SwerveSimpleTrajectoryMode.KinematicsForPointsList) { - double magnitude = calculator.geDistanceTravelledAtCompletionPercentage(lerpFraction); - multiplier = magnitude / calculator.getTotalDistanceInMeters(); // Magnitude progress - chasePoint = baseline.getTranslation2d().interpolate( - targetKeyPoint.getTranslation2d(), multiplier); - } else { - chasePoint = baseline.getTranslation2d().interpolate( - targetKeyPoint.getTranslation2d(), lerpFraction); - } + chasePoint = baseline.getTranslation2d().interpolate( + targetKeyPoint.getTranslation2d(), lerpFraction); } // But if that chase point is "too far ahead", we need to freeze the chasePoint + // until the robot has a chance to catch up. if (currentLocation.getDistance(chasePoint) > maximumDistanceFromChasePointInMeters) { // This effectively "rewinds time" for the next loop. - aKitLog.record("Freezing ChasePoint", true); accumulatedProductiveSeconds -= secondsSinceLastExecute; } @@ -201,11 +157,11 @@ public InterpolationResult calculateTarget(Translation2d currentLocation, Swerve var plannedVector = targetKeyPoint.getTranslation2d().minus(baseline.getTranslation2d()) .div(targetKeyPoint.getSecondsForSegment()); - plannedVector.times(multiplier > 0 ? multiplier : 0.01); - boolean targetingFinalPoint = index == keyPoints.size()-1 && lerpFraction >= 1; boolean isOnFinalLeg = index == keyPoints.size()-1; return new InterpolationResult(chasePoint, targetingFinalPoint, targetKeyPoint.getRotation2d(), plannedVector, currentLocation.getDistance(targetKeyPoint.getTranslation2d()), lerpFraction, isOnFinalLeg); } + + } \ No newline at end of file diff --git a/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java b/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java index b0deef408..559c07cce 100644 --- a/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java +++ b/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java @@ -5,14 +5,10 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import org.apache.logging.log4j.LogManager; -import org.apache.logging.log4j.Logger; + import xbot.common.advantage.AKitLogger; -import xbot.common.logging.RobotAssertionManager; import xbot.common.math.PIDManager; import xbot.common.math.XYPair; -import xbot.common.subsystems.drive.SwerveKinematicsCalculator; -import xbot.common.subsystems.drive.SwervePointKinematics; -import xbot.common.subsystems.drive.SwerveSimpleTrajectoryMode; import xbot.common.subsystems.drive.control_logic.HeadingModule; import java.util.ArrayList; @@ -21,13 +17,15 @@ public class SwerveSimpleTrajectoryLogic { - Logger log = LogManager.getLogger(this.getClass()); + org.apache.logging.log4j.Logger log = LogManager.getLogger(this.getClass()); final AKitLogger aKitLog = new AKitLogger("SimpleTimeInterpolator/"); private Supplier> keyPointsProvider; private List keyPoints; + private boolean enableConstantVelocity = false; + private double constantVelocity = 0.25; private boolean stopWhenFinished = true; - private final SimpleTimeInterpolator interpolator; + private final SimpleTimeInterpolator interpolator = new SimpleTimeInterpolator(); SimpleTimeInterpolator.InterpolationResult lastResult; double maxPower = 1.0; double maxTurningPower = 1.0; @@ -40,14 +38,9 @@ public class SwerveSimpleTrajectoryLogic { private Pose2d specialAimTarget; private boolean prioritizeRotationIfCloseToGoal = false; private double distanceThresholdToPrioritizeRotation = 1.5; - private double constantVelocity = 0; - private SwervePointKinematics globalKinematics = null; - private SwerveSimpleTrajectoryMode mode = SwerveSimpleTrajectoryMode.DurationInSeconds; - RobotAssertionManager assertionManager; - - public SwerveSimpleTrajectoryLogic(RobotAssertionManager assertionManager) { - this.assertionManager = assertionManager; - interpolator = new SimpleTimeInterpolator(this.assertionManager); + + public SwerveSimpleTrajectoryLogic() { + } // -------------------------------------------------------------- @@ -98,10 +91,16 @@ public void setMaxTurningPower(double maxTurningPower) { this.maxTurningPower = maxTurningPower; } - public List getKeyPoints() { - return this.keyPointsProvider.get(); + public void setEnableConstantVelocity(boolean enableConstantVelocity) { + this.enableConstantVelocity = enableConstantVelocity; + } + + public void setConstantVelocity(double constantVelocity) { + this.constantVelocity = constantVelocity; } + public List getKeyPoints() { return this.keyPointsProvider.get(); } + public List getResolvedKeyPoints() { return keyPoints; } @@ -118,18 +117,6 @@ public void setDistanceThresholdToPrioritizeRotation(double distanceThresholdToP this.distanceThresholdToPrioritizeRotation = distanceThresholdToPrioritizeRotation; } - public void setVelocityMode(SwerveSimpleTrajectoryMode mode) { - this.mode = mode; - } - - public void setConstantVelocity(double constantVelocity) { - this.constantVelocity = constantVelocity; - } - - public void setGlobalKinematicValues(SwervePointKinematics globalKinematics) { - this.globalKinematics = globalKinematics; - } - // -------------------------------------------------------------- // Major Command Elements // -------------------------------------------------------------- @@ -138,7 +125,6 @@ public void reset(Pose2d currentPose) { log.info("Resetting"); keyPoints = keyPointsProvider.get(); log.info("Key points size: " + keyPoints.size()); - interpolator.resetCalculator(); var initialPoint = new XbotSwervePoint(currentPose, 0); @@ -149,7 +135,7 @@ public void reset(Pose2d currentPose) { // Visualize direct raycast var start = new XbotSwervePoint(currentPose.getTranslation(), currentPose.getRotation(), 0); var raycast = XbotSwervePoint.generateTrajectory(List.of( - start, keyPoints.get(0)) + start, keyPoints.get(0)) ); aKitLog.record("Raycast", raycast); @@ -158,28 +144,8 @@ public void reset(Pose2d currentPose) { new Pose2d(targetPoint.getTranslation2d(), targetPoint.getRotation2d())); } - // Adjust points based on mode, since the logic at default is optimized for "DurationInSeconds" we need - // To convert over modes to have values in DurationInSeconds. - switch (mode) { - case DurationInSeconds -> {} // The logic at default is optimized for duration in seconds. - case KinematicsForIndividualPoints -> { - for (XbotSwervePoint point : keyPoints) { - if (point.getKinematics() == null) { - assertionManager.throwException("Needs to set kinematics for swerve point!", new Exception()); - } - } - keyPoints = getKinematicsAdjustedSwervePoints(initialPoint, keyPoints); - } - case KinematicsForPointsList -> { - if (globalKinematics == null) { - assertionManager.throwException("Needs to set globalKinematics!", new Exception()); - } - keyPoints = getGlobalKinematicsAdjustedSwervePoints(initialPoint, keyPoints, currentPose); - } - case ConstantVelocity -> { - keyPoints = getVelocityAdjustedSwervePoints(initialPoint, keyPoints, constantVelocity); - } - default -> assertionManager.throwException("No handling for SwerveSimpleTrajectoryMode!", new Exception()); + if (enableConstantVelocity) { + keyPoints = getVelocityAdjustedSwervePoints(initialPoint, keyPoints, constantVelocity); } handleAimingAtFinalLeg(currentPose); @@ -212,7 +178,8 @@ public void reset(Pose2d currentPose) { } } - aKitLog.record("Trajectory", XbotSwervePoint.generateTrajectory(keyPoints)); + aKitLog.record("Trajectory", + XbotSwervePoint.generateTrajectory(keyPoints)); interpolator.setMaximumDistanceFromChasePointInMeters(0.5); interpolator.setKeyPoints(keyPoints); @@ -296,111 +263,8 @@ private List getVelocityAdjustedSwervePoints( return velocityAdjustedPoints; } - private List getKinematicsAdjustedSwervePoints( - XbotSwervePoint initialPoint, - List swervePoints) { - - ArrayList adjustedPoints = new ArrayList<>(); - - SwerveKinematicsCalculator calculator = null; - for (int i = 0; i < swervePoints.size(); i++) { - XbotSwervePoint previous = initialPoint; - var current = swervePoints.get(i); - if (i > 0) { - // If we've moved on to later points, we can now safely get previous entries in the list. - previous = swervePoints.get(i - 1); - // Calculate the initial velocity of current node - current.setKinematics(current.kinematics.kinematicsWithNewVi(calculator.getVelocityAtFinish())); - } - double distance = previous.getTranslation2d().getDistance(current.getTranslation2d()); - calculator = new SwerveKinematicsCalculator( - assertionManager, - 0, - distance, - current.getKinematics() - ); - double adjustedDuration = calculator.getTotalOperationTime(); - - if (adjustedDuration > 0) { - XbotSwervePoint point = new XbotSwervePoint(current.keyPose, adjustedDuration); - point.setKinematics(current.getKinematics()); - adjustedPoints.add(point); - } - } - - if (adjustedPoints.size() == 0) { - var dummyPoint = new XbotSwervePoint(initialPoint.keyPose, 0.05); - adjustedPoints.add(dummyPoint); - } - - return adjustedPoints; - } - - // Instead of using kinematic values of each individual SwervePoint - // We use kinematic values "globalKinematics" set in this logic - private List getGlobalKinematicsAdjustedSwervePoints( - XbotSwervePoint initialPoint, - List swervePoints, - Pose2d startingPose) { - - ArrayList adjustedPoints = new ArrayList<>(); - - double totalDistance = 0; - Translation2d currentPosition = startingPose.getTranslation(); - for (XbotSwervePoint point : swervePoints) { - totalDistance += currentPosition.getDistance(point.keyPose.getTranslation()); - currentPosition = point.keyPose.getTranslation(); - } - - SwerveKinematicsCalculator calculator = new SwerveKinematicsCalculator( - assertionManager, - 0, - totalDistance, - globalKinematics - ); - - double accumulatedDistance = 0; - for (int i = 0; i < swervePoints.size(); i++) { - XbotSwervePoint previous = initialPoint; - var current = swervePoints.get(i); - if (i > 0) { - // If we've moved on to later points, we can now safely get previous entries in the list. - previous = swervePoints.get(i - 1); - } - - // NEED: acceleration, initialVelocity, finalVelocity, maxVelocity, - // we got a and vMax which is global now we need vInitial and vFinal - double vi = calculator.getVelocityAtDistanceTravelled(accumulatedDistance); - double distance = previous.getTranslation2d().getDistance(current.getTranslation2d()); - accumulatedDistance += distance; - double vf = calculator.getVelocityAtDistanceTravelled(accumulatedDistance); - - SwerveKinematicsCalculator operationCalculator = new SwerveKinematicsCalculator( - assertionManager, - 0, - distance, - new SwervePointKinematics(globalKinematics.getAcceleration(), vi, vf, globalKinematics.getMaxVelocity()) - ); - - double adjustedDuration = operationCalculator.getTotalOperationTime(); - - if (adjustedDuration > 0) { - XbotSwervePoint point = new XbotSwervePoint(current.keyPose, adjustedDuration); - point.setKinematics(new SwervePointKinematics(globalKinematics.getAcceleration(), vi, vf, globalKinematics.getMaxVelocity())); - adjustedPoints.add(point); - } - } - - if (adjustedPoints.size() == 0) { - var dummyPoint = new XbotSwervePoint(initialPoint.keyPose, 0.05); - adjustedPoints.add(dummyPoint); - } - - return adjustedPoints; - } - public XYPair getGoalVector(Pose2d currentPose) { - lastResult = interpolator.calculateTarget(currentPose.getTranslation(), mode); + lastResult = interpolator.calculateTarget(currentPose.getTranslation()); var chasePoint = lastResult.chasePoint; aKitLog.record("chasePoint", new Pose2d(chasePoint, Rotation2d.fromDegrees(0))); @@ -442,7 +306,7 @@ public Twist2d calculatePowers(Pose2d currentPose, PIDManager positionalPid, Hea boolean lastLegAndSpecialAim = enableSpecialAimDuringFinalLeg && lastResult.isOnFinalLeg; - if (specialAimTarget != null && (enableSpecialAimTarget || lastLegAndSpecialAim)) { + if (specialAimTarget != null && (enableSpecialAimTarget || lastLegAndSpecialAim )) { degreeTarget = getAngleBetweenTwoPoints( currentPose.getTranslation(), specialAimTarget.getTranslation() ).getDegrees(); @@ -460,13 +324,12 @@ public Twist2d calculatePowers(Pose2d currentPose, PIDManager positionalPid, Hea } aKitLog.record("UpdatedIntent", intent); - - // In the future we can add different adjustments for if we are ahead/behind in schedule // If we have no max velocity set, or we are on the last point and almost done, just use the position PID if (maximumVelocity <= 0 || (lastResult.isOnFinalPoint && lastResult.distanceToTargetPoint < 0.5) || lastResult.lerpFraction > 1) { return new Twist2d(intent.x, intent.y, headingPower); - - } else { + } + else + { // Otherwise, add the known velocity vector, so we can catch up to our goal. // Get the dot product between the normalized goal vector and the normalized velocity vector. @@ -530,4 +393,6 @@ public boolean recommendIsFinished(Pose2d currentPose, PIDManager positionalPid, public SimpleTimeInterpolator.InterpolationResult getLastResult() { return lastResult; } + + } diff --git a/src/main/java/xbot/common/trajectory/XbotSwervePoint.java b/src/main/java/xbot/common/trajectory/XbotSwervePoint.java index 2112f75d0..681462f61 100644 --- a/src/main/java/xbot/common/trajectory/XbotSwervePoint.java +++ b/src/main/java/xbot/common/trajectory/XbotSwervePoint.java @@ -5,7 +5,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; import xbot.common.math.WrappedRotation2d; -import xbot.common.subsystems.drive.SwervePointKinematics; import xbot.common.subsystems.pose.BasePoseSubsystem; import java.util.ArrayList; @@ -16,7 +15,6 @@ public class XbotSwervePoint implements ProvidesInterpolationData { public Pose2d keyPose; public double secondsToPoint; - SwervePointKinematics kinematics; public XbotSwervePoint(Pose2d keyPose, double secondsToPoint) { this.keyPose = keyPose; @@ -33,13 +31,6 @@ public XbotSwervePoint(double x, double y, double degrees, double secondsToPoint this.secondsToPoint = secondsToPoint; } - public void setKinematics(SwervePointKinematics kinematics) { - this.kinematics = kinematics; - } - - public SwervePointKinematics getKinematics() { - return kinematics; - } public void setPose(Pose2d pose) { this.keyPose = pose; diff --git a/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java b/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java deleted file mode 100644 index d8114335f..000000000 --- a/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java +++ /dev/null @@ -1,125 +0,0 @@ -package xbot.common.subsystems.drive; - -import org.junit.Test; -import xbot.common.injection.BaseCommonLibTest; -import xbot.common.logging.RobotAssertionManager; - -import javax.inject.Inject; -import java.util.ArrayList; -import java.util.List; - -import static org.junit.Assert.assertEquals; - -public class SwerveCalculatorTest extends BaseCommonLibTest { - - RobotAssertionManager assertionManager; - - @Override - public void setUp() { - super.setUp(); - this.assertionManager = getInjectorComponent().robotAssertionManager(); - } - - // startPosition, endPosition, maximumAcceleration, startingVelocity, goalVelocity, maximumVelocity - private SwerveKinematicsCalculator newCalculator(double a, double vInitial, double vGoal, double vMax) { - return new SwerveKinematicsCalculator( - assertionManager, - 0, - 10, - new SwervePointKinematics(a, vInitial, vGoal, vMax) - ); - } - - private void compareNodeMaps(List map1, List map2) { - assertEquals(map1.size(), map2.size()); - for (int i = 0; i < map1.size(); i++) { - assertEquals(map1.get(i).operationTime(), map2.get(i).operationTime(), 0.001); - assertEquals(map1.get(i).operationAcceleration(), map2.get(i).operationAcceleration(), 0.001); - assertEquals(map1.get(i).operationEndingVelocity(), map2.get(i).operationEndingVelocity(), 0.001); - } - } - - @Test - public void testAccelerateMaxThenCruiseThenGoal() { - // Accelerate to max, then cruise, then down to goal - SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 0, 2); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(4, 0.5, 2)); - nodeMap.add(new SwerveCalculatorNode(1, 0, 2)); - nodeMap.add(new SwerveCalculatorNode(4, -0.5, 0)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } - - @Test - public void testAccelerateGoalThenMaxThenCruiseThenGoal() { - // Accelerate to goal, then max, then cruise, then down to goal - SwerveKinematicsCalculator calculator = newCalculator(0.5, 0., 1, 2); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(2, 0.5, 1)); - nodeMap.add(new SwerveCalculatorNode(2, 0.5, 2)); - nodeMap.add(new SwerveCalculatorNode(1.5, 0, 2)); - nodeMap.add(new SwerveCalculatorNode(2, -0.5, 1)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } - - @Test - public void testAccelerateGoalThenAccelerateThenDecelerate() { - // Accelerate to goal, then accelerate, then decelerate, forming a peak, will not reach vMax so no cruise - SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 1, 5); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(2, 0.5, 1)); - nodeMap.add(new SwerveCalculatorNode(2.69042, 0.5, 2.34521)); - nodeMap.add(new SwerveCalculatorNode(2.69042, -0.5, 1)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - - } - - @Test - public void testAccelerateAsMuchAsPossible() { - // Accelerate as much as possible because endingVelocity is impossible to reach - SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 10, 10); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(6.32456, 0.5, 3.16228)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } - - @Test - public void testAccelerateThenCruiseToTheEnd() { - // Accelerate to goal velocity, in this case will be the same as max velocity, then cruise - SwerveKinematicsCalculator calculator = newCalculator(0.5, 0, 3, 3); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(6, 0.5, 3)); - nodeMap.add(new SwerveCalculatorNode(0.33333, 0, 3)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } - - @Test - public void testDecelerateAsMuchAsPossible() { - // Decelerate as much as possible because endingVelocity is impossible to reach - SwerveKinematicsCalculator calculator = newCalculator(0.5, 8, 0, 10); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(1.30306, -0.5, 7.34847)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } - - @Test - public void testAccelerateThenCruiseThenDecelerateToGoal() { - SwerveKinematicsCalculator calculator = newCalculator(0.5, 1, 0, 1.5); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(1, 0.5, 1.5)); - nodeMap.add(new SwerveCalculatorNode(4.33333, 0, 1.5)); - nodeMap.add(new SwerveCalculatorNode(1, -0.5, 1)); - nodeMap.add(new SwerveCalculatorNode(2, -0.5, 0)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } - - @Test - public void testAccelerateThenDecelerateToGoal() { - SwerveKinematicsCalculator calculator = newCalculator(0.5, 1, 0, 10); - List nodeMap = new ArrayList<>(); - nodeMap.add(new SwerveCalculatorNode(2.69042, 0.5, 2.34521)); - nodeMap.add(new SwerveCalculatorNode(2.69042, -0.5, 1)); - nodeMap.add(new SwerveCalculatorNode(2, -0.5, 0)); - compareNodeMaps(nodeMap, calculator.getNodeMap()); - } -}