diff --git a/build.gradle b/build.gradle index 99dd8433..8f1d9c68 100644 --- a/build.gradle +++ b/build.gradle @@ -111,7 +111,7 @@ task(generateDriveTrajectories, dependsOn: "classes", type: JavaExec) { mainClass = "org.littletonrobotics.frc2024.subsystems.drive.trajectory.GenerateTrajectories" classpath = sourceSets.main.runtimeClasspath } -//compileJava.finalizedBy generateDriveTrajectories +compileJava.finalizedBy generateDriveTrajectories // Check robot type when deploying task(checkRobot, dependsOn: "classes", type: JavaExec) { @@ -171,6 +171,7 @@ dependencies { implementation "io.grpc:grpc-stub:${grpcVersion}" implementation "javax.annotation:javax.annotation-api:1.3.2" runtimeOnly "io.grpc:grpc-netty-shaded:${grpcVersion}" + implementation "com.google.guava:guava:31.1-jre" def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/src/main/java/org/littletonrobotics/frc2024/Constants.java b/src/main/java/org/littletonrobotics/frc2024/Constants.java index 61e4f76c..0949b15a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Constants.java +++ b/src/main/java/org/littletonrobotics/frc2024/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.DEVBOT; + private static RobotType robotType = RobotType.SIMBOT; public static final boolean tuningMode = true; private static boolean invalidRobotAlertSent = false; diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index adb2bb3b..3cc8a521 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -16,18 +16,14 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.io.File; -import java.util.Objects; -import java.util.Optional; -import java.util.function.Function; import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization; +import org.littletonrobotics.frc2024.commands.auto.AutoCommands; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionConstants; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO; @@ -51,8 +47,6 @@ import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOKrakenFOC; import org.littletonrobotics.frc2024.subsystems.superstructure.intake.IntakeIOSim; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; -import org.littletonrobotics.frc2024.util.trajectory.ChoreoTrajectoryDeserializer; -import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -168,6 +162,7 @@ public RobotContainer() { } autoChooser = new LoggedDashboardChooser<>("Auto Choices"); + autoChooser.addDefaultOption("Do Nothing", Commands.none()); // Set up feedforward characterization autoChooser.addOption( "Drive FF Characterization", @@ -188,24 +183,30 @@ public RobotContainer() { flywheels::getRightCharacterizationVelocity)); autoChooser.addOption("Arm get static current", arm.getStaticCurrent()); + AutoCommands autoCommands = new AutoCommands(drive, superstructure); + + autoChooser.addOption("Drive Straight", autoCommands.driveStraight()); + // Testing autos paths - Function> trajectoryCommandFactory = - trajectoryFile -> { - Optional trajectory = - ChoreoTrajectoryDeserializer.deserialize(trajectoryFile); - return trajectory.map( - traj -> - Commands.runOnce( - () -> robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) - .andThen(Commands.runOnce(() -> drive.setTrajectoryGoal(traj))) - .until(drive::isTrajectoryGoalCompleted)); - }; - final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); - for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { - trajectoryCommandFactory - .apply(trajectoryFile) - .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); - } + // Function> trajectoryCommandFactory = + // trajectoryFile -> { + // Optional trajectory = + // ChoreoTrajectoryDeserializer.deserialize(trajectoryFile); + // return trajectory.map( + // traj -> + // Commands.runOnce( + // () -> + // robotState.resetPose(AllianceFlipUtil.apply(traj.getStartPose()))) + // .andThen(Commands.runOnce(() -> + // drive.setTrajectoryGoal(traj))) + // .until(drive::isTrajectoryGoalCompleted)); + // }; + // final File rootTrajectoryDir = new File(Filesystem.getDeployDirectory(), "choreo"); + // for (File trajectoryFile : Objects.requireNonNull(rootTrajectoryDir.listFiles())) { + // trajectoryCommandFactory + // .apply(trajectoryFile) + // .ifPresent(command -> autoChooser.addOption(trajectoryFile.getName(), command)); + // } // Configure the button bindings configureButtonBindings(); diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java index 2de63fcc..674cc08e 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/auto/AutoCommands.java @@ -1,50 +1,44 @@ package org.littletonrobotics.frc2024.commands.auto; -import edu.wpi.first.math.geometry.Translation2d; +import static edu.wpi.first.wpilibj2.command.Commands.*; + import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import java.util.function.Supplier; import org.littletonrobotics.frc2024.RobotState; +import org.littletonrobotics.frc2024.subsystems.drive.Drive; +import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure; +import org.littletonrobotics.frc2024.util.AllianceFlipUtil; +import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory; public class AutoCommands { + private Drive drive; + private Superstructure superstructure; - public static boolean inRegion(Supplier region) { - return region.get().contains(RobotState.getInstance().getEstimatedPose().getTranslation()); - } - - public static Command waitForRegion(Supplier region) { - return Commands.waitUntil(() -> inRegion(region)); + public AutoCommands(Drive drive, Superstructure superstructure) { + this.drive = drive; + this.superstructure = superstructure; } - public interface Region { - boolean contains(Translation2d point); + private Command path(String pathName) { + HolonomicTrajectory trajectory = new HolonomicTrajectory(pathName); + + return startEnd( + () -> { + drive.setTrajectoryGoal(trajectory); + }, + () -> { + drive.clearTrajectoryGoal(); + }) + .until(() -> drive.isTrajectoryGoalCompleted()); } - public static class RectangularRegion implements Region { - public final Translation2d topLeft; - public final Translation2d bottomRight; - - public RectangularRegion(Translation2d topLeft, Translation2d bottomRight) { - this.topLeft = topLeft; - this.bottomRight = bottomRight; - } - - public RectangularRegion(Translation2d center, double width, double height) { - topLeft = new Translation2d(center.getX() - width / 2, center.getY() + height / 2); - bottomRight = new Translation2d(center.getX() + width / 2, center.getY() - height / 2); - } - - public boolean contains(Translation2d point) { - return point.getX() >= topLeft.getX() - && point.getX() <= bottomRight.getX() - && point.getY() <= topLeft.getY() - && point.getY() >= bottomRight.getY(); - } + private Command reset(String path) { + HolonomicTrajectory trajectory = new HolonomicTrajectory(path); + return runOnce( + () -> + RobotState.getInstance().resetPose(AllianceFlipUtil.apply(trajectory.getStartPose()))); } - public record CircularRegion(Translation2d center, double radius) implements Region { - public boolean contains(Translation2d point) { - return center.getDistance(point) <= radius; - } + public Command driveStraight() { + return reset("driveStraight").andThen(path("driveStraight")); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java index 786899df..d9a0b915 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/controllers/TrajectoryController.java @@ -1,10 +1,10 @@ package org.littletonrobotics.frc2024.subsystems.drive.controllers; import static org.littletonrobotics.frc2024.subsystems.drive.DriveConstants.trajectoryConstants; +import static org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; @@ -26,25 +26,6 @@ public class TrajectoryController { new LoggedTunableNumber("Trajectory/thetakP", trajectoryConstants.thetakP()); private static LoggedTunableNumber trajectoryThetakD = new LoggedTunableNumber("Trajectory/thetakD", trajectoryConstants.thetakD()); - private static LoggedTunableNumber trajectoryLinearTolerance = - new LoggedTunableNumber( - "Trajectory/controllerLinearTolerance", trajectoryConstants.linearTolerance()); - private static LoggedTunableNumber trajectoryThetaTolerance = - new LoggedTunableNumber( - "Trajectory/controllerThetaTolerance", trajectoryConstants.thetaTolerance()); - private static LoggedTunableNumber trajectoryGoalLinearTolerance = - new LoggedTunableNumber( - "Trajectory/goalLinearTolerance", trajectoryConstants.goalLinearTolerance()); - private static LoggedTunableNumber trajectoryGoalThetaTolerance = - new LoggedTunableNumber( - "Trajectory/goalThetaTolerance", trajectoryConstants.goalThetaTolerance()); - private static LoggedTunableNumber trajectoryLinearVelocityTolerance = - new LoggedTunableNumber( - "Trajectory/goalLinearVelocityTolerance", trajectoryConstants.linearVelocityTolerance()); - private static LoggedTunableNumber trajectoryAngularVelocityTolerance = - new LoggedTunableNumber( - "Trajectory/goalAngularVelocityTolerance", - trajectoryConstants.angularVelocityTolerance()); private HolonomicDriveController controller; private HolonomicTrajectory trajectory; @@ -58,8 +39,7 @@ public TrajectoryController(HolonomicTrajectory trajectory) { trajectoryLinearkD.get(), trajectoryThetakP.get(), trajectoryThetakD.get()); - controller.setGoalState(AllianceFlipUtil.apply(trajectory.getEndState())); - configTrajectoryTolerances(); + startTime = Timer.getFPGATimestamp(); // Log poses Logger.recordOutput( @@ -78,31 +58,34 @@ public ChassisSpeeds update() { trajectoryLinearkD, trajectoryThetakP, trajectoryThetakP); - // Tolerances - LoggedTunableNumber.ifChanged( - hashCode(), - this::configTrajectoryTolerances, - trajectoryLinearTolerance, - trajectoryThetaTolerance, - trajectoryGoalLinearTolerance, - trajectoryGoalThetaTolerance, - trajectoryLinearVelocityTolerance, - trajectoryAngularVelocityTolerance); // Run trajectory Pose2d currentPose = RobotState.getInstance().getEstimatedPose(); Twist2d fieldVelocity = RobotState.getInstance().fieldVelocity(); double sampletime = Timer.getFPGATimestamp() - startTime; - HolonomicTrajectory.State currentState = - new HolonomicTrajectory.State( - sampletime, currentPose, fieldVelocity.dx, fieldVelocity.dy, fieldVelocity.dtheta); + VehicleState currentState = + VehicleState.newBuilder() + .setX(currentPose.getTranslation().getX()) + .setY(currentPose.getTranslation().getY()) + .setTheta(currentPose.getRotation().getRadians()) + .setVx(fieldVelocity.dx) + .setVy(fieldVelocity.dy) + .setOmega(fieldVelocity.dtheta) + .build(); // Sample and flip state - HolonomicTrajectory.State setpointState = trajectory.sample(sampletime); + VehicleState setpointState = trajectory.sample(sampletime); setpointState = AllianceFlipUtil.apply(setpointState); + // calculate trajectory speeds ChassisSpeeds speeds = controller.calculate(currentState, setpointState); // Log trajectory data - Logger.recordOutput("Trajectory/SetpointPose", setpointState.pose()); + Logger.recordOutput( + "Trajectory/SetpointPose", + new Pose2d( + setpointState.getX(), setpointState.getY(), new Rotation2d(setpointState.getTheta()))); + Logger.recordOutput("Trajectory/SetpointSpeeds/vx", setpointState.getVx()); + Logger.recordOutput("Trajectory/SetpointSpeeds/vy", setpointState.getVy()); + Logger.recordOutput("Trajectory/SetpointSpeeds/omega", setpointState.getOmega()); Logger.recordOutput("Trajectory/FieldRelativeSpeeds", speeds); return speeds; } @@ -119,27 +102,6 @@ public Rotation2d getRotationError() { @AutoLogOutput(key = "Trajectory/Finished") public boolean isFinished() { - return Timer.getFPGATimestamp() - startTime > trajectory.getDuration() - && controller.atGoal() - && controller.atSetpoint(); - } - - private void configTrajectoryTolerances() { - controller.setControllerTolerance( - new Pose2d( - new Translation2d( - trajectoryLinearTolerance.get(), Rotation2d.fromRadians(Math.PI / 4.0)), - Rotation2d.fromRadians(trajectoryThetaTolerance.get()))); - double velocityXTolerance = trajectoryLinearVelocityTolerance.get() / Math.sqrt(2.0); - controller.setGoalTolerance( - new HolonomicTrajectory.State( - 0.0, - new Pose2d( - new Translation2d( - trajectoryGoalLinearTolerance.get(), Rotation2d.fromRadians(Math.PI / 4.0)), - Rotation2d.fromRadians(trajectoryGoalThetaTolerance.get())), - velocityXTolerance, - velocityXTolerance, // Same value - trajectoryAngularVelocityTolerance.get())); + return Timer.getFPGATimestamp() - startTime > trajectory.getDuration(); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java index 45aa572b..a45318c3 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/DriveTrajectories.java @@ -4,15 +4,23 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import java.util.HashMap; import java.util.List; +import java.util.Map; import lombok.experimental.ExtensionMethod; @ExtensionMethod({TrajectoryGenerationHelpers.class}) public class DriveTrajectories { - public static final List driveStraight = - List.of( - PathSegment.newBuilder() - .addPoseWaypoint(new Pose2d()) - .addPoseWaypoint(new Pose2d(2.0, 0.0, new Rotation2d()), 100) - .build()); + public static final Map> paths = new HashMap<>(); + + static { + paths.put( + "driveStraight", + List.of( + PathSegment.newBuilder() + .addPoseWaypoint(new Pose2d()) + .addPoseWaypoint(new Pose2d(2.0, 0.0, new Rotation2d()), 100) + .addPoseWaypoint(new Pose2d(4.0, 2.0, new Rotation2d(Math.PI))) + .build())); + } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java index f6e8215d..41d80708 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/trajectory/GenerateTrajectories.java @@ -2,8 +2,17 @@ import static org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.*; +import com.google.common.hash.Hashing; import io.grpc.Grpc; import io.grpc.InsecureChannelCredentials; +import java.io.*; +import java.math.RoundingMode; +import java.nio.charset.StandardCharsets; +import java.nio.file.Path; +import java.text.DecimalFormat; +import java.util.HashMap; +import java.util.List; +import java.util.Map; import org.littletonrobotics.frc2024.Constants; import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants; import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceGrpc; @@ -11,10 +20,8 @@ public class GenerateTrajectories { public static void main(String[] args) { Constants.disableHAL(); - var channel = - Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build(); - var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel); + // Create vehicle model VehicleModel model = VehicleModel.newBuilder() .setMass(70) @@ -28,13 +35,116 @@ public static void main(String[] args) { / DriveConstants.driveConfig.wheelRadius()) .build(); - PathRequest request = - PathRequest.newBuilder() - .setModel(model) - .addAllSegments(DriveTrajectories.driveStraight) - .build(); + // Check hashcodes + Map> pathQueue = new HashMap<>(); + for (Map.Entry> entry : DriveTrajectories.paths.entrySet()) { + String hashCode = getHashCode(model, entry.getValue()); + File pathFile = + Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile(); + if (pathFile.exists()) { + try { + InputStream fileStream = new FileInputStream(pathFile); + Trajectory trajectory = Trajectory.parseFrom(fileStream); + // trajectory.getStatesC + if (!trajectory.getHashCode().equals(hashCode)) { + pathQueue.put(entry.getKey(), entry.getValue()); + } + } catch (IOException e) { + e.printStackTrace(); + } + } else { + pathQueue.put(entry.getKey(), entry.getValue()); + } + } + + // Exit if trajectories up-to-date + if (pathQueue.isEmpty()) { + return; + } + + // Connect to service + var channel = + Grpc.newChannelBuilder("127.0.0.1:56328", InsecureChannelCredentials.create()).build(); + var service = VehicleTrajectoryServiceGrpc.newBlockingStub(channel); + + // Generate trajectories + for (Map.Entry> entry : pathQueue.entrySet()) { + PathRequest request = + PathRequest.newBuilder().setModel(model).addAllSegments(entry.getValue()).build(); + + TrajectoryResponse response = service.generateTrajectory(request); + + String responseHashCode = getHashCode(model, entry.getValue()); + + response = + response.toBuilder() + .setTrajectory( + response.getTrajectory().toBuilder().setHashCode(responseHashCode).build()) + .build(); + + File pathFile = + Path.of("src", "main", "deploy", "trajectories", entry.getKey() + ".pathblob").toFile(); + + try { + OutputStream fileStream = new FileOutputStream(pathFile); + System.out.println("Writing to " + pathFile.getAbsolutePath()); + response.getTrajectory().writeTo(fileStream); + } catch (IOException e) { + e.printStackTrace(); + } + } + } + + // create a hashcode for the vehicle model and path segments + + private static String getHashCode(VehicleModel model, List segements) { + StringBuilder hashString = new StringBuilder(); + + DecimalFormat format = new DecimalFormat("#.000000"); + format.setRoundingMode(RoundingMode.HALF_DOWN); + + hashString.append(format.format(model.getMass())); + hashString.append(format.format(model.getMoi())); + hashString.append(format.format(model.getVehicleLength())); + hashString.append(format.format(model.getVehicleWidth())); + hashString.append(format.format(model.getWheelRadius())); + hashString.append(format.format(model.getMaxWheelTorque())); + hashString.append(format.format(model.getMaxWheelOmega())); + + for (PathSegment segment : segements) { + for (Waypoint waypoint : segment.getWaypointsList()) { + hashString.append(format.format(waypoint.getX())); + hashString.append(format.format(waypoint.getY())); + if (waypoint.hasHeadingConstraint()) { + hashString.append(format.format(waypoint.getHeadingConstraint())); + } + + if (waypoint.hasSamples()) { + hashString.append(waypoint.getSamples()); + } + + switch (waypoint.getVelocityConstraintCase()) { + case ZERO_VELOCITY -> { + hashString.append(format.format(0)); + } + case VEHICLE_VELOCITY -> { + hashString.append(format.format(waypoint.getVehicleVelocity().getVx())); + hashString.append(format.format(waypoint.getVehicleVelocity().getVy())); + hashString.append(format.format(waypoint.getVehicleVelocity().getOmega())); + } + } + } + + if (segment.hasMaxVelocity()) { + hashString.append(format.format(segment.getMaxVelocity())); + } + if (segment.hasMaxOmega()) { + hashString.append(format.format(segment.getMaxOmega())); + } + + hashString.append(segment.getStraightLine()); + } - TrajectoryResponse response = service.generateTrajectory(request); - System.out.println(response.toString()); + return Hashing.sha256().hashString(hashString, StandardCharsets.UTF_8).toString(); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java index f4efb69f..4be6f237 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/AllianceFlipUtil.java @@ -7,8 +7,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import org.littletonrobotics.frc2024.FieldConstants; -import org.littletonrobotics.frc2024.commands.auto.AutoCommands; -import org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory; +import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.VehicleState; /** Utility functions for flipping from the blue to red alliance. */ public class AllianceFlipUtil { @@ -60,43 +59,21 @@ public static Translation3d apply(Translation3d translation3d) { /** * Flips a trajectory state to the correct side of the field based on the current alliance color. */ - public static HolonomicTrajectory.State apply(HolonomicTrajectory.State state) { + public static VehicleState apply(VehicleState state) { if (shouldFlip()) { - return new HolonomicTrajectory.State( - state.timeSeconds(), - apply(state.pose()), - -state.velocityX(), - state.velocityY(), - -state.angularVelocity()); + return VehicleState.newBuilder() + .setX(apply(state.getX())) + .setY(state.getY()) + .setTheta(apply(new Rotation2d(state.getTheta())).getRadians()) + .setVx(-state.getVx()) + .setVy(state.getVy()) + .setOmega(state.getOmega()) + .build(); } else { return state; } } - /** - * Flip rectangular region to the correct side of the field based on the current alliance color - */ - public static AutoCommands.RectangularRegion apply(AutoCommands.RectangularRegion region) { - if (shouldFlip()) { - Translation2d topRight = apply(region.topLeft); - Translation2d bottomLeft = apply(region.bottomRight); - return new AutoCommands.RectangularRegion( - new Translation2d(bottomLeft.getX(), topRight.getY()), - new Translation2d(topRight.getX(), bottomLeft.getY())); - } else { - return region; - } - } - - /** Flip circular region to the correct side of the field based on the current alliance color */ - public static AutoCommands.CircularRegion apply(AutoCommands.CircularRegion region) { - if (shouldFlip()) { - return new AutoCommands.CircularRegion(apply(region.center()), region.radius()); - } else { - return region; - } - } - public static boolean shouldFlip() { return DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/ChoreoTrajectoryDeserializer.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/ChoreoTrajectoryDeserializer.java deleted file mode 100644 index 5593310c..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/ChoreoTrajectoryDeserializer.java +++ /dev/null @@ -1,132 +0,0 @@ -package org.littletonrobotics.frc2024.util.trajectory; - -import static org.littletonrobotics.frc2024.util.trajectory.HolonomicTrajectory.State; - -import com.fasterxml.jackson.core.type.TypeReference; -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import java.io.File; -import java.io.IOException; -import java.util.ArrayList; -import java.util.List; -import java.util.Optional; - -public final class ChoreoTrajectoryDeserializer { - private static ObjectMapper objectMapper = new ObjectMapper(); - - /** Load Trajectory file (.traj) */ - public static Optional deserialize(File file) { - List choreoStates = new ArrayList<>(); - try { - choreoStates = - objectMapper.convertValue( - objectMapper.readTree(file).get("samples"), new TypeReference<>() {}); - } catch (IOException e) { - System.out.println("Failed to read states from file"); - return Optional.empty(); - } - - // Generate trajectory object - return Optional.of(generateTrajectoryImplementation(choreoStates)); - } - - private static HolonomicTrajectory generateTrajectoryImplementation( - final List states) { - return new HolonomicTrajectory() { - @Override - public double getDuration() { - return states.get(states.size() - 1).timestamp(); - } - - @Override - public Pose2d getStartPose() { - ChoreoTrajectoryState start = states.get(0); - return new Pose2d(start.x(), start.y(), new Rotation2d(start.heading())); - } - - @Override - public Pose2d[] getTrajectoryPoses() { - return states.stream() - .map(state -> new Pose2d(state.x(), state.y(), new Rotation2d(state.heading()))) - .toArray(Pose2d[]::new); - } - - @Override - public State getStartState() { - return fromChoreoState(states.get(0)); - } - - @Override - public State getEndState() { - return fromChoreoState(states.get(states.size() - 1)); - } - - @Override - public State sample(double timeSeconds) { - ChoreoTrajectoryState before = null; - ChoreoTrajectoryState after = null; - - for (ChoreoTrajectoryState state : states) { - if (state.timestamp() == timeSeconds) { - return fromChoreoState(state); - } - - if (state.timestamp() < timeSeconds) { - before = state; - } else { - after = state; - break; - } - } - - if (before == null) { - return fromChoreoState(states.get(0)); - } - - if (after == null) { - return fromChoreoState(states.get(states.size() - 1)); - } - - double s = (timeSeconds - before.timestamp()) / (after.timestamp() - before.timestamp()); - - State beforeState = fromChoreoState(before); - State afterState = fromChoreoState(after); - - Pose2d interpolatedPose = beforeState.pose().interpolate(afterState.pose(), s); - double interpolatedVelocityX = - MathUtil.interpolate(beforeState.velocityX(), afterState.velocityX(), s); - double interpolatedVelocityY = - MathUtil.interpolate(beforeState.velocityY(), afterState.velocityY(), s); - double interpolatedAngularVelocity = - MathUtil.interpolate(beforeState.angularVelocity(), afterState.angularVelocity(), s); - - return new State( - timeSeconds, - interpolatedPose, - interpolatedVelocityX, - interpolatedVelocityY, - interpolatedAngularVelocity); - } - }; - } - - private static State fromChoreoState(ChoreoTrajectoryState state) { - return new State( - state.timestamp(), - new Pose2d(state.x(), state.y(), new Rotation2d(state.heading())), - state.velocityX(), - state.velocityY(), - state.angularVelocity()); - } - - record ChoreoTrajectoryState( - double x, - double y, - double heading, - double angularVelocity, - double velocityX, - double velocityY, - double timestamp) {} -} diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java index 8d37d6a1..c65409a2 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicDriveController.java @@ -1,26 +1,17 @@ package org.littletonrobotics.frc2024.util.trajectory; +import static org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.*; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import lombok.Getter; -import lombok.Setter; public class HolonomicDriveController { private final PIDController linearController; private final PIDController thetaController; - private HolonomicTrajectory.State currentState = null; - - /** -- SETTER -- Set tolerance for goal state */ - @Setter private HolonomicTrajectory.State goalTolerance = null; - - private Pose2d controllerTolerance = null; - - /** -- SETTER -- Set goal state */ - @Setter private HolonomicTrajectory.State goalState = null; - @Getter private Pose2d poseError; public HolonomicDriveController( @@ -53,59 +44,32 @@ public void setPID(double linearkP, double linearkD, double thetakP, double thet } /** Calculate robot relative chassis speeds */ - public ChassisSpeeds calculate( - HolonomicTrajectory.State currentState, HolonomicTrajectory.State setpointState) { - this.currentState = currentState; - Pose2d setpointPose = setpointState.pose(); - poseError = setpointPose.relativeTo(currentState.pose()); + public ChassisSpeeds calculate(VehicleState currentState, VehicleState setpointState) { + Pose2d currentPose = + new Pose2d( + currentState.getX(), currentState.getY(), new Rotation2d(currentState.getTheta())); + Pose2d setpointPose = + new Pose2d( + setpointState.getX(), setpointState.getY(), new Rotation2d(setpointState.getTheta())); + poseError = setpointPose.relativeTo(currentPose); // Calculate feedback velocities (based on position error). double linearFeedback = linearController.calculate( - 0, currentState.pose().getTranslation().getDistance(setpointPose.getTranslation())); + 0, currentPose.getTranslation().getDistance(setpointPose.getTranslation())); Rotation2d currentToStateAngle = - setpointPose.getTranslation().minus(currentState.pose().getTranslation()).getAngle(); + setpointPose.getTranslation().minus(currentPose.getTranslation()).getAngle(); double xFeedback = linearFeedback * currentToStateAngle.getCos(); double yFeedback = linearFeedback * currentToStateAngle.getSin(); double thetaFeedback = thetaController.calculate( - currentState.pose().getRotation().getRadians(), - setpointPose.getRotation().getRadians()); - if (atGoal()) { - if (linearController.atSetpoint()) { - xFeedback = 0; - yFeedback = 0; - } - if (thetaController.atSetpoint()) { - thetaFeedback = 0; - } - } + currentPose.getRotation().getRadians(), setpointPose.getRotation().getRadians()); // Return next output. return ChassisSpeeds.fromFieldRelativeSpeeds( - setpointState.velocityX() + xFeedback, - setpointState.velocityY() + yFeedback, - setpointState.angularVelocity() + thetaFeedback, - currentState.pose().getRotation()); - } - - public boolean atSetpoint() { - return linearController.atSetpoint() && thetaController.atSetpoint(); - } - - /** Within tolerance of current goal */ - public boolean atGoal() { - Pose2d goalPoseError = goalState.pose().relativeTo(currentState.pose()); - boolean withinPoseTolerance = - goalPoseError.getTranslation().getNorm() <= goalTolerance.pose().getTranslation().getNorm() - && Math.abs(goalPoseError.getRotation().getRadians()) - <= goalTolerance.pose().getRotation().getRadians(); - boolean withinVelocityTolerance = - Math.abs(currentState.velocityX() - goalState.velocityX()) < goalTolerance.velocityX() - && Math.abs(currentState.velocityY() - goalState.velocityY()) - < goalTolerance.velocityY() - && Math.abs(currentState.angularVelocity() - goalState.angularVelocity()) - < goalTolerance.angularVelocity(); - return withinPoseTolerance && withinVelocityTolerance; + setpointState.getVx() + xFeedback, + setpointState.getVy() + yFeedback, + setpointState.getOmega() + thetaFeedback, + currentPose.getRotation()); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java index f6c16064..33d2b06a 100644 --- a/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java +++ b/src/main/java/org/littletonrobotics/frc2024/util/trajectory/HolonomicTrajectory.java @@ -1,24 +1,113 @@ package org.littletonrobotics.frc2024.util.trajectory; +import static org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.*; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import java.io.File; +import java.io.FileInputStream; +import java.io.IOException; +import java.io.InputStream; +import java.nio.file.Path; +import org.littletonrobotics.vehicletrajectoryservice.VehicleTrajectoryServiceOuterClass.VehicleState; + +public class HolonomicTrajectory { + private final Trajectory trajectory; + + public HolonomicTrajectory(String name) { + File file = Path.of("src", "main", "deploy", "trajectories", name + ".pathblob").toFile(); + try { + InputStream fileStream = new FileInputStream(file); + trajectory = Trajectory.parseFrom(fileStream); + } catch (IOException e) { + System.err.println("Could not load trajectory \"" + name + "\""); + throw new RuntimeException(); + } + } + + public double getDuration() { + return trajectory.getStates(trajectory.getStatesCount() - 1).getTime(); + } + ; + + public Pose2d getStartPose() { + VehicleState startState = getStartState(); + return new Pose2d(startState.getX(), startState.getY(), new Rotation2d(startState.getTheta())); + } + + public Pose2d[] getTrajectoryPoses() { + Pose2d[] poses = new Pose2d[trajectory.getStatesCount()]; + + for (int i = 0; i < trajectory.getStatesCount(); i++) { + VehicleState state = trajectory.getStates(i).getState(); + poses[i] = new Pose2d(state.getX(), state.getY(), new Rotation2d(state.getTheta())); + } + return poses; + } + ; + + public VehicleState getStartState() { + return trajectory.getStates(0).getState(); + } + + public VehicleState getEndState() { + return trajectory.getStates(trajectory.getStatesCount() - 1).getState(); + } + + public VehicleState sample(double timeSeconds) { + TimestampedVehicleState before = null; + TimestampedVehicleState after = null; + + for (TimestampedVehicleState state : trajectory.getStatesList()) { + if (state.getTime() == timeSeconds) { + return state.getState(); + } -public interface HolonomicTrajectory { - double getDuration(); + if (state.getTime() < timeSeconds) { + before = state; + } else { + after = state; + break; + } + } - Pose2d getStartPose(); + if (before == null) { + return trajectory.getStates(0).getState(); + } - Pose2d[] getTrajectoryPoses(); + if (after == null) { + return trajectory.getStates(trajectory.getStatesCount() - 1).getState(); + } - State getStartState(); + double s = (timeSeconds - before.getTime()) / (after.getTime() - before.getTime()); - State getEndState(); + Pose2d beforePose = + new Pose2d( + before.getState().getX(), + before.getState().getY(), + new Rotation2d(before.getState().getTheta())); + Pose2d afterPose = + new Pose2d( + after.getState().getX(), + after.getState().getY(), + new Rotation2d(after.getState().getTheta())); - State sample(double timeSeconds); + Pose2d interpolatedPose = beforePose.interpolate(afterPose, s); + double interpolatedVelocityX = + MathUtil.interpolate(before.getState().getVx(), after.getState().getVx(), s); + double interpolatedVelocityY = + MathUtil.interpolate(before.getState().getVy(), after.getState().getVy(), s); + double interpolatedAngularVelocity = + MathUtil.interpolate(before.getState().getOmega(), after.getState().getOmega(), s); - record State( - double timeSeconds, - Pose2d pose, - double velocityX, - double velocityY, - double angularVelocity) {} + return VehicleState.newBuilder() + .setX(interpolatedPose.getTranslation().getX()) + .setY(interpolatedPose.getTranslation().getY()) + .setTheta(interpolatedPose.getRotation().getRadians()) + .setVx(interpolatedVelocityX) + .setVy(interpolatedVelocityY) + .setOmega(interpolatedAngularVelocity) + .build(); + } } diff --git a/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java b/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java new file mode 100644 index 00000000..b07bb3a6 --- /dev/null +++ b/src/test/java/org/littletonrobotics/frc2024/RobotContainerTest.java @@ -0,0 +1,12 @@ +package org.littletonrobotics.frc2024; + +import org.junit.jupiter.api.Test; + +public class RobotContainerTest { + + @Test + public void createRobotContainer() { + // Instantiate RobotContainer + new RobotContainer(); + } +} diff --git a/trajectory_native/src/trajectory_service.cpp b/trajectory_native/src/trajectory_service.cpp index 057fe509..ce6a2f8e 100644 --- a/trajectory_native/src/trajectory_service.cpp +++ b/trajectory_native/src/trajectory_service.cpp @@ -74,65 +74,6 @@ void convert_trajectory(vts::Trajectory *trajectory_out, const trajopt::Holonomi } } -std::string hash_request(const vts::PathRequest &request) { - // Function is somewhat messy but this is the simplest way I could think of to hash the request with - // fixed precision while accounting for optional fields - - std::stringstream stream; - stream << std::fixed << std::setprecision(6); - stream << request.model().mass(); - stream << request.model().moi(); - stream << request.model().vehicle_length(); - stream << request.model().vehicle_width(); - stream << request.model().wheel_radius(); - stream << request.model().max_wheel_omega(); - stream << request.model().max_wheel_torque(); - - for (const vts::PathSegment &segment: request.segments()) { - for (const vts::Waypoint &waypoint: segment.waypoints()) { - stream << waypoint.x(); - stream << waypoint.y(); - - if (waypoint.has_heading_constraint()) { - stream << waypoint.heading_constraint(); - } - - if (waypoint.has_samples()) { - stream << waypoint.samples(); - } - - switch (waypoint.velocity_constraint_case()) { - case vts::Waypoint::kZeroVelocity: - stream << "0"; - break; - case vts::Waypoint::kVehicleVelocity: - stream << waypoint.vehicle_velocity().vx(); - stream << waypoint.vehicle_velocity().vy(); - stream << waypoint.vehicle_velocity().omega(); - break; - case org::littletonrobotics::vehicletrajectoryservice::Waypoint::VELOCITY_CONSTRAINT_NOT_SET: - break; - } - } - - if (segment.has_max_velocity()) { - stream << segment.max_velocity(); - } - - if (segment.has_max_omega()) { - stream << segment.max_omega(); - } - - if (segment.straight_line()) { - stream << "1"; - } else { - stream << "0"; - } - } - - return std::to_string(std::hash{}(stream.str())); -} - class VehicleTrajectoryService final : public vts::VehicleTrajectoryService::Service { public: @@ -253,7 +194,6 @@ class VehicleTrajectoryService final trajopt::HolonomicTrajectory trajectory{trajopt::OptimalTrajectoryGenerator::Generate(builder)}; fmt::print("Generation finished\n"); convert_trajectory(response->mutable_trajectory(), trajectory); - response->mutable_trajectory()->set_hash_code(hash_request(*request)); } catch (std::exception &e) { fmt::print("Generation failed: {}\n", std::string(e.what())); response->mutable_error()->set_reason(std::string(e.what()));