Get rid of constraint abstraction

Author: jonahsnider

src/main/java/frc/robot/autos/trailblazer/AutoPoint.java

@@ -3,47 +3,52 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.autos.trailblazer.constraints.AutoPointConstraint;
-import java.util.List;
+import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions;
+import java.util.Optional;
 import java.util.function.Supplier;
 
 public class AutoPoint {
   public final Supplier<Pose2d> poseSupplier;
-  public final List<AutoPointConstraint> constraints;
+  public final Optional<AutoConstraintOptions> constraints;
   public final Command command;
 
   public AutoPoint(
-      Supplier<Pose2d> poseSupplier, Command command, List<AutoPointConstraint> constraints) {
+      Supplier<Pose2d> poseSupplier, Command command, Optional<AutoConstraintOptions> constraints) {
     this.poseSupplier = poseSupplier;
     this.command = command;
     this.constraints = constraints;
   }
 
+  public AutoPoint(
+      Supplier<Pose2d> poseSupplier, Command command, AutoConstraintOptions constraints) {
+    this(poseSupplier, command, Optional.of(constraints));
+  }
+
   public AutoPoint(Supplier<Pose2d> poseSupplier, Command command) {
-    this(poseSupplier, command, List.of());
+    this(poseSupplier, command, Optional.empty());
   }
 
-  public AutoPoint(Supplier<Pose2d> poseSupplier, List<AutoPointConstraint> constraints) {
+  public AutoPoint(Supplier<Pose2d> poseSupplier, AutoConstraintOptions constraints) {
     this(poseSupplier, Commands.none(), constraints);
   }
 
   public AutoPoint(Supplier<Pose2d> poseSupplier) {
-    this(poseSupplier, Commands.none(), List.of());
+    this(poseSupplier, Commands.none(), Optional.empty());
   }
 
-  public AutoPoint(Pose2d pose, Command command, List<AutoPointConstraint> constraints) {
+  public AutoPoint(Pose2d pose, Command command, AutoConstraintOptions constraints) {
     this(() -> pose, command, constraints);
   }
 
   public AutoPoint(Pose2d pose, Command command) {
-    this(pose, command, List.of());
+    this(() -> pose, command);
   }
 
-  public AutoPoint(Pose2d pose, List<AutoPointConstraint> constraints) {
+  public AutoPoint(Pose2d pose, AutoConstraintOptions constraints) {
     this(pose, Commands.none(), constraints);
   }
 
   public AutoPoint(Pose2d pose) {
-    this(pose, Commands.none(), List.of());
+    this(pose, Commands.none());
   }
 }

src/main/java/frc/robot/autos/trailblazer/AutoSegment.java

@@ -1,6 +1,6 @@
 package frc.robot.autos.trailblazer;
 
-import frc.robot.autos.trailblazer.constraints.AutoPointConstraint;
+import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions;
 import java.util.List;
 
 /**
@@ -13,22 +13,18 @@ public class AutoSegment {
    * Constraints to apply to any points that don't have their own constraints specified. If a point
    * specifies its own constraints, this field will be ignored.
    */
-  public final List<AutoPointConstraint> defaultConstraints;
+  public final AutoConstraintOptions defaultConstraints;
 
-  public AutoSegment(List<AutoPointConstraint> defaultConstraints, List<AutoPoint> points) {
+  public AutoSegment(AutoConstraintOptions defaultConstraints, List<AutoPoint> points) {
     this.defaultConstraints = defaultConstraints;
     this.points = points;
   }
 
-  public AutoSegment(List<AutoPointConstraint> defaultConstraints, AutoPoint... points) {
+  public AutoSegment(AutoConstraintOptions defaultConstraints, AutoPoint... points) {
     this(defaultConstraints, List.of(points));
   }
 
-  public AutoSegment(List<AutoPoint> points) {
-    this(List.of(), points);
-  }
-
   public AutoSegment(AutoPoint... points) {
-    this(List.of(), points);
+    this(new AutoConstraintOptions(), points);
   }
 }

src/main/java/frc/robot/autos/trailblazer/Trailblazer.java

@@ -5,33 +5,23 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.autos.trailblazer.constraints.AutoPointConstraint;
+import frc.robot.autos.trailblazer.constraints.AutoConstraintCalculator;
+import frc.robot.autos.trailblazer.constraints.AutoConstraintOptions;
 import frc.robot.autos.trailblazer.followers.PathFollower;
 import frc.robot.autos.trailblazer.followers.PidPathFollower;
 import frc.robot.autos.trailblazer.trackers.HeuristicPathTracker;
 import frc.robot.autos.trailblazer.trackers.PathTracker;
 import frc.robot.localization.LocalizationSubsystem;
 import frc.robot.swerve.SwerveSubsystem;
-import java.util.List;
 
 public class Trailblazer {
   /**
-   * Global default constraints, which will be applied to points that have no constraints specified,
-   * and no default constraints specified for the segment.
+   * Given a point and the constraints for its parent segment, resolve the constraint options to use
+   * while following that point.
    */
-  private static final List<AutoPointConstraint> globalDefaultConstraints = List.of();
-
-  private static List<AutoPointConstraint> resolveConstraints(
-      List<AutoPointConstraint> segmentConstraints, AutoPoint point) {
-    if (!point.constraints.isEmpty()) {
-      return point.constraints;
-    }
-
-    if (!segmentConstraints.isEmpty()) {
-      return segmentConstraints;
-    }
-
-    return globalDefaultConstraints;
+  private static AutoConstraintOptions resolveConstraints(
+      AutoPoint point, AutoConstraintOptions segmentConstraints) {
+    return point.constraints.orElse(segmentConstraints);
   }
 
   private final SwerveSubsystem swerve;
@@ -53,6 +43,7 @@ public Command followSegment(AutoSegment segment) {
             Commands.run(
                 () -> {
                   var currentAutoPoint = pathTracker.getCurrentPoint();
+
                   var constrainedVelocityGoal =
                       getSwerveSetpoint(currentAutoPoint, segment.defaultConstraints);
                   swerve.setFieldRelativeAutoSpeeds(constrainedVelocityGoal);
@@ -67,16 +58,16 @@ public Command followSegment(AutoSegment segment) {
   }
 
   private ChassisSpeeds getSwerveSetpoint(
-      AutoPoint point, List<AutoPointConstraint> segmentConstraints) {
-    var usedConstraints = resolveConstraints(segmentConstraints, point);
+      AutoPoint point, AutoConstraintOptions segmentConstraints) {
+    var usedConstraints = resolveConstraints(point, segmentConstraints);
 
     var currentPose = localization.getPose();
     var fieldRelativeRobotSpeeds = swerve.getFieldRelativeSpeeds();
     var originalTargetPose = pathTracker.getTargetPose(currentPose, fieldRelativeRobotSpeeds);
     var constrainedTargetPose =
-        AutoPointConstraint.applyPoseConstraints(originalTargetPose, usedConstraints);
+        AutoConstraintCalculator.constrainTargetPose(originalTargetPose, usedConstraints);
 
     var originalVelocityGoal = pathFollower.calculateSpeeds(currentPose, constrainedTargetPose);
-    return AutoPointConstraint.applyMotionConstraints(originalVelocityGoal, usedConstraints);
+    return AutoConstraintCalculator.constrainVelocityGoal(originalVelocityGoal, usedConstraints);
   }
 }

src/main/java/frc/robot/autos/trailblazer/constraints/AutoAccelerationConstraint.java

@@ -0,0 +1,37 @@
+package frc.robot.autos.trailblazer.constraints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+public class AutoConstraintCalculator {
+  public static Pose2d constrainTargetPose(Pose2d inputPose, AutoConstraintOptions options) {
+    if (options.collisionAvoidance()) {
+      // TODO: Implement collision avoidance
+    }
+
+    return inputPose;
+  }
+
+  public static ChassisSpeeds constrainVelocityGoal(
+      ChassisSpeeds inputSpeeds, AutoConstraintOptions options) {
+    if (options.maxLinearVelocity() != 0) {
+      // TODO: Implement linear velocity constraint
+    }
+
+    if (options.maxAngularVelocity() != 0) {
+      // TODO: Implement angular velocity constraint
+    }
+
+    if (options.maxLinearAcceleration() != 0) {
+      // TODO: Implement linear acceleration constraint
+    }
+
+    if (options.maxAngularAcceleration() != 0) {
+      // TODO: Implement angular acceleration constraint
+    }
+
+    return inputSpeeds;
+  }
+
+  private AutoConstraintCalculator() {}
+}

src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java

@@ -0,0 +1,63 @@
+package frc.robot.autos.trailblazer.constraints;
+
+public record AutoConstraintOptions(
+    /** Whether collision avoidance should be enabled. */
+    boolean collisionAvoidance,
+    /** Max linear velocity allowed in meters per second. Set to 0 to disable. */
+    double maxLinearVelocity,
+    /** Max angular velocity allowed in radians per second. Set to 0 to disable. */
+    double maxAngularVelocity,
+    /** Max linear acceleration allowed in meters per second squared. Set to 0 to disable. */
+    double maxLinearAcceleration,
+    /** Max angular acceleration allowed in radians per second squared. Set to 0 to disable. */
+    double maxAngularAcceleration) {
+  /** Default constraint options to use if no point or segment specific options are set. */
+  public AutoConstraintOptions() {
+    this(false, 0, 0, 0, 0);
+  }
+
+  public AutoConstraintOptions withCollisionAvoidance(boolean collisionAvoidance) {
+    return new AutoConstraintOptions(
+        collisionAvoidance,
+        maxLinearVelocity(),
+        maxAngularVelocity(),
+        maxLinearAcceleration(),
+        maxAngularAcceleration());
+  }
+
+  public AutoConstraintOptions withMaxLinearVelocity(double maxLinearVelocity) {
+    return new AutoConstraintOptions(
+        collisionAvoidance(),
+        maxLinearVelocity,
+        maxAngularVelocity(),
+        maxLinearAcceleration(),
+        maxAngularAcceleration());
+  }
+
+  public AutoConstraintOptions withMaxAngularVelocity(double maxAngularVelocity) {
+    return new AutoConstraintOptions(
+        collisionAvoidance(),
+        maxLinearVelocity(),
+        maxAngularVelocity,
+        maxLinearAcceleration(),
+        maxAngularAcceleration());
+  }
+
+  public AutoConstraintOptions withMaxLinearAcceleration(double maxLinearAcceleration) {
+    return new AutoConstraintOptions(
+        collisionAvoidance(),
+        maxLinearVelocity(),
+        maxAngularVelocity(),
+        maxLinearAcceleration,
+        maxAngularAcceleration());
+  }
+
+  public AutoConstraintOptions withMaxAngularAcceleration(double maxAngularAcceleration) {
+    return new AutoConstraintOptions(
+        collisionAvoidance(),
+        maxLinearVelocity(),
+        maxAngularVelocity(),
+        maxLinearAcceleration(),
+        maxAngularAcceleration);
+  }
+}