feat: add distance-based path follower; fix interpolate bugs

pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPCrossTrackHolonomicController.java

@@ -0,0 +1,146 @@
+package com.pathplanner.lib.controllers;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState;
+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.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+/**
+ * Holonomic path-following controller that applies cross-track PD on the perpendicular-to-tangent
+ * error rather than per-axis PID, plus a curvature feedforward to anticipate centripetal drift on
+ * curves. The tangent-direction velocity comes from {@code targetState.fieldSpeeds} as a
+ * feedforward; rotation is closed-loop PID against the target holonomic rotation.
+ *
+ * <p>Designed to be paired with {@link com.pathplanner.lib.commands.FollowPathDistanceCommand},
+ * which samples the trajectory by arc length and feeds a projected target state to this controller.
+ *
+ * <p>Differences vs {@link PPHolonomicDriveController}:
+ *
+ * <ul>
+ *   <li>Cross-track (1D, perpendicular to path) PD instead of independent x/y PID, so the
+ *       controller doesn't fight the planned tangent-direction velocity
+ *   <li>Optional curvature feedforward to reduce steady-state lateral error on curves
+ * </ul>
+ */
+public class PPCrossTrackHolonomicController implements PathFollowingController {
+  private final double crossTrackKp;
+  private final double crossTrackKd;
+  private final PIDController rotationController;
+  private final double curvatureFfGain;
+  private final double period;
+
+  private double lastCrossTrackError = 0.0;
+  private boolean hasLastError = false;
+
+  /**
+   * Construct a cross-track holonomic controller with full configuration.
+   *
+   * @param crossTrackConstants PID constants for cross-track correction. Only kP and kD are used
+   *     (cross-track is 1D and reset every cycle, so kI/iZone are ignored).
+   * @param rotationConstants PID constants for the rotation controller. All fields used.
+   * @param curvatureFfGain Centripetal feedforward gain, in seconds. Applied as a
+   *     perpendicular-to-tangent velocity offset of magnitude {@code gain * v^2 * kappa} to counter
+   *     outward drift on curves. Set to 0 to disable.
+   * @param period Control-loop period in seconds.
+   */
+  public PPCrossTrackHolonomicController(
+      PIDConstants crossTrackConstants,
+      PIDConstants rotationConstants,
+      double curvatureFfGain,
+      double period) {
+    this.crossTrackKp = crossTrackConstants.kP;
+    this.crossTrackKd = crossTrackConstants.kD;
+    this.rotationController =
+        new PIDController(rotationConstants.kP, rotationConstants.kI, rotationConstants.kD, period);
+    this.rotationController.setIntegratorRange(-rotationConstants.iZone, rotationConstants.iZone);
+    this.rotationController.enableContinuousInput(-Math.PI, Math.PI);
+    this.curvatureFfGain = curvatureFfGain;
+    this.period = period;
+  }
+
+  /**
+   * Construct a cross-track holonomic controller with a default 20 ms period.
+   *
+   * @param crossTrackConstants Cross-track PD constants (kI/iZone ignored).
+   * @param rotationConstants Rotation PID constants.
+   * @param curvatureFfGain Curvature feedforward gain in seconds.
+   */
+  public PPCrossTrackHolonomicController(
+      PIDConstants crossTrackConstants, PIDConstants rotationConstants, double curvatureFfGain) {
+    this(crossTrackConstants, rotationConstants, curvatureFfGain, 0.02);
+  }
+
+  /**
+   * Construct a cross-track holonomic controller with the tuning values validated on the reference
+   * FRC swerve: crossTrackKp=3.0, crossTrackKd=0.5, rotationKp=5.0, curvatureFfGain=0.1.
+   *
+   * @return Controller with default tuning
+   */
+  public static PPCrossTrackHolonomicController defaults() {
+    return new PPCrossTrackHolonomicController(
+        new PIDConstants(3.0, 0.0, 0.5), new PIDConstants(5.0, 0.0, 0.0), 0.1);
+  }
+
+  @Override
+  public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
+    rotationController.reset();
+    lastCrossTrackError = 0.0;
+    hasLastError = false;
+  }
+
+  @Override
+  public ChassisSpeeds calculateRobotRelativeSpeeds(
+      Pose2d currentPose, PathPlannerTrajectoryState targetState) {
+    // Tangent direction of travel along the path at the target sample.
+    Rotation2d tangent = targetState.heading;
+    double tx = tangent.getCos();
+    double ty = tangent.getSin();
+    // Left-perpendicular to tangent (90 deg CCW).
+    double nx = -ty;
+    double ny = tx;
+
+    // Signed cross-track error: positive if robot is left of the path tangent.
+    Translation2d delta = currentPose.getTranslation().minus(targetState.pose.getTranslation());
+    double crossTrackError = delta.getX() * nx + delta.getY() * ny;
+
+    // Finite-difference rate. Initialized to 0 on the first sample to avoid a spurious kick.
+    double crossTrackRate;
+    if (hasLastError) {
+      crossTrackRate = (crossTrackError - lastCrossTrackError) / period;
+    } else {
+      crossTrackRate = 0.0;
+    }
+    lastCrossTrackError = crossTrackError;
+    hasLastError = true;
+
+    // Cross-track correction pulls the robot back toward the path (so subtract the error).
+    double crossTrackCorrection = -(crossTrackKp * crossTrackError + crossTrackKd * crossTrackRate);
+
+    // Curvature FF anticipates centripetal drift: extra perpendicular velocity = gain * v^2 * kappa
+    double v = targetState.linearVelocity;
+    double curvatureFf = curvatureFfGain * v * v * targetState.curvatureRadPerMeter;
+
+    double perpVelocity = crossTrackCorrection + curvatureFf;
+
+    // Sum tangent FF velocity + perpendicular correction (field frame).
+    double vxField = targetState.fieldSpeeds.vxMetersPerSecond + perpVelocity * nx;
+    double vyField = targetState.fieldSpeeds.vyMetersPerSecond + perpVelocity * ny;
+
+    // Heading PID against target holonomic rotation, plus rotational FF from planned omega.
+    double rotationFeedback =
+        rotationController.calculate(
+            currentPose.getRotation().getRadians(), targetState.pose.getRotation().getRadians());
+    double omega = targetState.fieldSpeeds.omegaRadiansPerSecond + rotationFeedback;
+
+    return ChassisSpeeds.fromFieldRelativeSpeeds(
+        vxField, vyField, omega, currentPose.getRotation());
+  }
+
+  @Override
+  public boolean isHolonomic() {
+    return true;
+  }
+}

pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java

@@ -34,6 +34,7 @@ public PathPlannerTrajectory(List<PathPlannerTrajectoryState> states, List<Event
     this.states = states;
     this.events = events;
     populateDistanceAlongPath(this.states);
+    populateCurvature(this.states);
   }
 
   /**
@@ -52,6 +53,35 @@ private static void populateDistanceAlongPath(List<PathPlannerTrajectoryState> s
     }
   }
 
+  /**
+   * Walk the state list and assign each state's signed path curvature in radians per meter (1/m),
+   * computed geometrically from the three adjacent state positions. Endpoints get zero. Sign
+   * convention matches {@link com.pathplanner.lib.util.GeometryUtil#calculateRadius}: positive
+   * curvature corresponds to a left turn. Works for any construction path, including Choreo
+   * trajectories.
+   */
+  private static void populateCurvature(List<PathPlannerTrajectoryState> states) {
+    int n = states.size();
+    if (n < 3) {
+      for (var s : states) s.curvatureRadPerMeter = 0.0;
+      return;
+    }
+    states.get(0).curvatureRadPerMeter = 0.0;
+    states.get(n - 1).curvatureRadPerMeter = 0.0;
+    for (int i = 1; i < n - 1; i++) {
+      double signedRadius =
+          GeometryUtil.calculateRadius(
+              states.get(i - 1).pose.getTranslation(),
+              states.get(i).pose.getTranslation(),
+              states.get(i + 1).pose.getTranslation());
+      if (!Double.isFinite(signedRadius) || Math.abs(signedRadius) < 1e-9) {
+        states.get(i).curvatureRadPerMeter = 0.0;
+      } else {
+        states.get(i).curvatureRadPerMeter = 1.0 / signedRadius;
+      }
+    }
+  }
+
   /**
    * Create a trajectory with pre-generated states
    *
@@ -233,6 +263,7 @@ public PathPlannerTrajectory(
     // Populate cumulative arc length from pose positions. Works for both the Choreo branch
     // (states come from the ideal-trajectory cache) and the generated branch.
     populateDistanceAlongPath(this.states);
+    populateCurvature(this.states);
   }
 
   private static void generateStates(

pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java

@@ -24,6 +24,11 @@ public class PathPlannerTrajectoryState implements Interpolatable<PathPlannerTra
   public Rotation2d heading = Rotation2d.kZero;
   /** The cumulative arc length traveled along the path to reach this state, in meters */
   public double distanceAlongPath = 0.0;
+  /**
+   * The signed path curvature at this state in radians per meter (1/m). Positive curves left,
+   * negative curves right. Used by curvature-feedforward path-following controllers.
+   */
+  public double curvatureRadPerMeter = 0.0;
 
   /** The feedforwards for each module */
   public DriveFeedforwards feedforwards;
@@ -68,34 +73,48 @@ public PathPlannerTrajectoryState interpolate(PathPlannerTrajectoryState endVal,
             MathUtil.interpolate(
                 fieldSpeeds.omegaRadiansPerSecond, endVal.fieldSpeeds.omegaRadiansPerSecond, t));
 
+    // heading is the chord direction (state[k] -> state[k+1]) and is constant along a segment.
+    // Integration below uses this start-state heading throughout, which keeps the integrated
+    // position on the chord rather than drifting toward the underlying curve. Callers reading
+    // targetState.heading see the current-segment chord direction, which is the correct
+    // tangent for perpendicular cross-track measurement.
     lerpedState.heading = heading;
     lerpedState.linearVelocity = MathUtil.interpolate(linearVelocity, endVal.linearVelocity, t);
     lerpedState.distanceAlongPath =
         MathUtil.interpolate(distanceAlongPath, endVal.distanceAlongPath, t);
+    lerpedState.curvatureRadPerMeter =
+        MathUtil.interpolate(curvatureRadPerMeter, endVal.curvatureRadPerMeter, t);
 
     // Integrate the field speeds to get the pose for this interpolated state, since linearly
-    // interpolating the pose gives an inaccurate result if the speeds are changing between states
+    // interpolating the pose gives an inaccurate result if the speeds are changing between
+    // states. Forward Euler with 10 ms steps, plus a remainder step for the last partial
+    // interval. Linear velocity is lerped per step; heading stays constant (the chord
+    // direction).
     double lerpedXPos = pose.getX();
     double lerpedYPos = pose.getY();
-    double intTime = timeSeconds + 0.01;
-    while (true) {
-      double intT = (intTime - timeSeconds) / (lerpedState.timeSeconds - timeSeconds);
-      double intLinearVel = MathUtil.interpolate(linearVelocity, lerpedState.linearVelocity, intT);
-      double intVX = intLinearVel * lerpedState.heading.getCos();
-      double intVY = intLinearVel * lerpedState.heading.getSin();
-
-      if (intTime >= lerpedState.timeSeconds - 0.01) {
-        double dt = lerpedState.timeSeconds - intTime;
-        lerpedXPos += intVX * dt;
-        lerpedYPos += intVY * dt;
-        break;
+    if (deltaT > 0) {
+      double cosH = heading.getCos();
+      double sinH = heading.getSin();
+      double intTime = timeSeconds;
+      while (true) {
+        double intT = (intTime - timeSeconds) / deltaT;
+        double intLinearVel = MathUtil.interpolate(linearVelocity, endVal.linearVelocity, intT);
+        double intVX = intLinearVel * cosH;
+        double intVY = intLinearVel * sinH;
+
+        double remainingTime = lerpedState.timeSeconds - intTime;
+        if (remainingTime <= 0.01) {
+          lerpedXPos += intVX * remainingTime;
+          lerpedYPos += intVY * remainingTime;
+          break;
+        }
+
+        lerpedXPos += intVX * 0.01;
+        lerpedYPos += intVY * 0.01;
+        intTime += 0.01;
       }
-
-      lerpedXPos += intVX * 0.01;
-      lerpedYPos += intVY * 0.01;
-
-      intTime += 0.01;
     }
+    // If deltaT == 0, pose stays at this.pose -- no integration needed, no divide-by-zero.
 
     lerpedState.pose =
         new Pose2d(
@@ -125,6 +144,8 @@ public PathPlannerTrajectoryState reverse() {
     reversed.feedforwards = feedforwards.reverse();
     reversed.heading = heading.plus(Rotation2d.k180deg);
     reversed.distanceAlongPath = distanceAlongPath;
+    // Reversing direction of travel flips the sign of curvature (left becomes right).
+    reversed.curvatureRadPerMeter = -curvatureRadPerMeter;
 
     return reversed;
   }
@@ -144,6 +165,13 @@ public PathPlannerTrajectoryState flip() {
     flipped.feedforwards = feedforwards.flip();
     flipped.heading = FlippingUtil.flipFieldRotation(heading);
     flipped.distanceAlongPath = distanceAlongPath;
+    // Sign of signed curvature is preserved under 180-deg rotation (chirality preserved) but
+    // inverted under mirror reflection (chirality flipped).
+    flipped.curvatureRadPerMeter =
+        switch (FlippingUtil.symmetryType) {
+          case kMirrored -> -curvatureRadPerMeter;
+          case kRotational -> curvatureRadPerMeter;
+        };
 
     return flipped;
   }
@@ -163,6 +191,7 @@ public PathPlannerTrajectoryState copyWithTime(double time) {
     copy.feedforwards = feedforwards;
     copy.heading = heading;
     copy.distanceAlongPath = distanceAlongPath;
+    copy.curvatureRadPerMeter = curvatureRadPerMeter;
     copy.deltaPos = deltaPos;
     copy.deltaRot = deltaRot;
     copy.moduleStates = moduleStates;