diff --git a/Assets/choreo.png b/Assets/choreo.png new file mode 100644 index 0000000..95916c9 Binary files /dev/null and b/Assets/choreo.png differ diff --git a/Assets/convex.png b/Assets/convex.png new file mode 100644 index 0000000..d4fe042 Binary files /dev/null and b/Assets/convex.png differ diff --git a/Docs/Specifics/Choreo.md b/Docs/Specifics/Choreo.md new file mode 100644 index 0000000..0dfb8d7 --- /dev/null +++ b/Docs/Specifics/Choreo.md @@ -0,0 +1,175 @@ +# Choreo + +Choreo is a library that helps us create autonomous paths. +Each path is composed of waypoints (places that we would like the robot to hit along the way) and constraints (limitations of the path or robot). +Choreo allows us to find the most optimal path that gets to all those waypoints, while also staying within those constraints. + +## Choreo GUI + +Choreo screenshot + +This is where we create and save the paths. +It's an independent app from Visual Studio or WPILib. +More extensive documentation can be found [here](https://choreo.autos/), but generally you'll just click on the field to add waypoints (usually at pickup/scoring locations), add any constraints from the top toolbar, and click the Generate Path button to generate a file for the path that we can then use. + +So what happens when you hit Generate? + +## Numerical Optimization +You might have done optimization problems in classes like HMA/precalc or Calculus (it's okay if you haven't), where you're trying to find the minimum/maximum amount of something (say I'm trying to maximize my backyard's area) given some constraint (I only have 100 feet of fence). +Choreo uses the same basic idea of modeling our real life problem (and its constraints) with equations and mathematically finding the best solution. +There are infinite ways to go from point A to point B, but we want to find the fastest way for our robot to do so. +In other words, time is a constraint. +We also need to consider other constraints, like our particular robot's maximum acceleration, velocity, motor torque, etc. + +Choreo uses the Sleipnir solver (which in turn uses CasADi, a numerical optimization software) to calculate the fastest path. +At a high level, the solver takes the waypoints and repeatedly "guesses" better ways to interpolate between those waypoints. +(This is what's happening when the path sort of balloons around after you click the generate button). +If you're interested in learning more about how this works under the hood, check out [Sleipnir](https://github.com/SleipnirGroup/Sleipnir). + +## Why Choreo? + +8033 used to use [PathPlanner](https://pathplanner.dev/), which instead uses [Bezier splines](https://en.wikipedia.org/wiki/Bézier_curve). +However, these splines aren't able to take real-life constraints into account, so there would often be a discrepancy between what was "supposed" to work and what actually happened because the spline would require the robot to do something it couldn't physically do. + +## Choreolib Installation + +First, follow the [instructions](https://choreo.autos/choreolib/getting-started/) to add the Choreo vendordep. + +As of 2025, this is how we set up Choreo (and other auto things) in code. +This may change, so leads should ensure they keep this page up to date. +Check the [docs](https://choreo.autos/) for more info. + +There are 3 major files that concern autos: `Autos.java`, `SwerveSubsystem.java`, and `Robot.java`. + +### `Autos.java` + +This file will contain an `AutoFactory`, which is the class Choreo uses to create auto trajectories. +The constructor of this file will look something like this: + +```Java +public Autos( + SwerveSubsystem swerve + // other subsystems + ) { + this.swerve = swerve; + // other subsystems + factory = + new AutoFactory( + swerve::getPose, // A function that returns the current field-relative Pose2d of the robot + swerve::resetPose, // A function that receives a field-relative Pose2d to reset the robot's odometry to. + swerve.choreoDriveController(), // A function that receives the current SampleType and controls the robot. More info below + true, // If this is true, when on the red alliance, the path will be mirrored to the opposite side, while keeping the same coordinate system origin. + swerve, // The drive Subsystem to require for AutoTrajectory Commands. + (traj, edge) -> { // Log trajectories + if (Robot.ROBOT_TYPE != RobotType.REAL) + Logger.recordOutput( + "Choreo/Active Traj", + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get().equals(Alliance.Blue) + ? traj.getPoses() + : traj.flipped().getPoses()); + }); + } +``` +The rest of the file has methods that return a `Command` corresponding to each auto path. +They'll generally look like this: + +```Java + public Command autoPath() { + final var routine = factory.newRoutine("Auto Path"); // Uses the AutoFactory to create a new routine + final var traj = routine.trajectory("AutoPath"); // Uses that routine to load a new trajectory. This should match what the name of the trajectory is in the Choreo app + routine.active().whileTrue(Commands.sequence(traj.resetOdometry(), traj.cmd())); // When the routine starts, the pose will first reset and then the trajectory will be run + // May include other trigger bindings (scoring, intaking, etc) + return routine.cmd(); // Returns command to run this routine + } +``` +Choreolib relies heavily on `Triggers`, which will not be discussed here. +Refer to the [WPILib docs](https://docs.wpilib.org/en/stable/docs/software/commandbased/binding-commands-to-triggers.html) for more *or potentially another article here?* + +You'll notice one of the parameters above is the `choreoDriveController()` method. +This is in `SwerveSubsystem`. + +### `SwerveSubsystem.java` + +The `SwerveSample` class is used by Choreo to represent the robot's state (position, velocity, acceleration, and the forces applied to each swerve module) at a point in time along the trajectory. +The `choreoDriveController()` method returns a function that "consumes" (takes as a parameter) a `SwerveSample` and drives to it. +Because different teams might have different ways of doing that driving (for example we've added logging statements), the implementation is left up to teams. + +```Java + +/** + * This function bypasses the command-based framework because Choreolib handles setting + * requirements internally. Do NOT use outside of ChoreoLib + * + * @return a Consumer that runs the drivebase to follow a SwerveSample with PID feedback, sample + * target vel feedforward, and module force feedforward. + */ + @SuppressWarnings("resource") // This is here because otherwise it gets angry about the pid controllers not being closed + public Consumer choreoDriveController() { + // PID controllers for x, y, and theta + final PIDController xController = new PIDController(5.0, 0.0, 0.0); + final PIDController yController = new PIDController(5.0, 0.0, 0.0); + final PIDController thetaController = + new PIDController(constants.getHeadingVelocityKP(), 0.0, 0.0); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + return (sample) -> { // returns a new function that takes in a sample and then does all the stuff inside the brackets + final var pose = getPose(); // Gets the current pose of the robot on the field + + Logger.recordOutput( + "Choreo/Target Pose", + new Pose2d(sample.x, sample.y, Rotation2d.fromRadians(sample.heading))); + Logger.recordOutput( + "Choreo/Target Speeds Field Relative", + new ChassisSpeeds(sample.vx, sample.vy, sample.omega)); + + var feedback = + new ChassisSpeeds( + xController.calculate(pose.getX(), sample.x), + yController.calculate(pose.getY(), sample.y), + thetaController.calculate(pose.getRotation().getRadians(), sample.heading)); // Calculates how to get from its current pose to the target pose in the sample + var speeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + new ChassisSpeeds(sample.vx, sample.vy, sample.omega).plus(feedback), getRotation()); // Adds what we just calculated ^ to the chassis speeds specified by the sample + + Logger.recordOutput("Choreo/Feedback + FF Target Speeds Robot Relative", speeds); + + // Drives with the chassis speeds we just calculated + this.drive( + speeds, + false, + useModuleForceFF ? sample.moduleForcesX() : new double[4], + useModuleForceFF ? sample.moduleForcesY() : new double[4]); + }; + } +``` + +### `Robot.java` + +We have several different auto options, so picking one is handled in `Robot.java` with the `LoggedDashboardChooser` which puts a dropdown list on the dashboard. +We then add all the methods in `Autos.java` that return our auto paths to that chooser in the `addAutos()` method, which is triggered on startup or by an alliance change. + +```Java +private final Autos autos; +private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Autos"); + +public Robot() { + //... + RobotModeTriggers.autonomous() + .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); // Starts whatever auto command is selected in the chooser once the autonomous period starts + + new Trigger( + () -> { // alliance changing stuff + var allianceChange = !DriverStation.getAlliance().equals(lastAlliance); + lastAlliance = DriverStation.getAlliance(); + return allianceChange && DriverStation.getAlliance().isPresent(); + }) + .onTrue( + Commands.runOnce(() -> addAutos())); +} + +private void addAutos() { + autoChooser.addOption("Auto Path", autos.getAutoPath()); // adds all our auto options to the chooser so they can be selected + //... +} +``` \ No newline at end of file diff --git a/Docs/Specifics/image.png b/Docs/Specifics/image.png new file mode 100644 index 0000000..4412468 Binary files /dev/null and b/Docs/Specifics/image.png differ