From 8ae3bcbc2a4512529ca40e598c379f1682a50b61 Mon Sep 17 00:00:00 2001 From: Rongrui Zhu <134637512+Rongrrz@users.noreply.github.com> Date: Sat, 18 Jan 2025 10:34:18 -0800 Subject: [PATCH] Swerve simple trajectory update (#485) * FRCRobotTemplate SCL update * End of day 10/19 * End of day 10/26 * Node map approach Hopefully this covers all the possibilities... * Checkstyle fix * End of day 11/02/2024 MM/DD/YY * Removing constant velocity * Remove constant velocity * Revert "Merge branch 'SwerveSimpleTrajectory-update' of https://github.com/Team488/SeriouslyCommonLib into SwerveSimpleTrajectory-update" This reverts commit ea9d1a0695d77d6f3778815ba98d26f0f6c17cd8, reversing changes made to 574d5957128aa2439f58e76b454a91ce4fd8234e. * Revert "Remove constant velocity" This reverts commit 574d5957128aa2439f58e76b454a91ce4fd8234e. * Working swerve with acceleration values * Duration fix * Deleted unnecessary comments * Added kinematics mode to SwerveSimpleTrajectoryCommand * Temporary checkstyle fix * Simplified kinematic values into a class * Swerve global kinematics mode * Tests for SwerveKinematicsCalculator * Minimize deceleration time Changed so that instead of cruising in deceleration, will accelerate to minimize time as well. * End of day 12/07 GlobalKinematics not working, look into it! * Kinematics bug fix * Clean up More concise variables names and cleaned comments * Resolving PR comments part 1 * Fixed overshooting bug * Checkstyle fix * RobotAssertionManager and clean up * Uncommented calculator test * More edge case checks --------- Co-authored-by: JohnGilb --- .../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, 694 insertions(+), 39 deletions(-) create mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java create mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java create mode 100644 src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java create mode 100644 src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java create 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 new file mode 100644 index 000000000..ebae44dd8 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/drive/SwerveCalculatorNode.java @@ -0,0 +1,12 @@ +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 new file mode 100644 index 000000000..bb75971b2 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/drive/SwerveKinematicsCalculator.java @@ -0,0 +1,288 @@ +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 new file mode 100644 index 000000000..38ea27d1e --- /dev/null +++ b/src/main/java/xbot/common/subsystems/drive/SwervePointKinematics.java @@ -0,0 +1,28 @@ +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 6663d3474..804a748da 100644 --- a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java +++ b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryCommand.java @@ -2,6 +2,7 @@ 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; @@ -24,14 +25,14 @@ public class SwerveSimpleTrajectoryCommand extends BaseCommand { @Inject public SwerveSimpleTrajectoryCommand(BaseSwerveDriveSubsystem drive, BasePoseSubsystem pose, PropertyFactory pf, - HeadingModuleFactory headingModuleFactory) { + HeadingModuleFactory headingModuleFactory, RobotAssertionManager assertionManager) { this.drive = drive; this.pose = pose; headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid()); pf.setPrefix(this); this.addRequirements(drive); - logic = new SwerveSimpleTrajectoryLogic(); + logic = new SwerveSimpleTrajectoryLogic(assertionManager); alternativeIsFinishedSupplier = () -> false; } diff --git a/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java new file mode 100644 index 000000000..d88553a3b --- /dev/null +++ b/src/main/java/xbot/common/subsystems/drive/SwerveSimpleTrajectoryMode.java @@ -0,0 +1,9 @@ +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 d9bb24a29..171737dfb 100644 --- a/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java +++ b/src/main/java/xbot/common/subsystems/pose/BasePoseSubsystem.java @@ -333,6 +333,7 @@ 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 156d00621..666eba99d 100644 --- a/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java +++ b/src/main/java/xbot/common/trajectory/ProvidesInterpolationData.java @@ -2,6 +2,7 @@ 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(); @@ -9,4 +10,6 @@ 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 bd98b522c..e8b8c2875 100644 --- a/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java +++ b/src/main/java/xbot/common/trajectory/SimpleTimeInterpolator.java @@ -7,6 +7,10 @@ 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 { @@ -17,10 +21,13 @@ 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; @@ -71,7 +78,9 @@ public InterpolationResult(Translation2d chasePoint, boolean isOnFinalPoint, Rot } } - public SimpleTimeInterpolator() {} + public SimpleTimeInterpolator(RobotAssertionManager assertionManager) { + this.assertionManager = assertionManager; + } public void setKeyPoints(List keyPoints) { this.keyPoints = keyPoints; @@ -87,9 +96,23 @@ 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 InterpolationResult calculateTarget(Translation2d currentLocation) { + public void resetCalculator() { + calculator = null; + } + + public InterpolationResult calculateTarget(Translation2d currentLocation, SwerveSimpleTrajectoryMode mode) { double currentTime = XTimer.getFPGATimestamp(); aKitLog.record("CurrentTime", currentTime); @@ -112,6 +135,14 @@ public InterpolationResult calculateTarget(Translation2d currentLocation) { 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(); @@ -122,8 +153,6 @@ public InterpolationResult calculateTarget(Translation2d currentLocation) { 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) { @@ -137,19 +166,34 @@ public InterpolationResult calculateTarget(Translation2d currentLocation) { 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) { - chasePoint = baseline.getTranslation2d().interpolate( - targetKeyPoint.getTranslation2d(), lerpFraction); + 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); + } } // 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; } @@ -157,11 +201,11 @@ public InterpolationResult calculateTarget(Translation2d currentLocation) { 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 559c07cce..b0deef408 100644 --- a/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java +++ b/src/main/java/xbot/common/trajectory/SwerveSimpleTrajectoryLogic.java @@ -5,10 +5,14 @@ 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; @@ -17,15 +21,13 @@ public class SwerveSimpleTrajectoryLogic { - org.apache.logging.log4j.Logger log = LogManager.getLogger(this.getClass()); + 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 = new SimpleTimeInterpolator(); + private final SimpleTimeInterpolator interpolator; SimpleTimeInterpolator.InterpolationResult lastResult; double maxPower = 1.0; double maxTurningPower = 1.0; @@ -38,9 +40,14 @@ public class SwerveSimpleTrajectoryLogic { private Pose2d specialAimTarget; private boolean prioritizeRotationIfCloseToGoal = false; private double distanceThresholdToPrioritizeRotation = 1.5; - - public SwerveSimpleTrajectoryLogic() { - + 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); } // -------------------------------------------------------------- @@ -91,16 +98,10 @@ public void setMaxTurningPower(double maxTurningPower) { this.maxTurningPower = maxTurningPower; } - 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 getKeyPoints() { return this.keyPointsProvider.get(); } - public List getResolvedKeyPoints() { return keyPoints; } @@ -117,6 +118,18 @@ 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 // -------------------------------------------------------------- @@ -125,6 +138,7 @@ 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); @@ -135,7 +149,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); @@ -144,8 +158,28 @@ public void reset(Pose2d currentPose) { new Pose2d(targetPoint.getTranslation2d(), targetPoint.getRotation2d())); } - if (enableConstantVelocity) { - keyPoints = getVelocityAdjustedSwervePoints(initialPoint, keyPoints, constantVelocity); + // 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()); } handleAimingAtFinalLeg(currentPose); @@ -178,8 +212,7 @@ public void reset(Pose2d currentPose) { } } - aKitLog.record("Trajectory", - XbotSwervePoint.generateTrajectory(keyPoints)); + aKitLog.record("Trajectory", XbotSwervePoint.generateTrajectory(keyPoints)); interpolator.setMaximumDistanceFromChasePointInMeters(0.5); interpolator.setKeyPoints(keyPoints); @@ -263,8 +296,111 @@ 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()); + lastResult = interpolator.calculateTarget(currentPose.getTranslation(), mode); var chasePoint = lastResult.chasePoint; aKitLog.record("chasePoint", new Pose2d(chasePoint, Rotation2d.fromDegrees(0))); @@ -306,7 +442,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(); @@ -324,12 +460,13 @@ 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. @@ -393,6 +530,4 @@ 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 681462f61..2112f75d0 100644 --- a/src/main/java/xbot/common/trajectory/XbotSwervePoint.java +++ b/src/main/java/xbot/common/trajectory/XbotSwervePoint.java @@ -5,6 +5,7 @@ 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; @@ -15,6 +16,7 @@ public class XbotSwervePoint implements ProvidesInterpolationData { public Pose2d keyPose; public double secondsToPoint; + SwervePointKinematics kinematics; public XbotSwervePoint(Pose2d keyPose, double secondsToPoint) { this.keyPose = keyPose; @@ -31,6 +33,13 @@ 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 new file mode 100644 index 000000000..d8114335f --- /dev/null +++ b/src/test/java/xbot/common/subsystems/drive/SwerveCalculatorTest.java @@ -0,0 +1,125 @@ +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()); + } +}