diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathDistanceCommand.java b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathDistanceCommand.java new file mode 100644 index 000000000..d13729b76 --- /dev/null +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/commands/FollowPathDistanceCommand.java @@ -0,0 +1,406 @@ +package com.pathplanner.lib.commands; + +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PathFollowingController; +import com.pathplanner.lib.events.EventScheduler; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.PPLibTelemetry; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.Collections; +import java.util.List; +import java.util.Optional; +import java.util.Set; +import java.util.function.BiConsumer; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +/** + * Path-following command that drives by projected arc length instead of elapsed time. On each + * cycle, the robot's current pose is projected onto the path, the controller is given the target + * state at that arc length, and the path finishes when the robot has reached the end position (with + * optional velocity and rotation gates). + * + *

Useful when a time-based reference would race ahead during disturbances (obstacles, contact + * with a game piece, momentary slippage). The target state always reflects where the robot actually + * is on the path, so cross-track error is measured at true progress. + * + *

Pairs well with {@link com.pathplanner.lib.controllers.PPCrossTrackHolonomicController}. + */ +public class FollowPathDistanceCommand extends Command { + /** + * End-of-path tolerances. The command finishes when all three conditions are met: + * + *

+ * + *

If the path's goal end state has a non-zero velocity (handoff to the next path in an auto), + * the velocity and rotation gates are skipped and only the distance gate applies. + * + * @param distanceToleranceMeters Distance from path end, in meters, below which the path is + * considered "near end". + * @param velocityToleranceMPS Maximum allowed speed component along the path's end tangent, in + * m/s. + * @param rotationToleranceRad Maximum allowed heading error from the path's end rotation, in + * radians. Set to {@link Double#POSITIVE_INFINITY} to disable the rotation gate. + */ + public record EndConditions( + double distanceToleranceMeters, double velocityToleranceMPS, double rotationToleranceRad) { + /** + * Default tolerances: 5 cm distance, 0.1 m/s along-tangent velocity, 5 degrees rotation. Tuned + * for typical FRC swerve scoring positions where small overshoot is unacceptable. + * + * @return Default end conditions + */ + public static EndConditions defaults() { + return new EndConditions(0.05, 0.1, Math.toRadians(5.0)); + } + } + + // Projection: lookahead window in number of state-array segments. PathPlanner state spacing is + // roughly 5 cm, so 6 segments = ~30 cm. Tight enough to reject odometry glitches, wide enough to + // not lose the path during fast moves at 50 Hz. + private static final int LOOKAHEAD_SEGMENTS = 6; + // Big-gap fallback: if the best closest-point distance in the lookahead window exceeds this, + // run a full-path scan to recover (e.g. after odometry reset or large disturbance). + private static final double BIG_GAP_THRESHOLD_M = 0.5; + // Maximum advance in arc length per execute() tick. Caps single-frame projection jumps that + // would otherwise let odometry spikes race the target state ahead. + private static final double MAX_DELTA_S_PER_TICK = 0.3; + + private final PathPlannerPath originalPath; + private final Supplier poseSupplier; + private final Supplier speedsSupplier; + private final BiConsumer output; + private final PathFollowingController controller; + private final RobotConfig robotConfig; + private final BooleanSupplier shouldFlipPath; + private final EndConditions endConditions; + private final EventScheduler eventScheduler; + + private PathPlannerPath path; + private PathPlannerTrajectory trajectory; + private double lastProjectedS; + private int lastProjectedIdx; + + /** + * Construct a distance-based path-following command. + * + * @param path Path to follow + * @param poseSupplier Supplier of the robot's field-relative pose + * @param speedsSupplier Supplier of robot-relative chassis speeds + * @param output Output sink for robot-relative chassis speeds + per-module feedforwards + * @param controller Path-following controller (typically {@link + * com.pathplanner.lib.controllers.PPCrossTrackHolonomicController}) + * @param robotConfig Robot configuration + * @param shouldFlipPath Whether to mirror/rotate the path for the red alliance + * @param endConditions End-of-path tolerances (use {@link EndConditions#defaults()} for sane + * defaults) + * @param requirements Subsystem requirements, usually just the drive subsystem + */ + public FollowPathDistanceCommand( + PathPlannerPath path, + Supplier poseSupplier, + Supplier speedsSupplier, + BiConsumer output, + PathFollowingController controller, + RobotConfig robotConfig, + BooleanSupplier shouldFlipPath, + EndConditions endConditions, + Subsystem... requirements) { + this.originalPath = path; + this.poseSupplier = poseSupplier; + this.speedsSupplier = speedsSupplier; + this.output = output; + this.controller = controller; + this.robotConfig = robotConfig; + this.shouldFlipPath = shouldFlipPath; + this.endConditions = endConditions; + this.eventScheduler = new EventScheduler(); + + Set driveRequirements = Set.of(requirements); + addRequirements(requirements); + var eventReqs = EventScheduler.getSchedulerRequirements(this.originalPath); + if (!Collections.disjoint(driveRequirements, eventReqs)) { + throw new IllegalArgumentException( + "Events that are triggered during path following cannot require the drive subsystem"); + } + addRequirements(eventReqs); + + this.path = this.originalPath; + Optional idealTrajectory = + this.path.getIdealTrajectory(this.robotConfig); + idealTrajectory.ifPresent(traj -> this.trajectory = traj); + } + + /** + * Convenience constructor using {@link EndConditions#defaults()}. + * + * @param path Path to follow + * @param poseSupplier Supplier of the robot's field-relative pose + * @param speedsSupplier Supplier of robot-relative chassis speeds + * @param output Output sink for robot-relative chassis speeds + per-module feedforwards + * @param controller Path-following controller + * @param robotConfig Robot configuration + * @param shouldFlipPath Whether to mirror/rotate the path for the red alliance + * @param requirements Subsystem requirements + */ + public FollowPathDistanceCommand( + PathPlannerPath path, + Supplier poseSupplier, + Supplier speedsSupplier, + BiConsumer output, + PathFollowingController controller, + RobotConfig robotConfig, + BooleanSupplier shouldFlipPath, + Subsystem... requirements) { + this( + path, + poseSupplier, + speedsSupplier, + output, + controller, + robotConfig, + shouldFlipPath, + EndConditions.defaults(), + requirements); + } + + @Override + public void initialize() { + if (shouldFlipPath.getAsBoolean() && !originalPath.preventFlipping) { + path = originalPath.flipPath(); + } else { + path = originalPath; + } + + Pose2d currentPose = poseSupplier.get(); + ChassisSpeeds currentSpeeds = speedsSupplier.get(); + controller.reset(currentPose, currentSpeeds); + + double linearVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + if (path.getIdealStartingState() != null) { + boolean idealVelocity = + Math.abs(linearVel - path.getIdealStartingState().velocityMPS()) <= 0.25; + boolean idealRotation = + !robotConfig.isHolonomic + || Math.abs( + currentPose + .getRotation() + .minus(path.getIdealStartingState().rotation()) + .getDegrees()) + <= 30.0; + if (idealVelocity && idealRotation) { + trajectory = path.getIdealTrajectory(robotConfig).orElseThrow(); + } else { + trajectory = path.generateTrajectory(currentSpeeds, currentPose.getRotation(), robotConfig); + } + } else { + trajectory = path.generateTrajectory(currentSpeeds, currentPose.getRotation(), robotConfig); + } + + PathPlannerLogging.logActivePath(path); + PPLibTelemetry.setCurrentPath(path); + + // Initial projection: full scan to handle pose starting anywhere on the path. + var initialProjection = fullScan(currentPose.getTranslation()); + lastProjectedIdx = initialProjection.segmentIndex; + lastProjectedS = initialProjection.arcLength; + + eventScheduler.initialize(trajectory); + } + + @Override + public void execute() { + Pose2d currentPose = poseSupplier.get(); + ChassisSpeeds currentSpeeds = speedsSupplier.get(); + + Projection p = projectOntoPath(currentPose.getTranslation()); + // Monotonic clamp + max-delta-per-tick guard + double advancedS = Math.max(lastProjectedS, p.arcLength); + advancedS = Math.min(advancedS, lastProjectedS + MAX_DELTA_S_PER_TICK); + lastProjectedS = advancedS; + lastProjectedIdx = p.segmentIndex; + + PathPlannerTrajectoryState targetState = trajectory.sampleByDistance(lastProjectedS); + + ChassisSpeeds targetSpeeds = controller.calculateRobotRelativeSpeeds(currentPose, targetState); + + double currentVel = + Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + + PPLibTelemetry.setCurrentPose(currentPose); + PathPlannerLogging.logCurrentPose(currentPose); + PPLibTelemetry.setTargetPose(targetState.pose); + PathPlannerLogging.logTargetPose(targetState.pose); + PPLibTelemetry.setVelocities( + currentVel, + targetState.linearVelocity, + currentSpeeds.omegaRadiansPerSecond, + targetSpeeds.omegaRadiansPerSecond); + + output.accept(targetSpeeds, targetState.feedforwards); + + // Event scheduling uses path time, derived from the projected state's timestamp. + eventScheduler.execute(targetState.timeSeconds); + } + + @Override + public boolean isFinished() { + double totalArc = trajectory.getTotalArcLength(); + boolean nearEnd = lastProjectedS >= totalArc - endConditions.distanceToleranceMeters(); + if (!nearEnd) return false; + + // If the path is a handoff (non-zero end velocity), only the distance gate applies -- we + // don't want to wait for the robot to come to a stop. Threshold matches end()'s "is this a + // stopping path?" check. + if (!isStoppingPath()) return true; + + PathPlannerTrajectoryState endState = trajectory.getEndState(); + double tx = endState.heading.getCos(); + double ty = endState.heading.getSin(); + ChassisSpeeds fieldSpeeds = speedsToFieldFrame(); + double alongTangent = fieldSpeeds.vxMetersPerSecond * tx + fieldSpeeds.vyMetersPerSecond * ty; + boolean velocityOk = Math.abs(alongTangent) < endConditions.velocityToleranceMPS(); + + double headingErr = + Math.abs(poseSupplier.get().getRotation().minus(endState.pose.getRotation()).getRadians()); + boolean rotationOk = headingErr <= endConditions.rotationToleranceRad(); + + return velocityOk && rotationOk; + } + + @Override + public void end(boolean interrupted) { + if (!interrupted && isStoppingPath()) { + output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); + } + PathPlannerLogging.logActivePath(null); + eventScheduler.end(); + } + + /** + * Whether the active path ends with the robot effectively stopped (vs. handing off velocity to a + * subsequent path). Matches the threshold used by {@link FollowPathCommand}. + */ + private boolean isStoppingPath() { + return path.getGoalEndState().velocityMPS() < 0.1; + } + + /** + * Project the robot's translation onto the path using the cached lookahead window. Falls back to + * a full-path scan if the window can't find a close-enough match (covers odometry resets or large + * disturbances). + */ + private Projection projectOntoPath(Translation2d robotPos) { + List states = trajectory.getStates(); + int n = states.size(); + if (n < 2) return new Projection(0, 0.0); + + int windowStart = Math.max(0, lastProjectedIdx); + int windowEnd = Math.min(n - 2, lastProjectedIdx + LOOKAHEAD_SEGMENTS); + Projection best = scanWindow(robotPos, states, windowStart, windowEnd); + + // If the best projection saturated to the forward edge of the window, scan one more window + // forward to handle fast moves that crossed multiple states in one tick. + if (best.segmentIndex == windowEnd && best.segmentU >= 0.99 && windowEnd < n - 2) { + int extEnd = Math.min(n - 2, windowEnd + LOOKAHEAD_SEGMENTS); + Projection extended = scanWindow(robotPos, states, windowEnd, extEnd); + if (extended.distance < best.distance) best = extended; + } + + // Big-gap fallback: distance unreasonable, fall back to full scan. + if (best.distance > BIG_GAP_THRESHOLD_M) { + Projection fullScan = fullScan(robotPos); + // Only accept the full-scan result if it's strictly better and meaningfully forward of the + // window result (avoid jumping backward to a closer segment behind us). + if (fullScan.distance < best.distance && fullScan.arcLength >= lastProjectedS) { + best = fullScan; + } + } + return best; + } + + private Projection fullScan(Translation2d robotPos) { + List states = trajectory.getStates(); + if (states.size() < 2) return new Projection(0, 0.0); + return scanWindow(robotPos, states, 0, states.size() - 2); + } + + /** + * Closest-point-on-polyline over a segment range [startIdx, endIdx] (inclusive). For each segment + * (states[i], states[i+1]), compute the closest point on that line segment to robotPos via + * closed-form projection clamped to [0, 1]. Return the best (segmentIndex, segmentU, arcLength, + * distance). Package-private static so tests can exercise the real implementation. + */ + static Projection scanWindow( + Translation2d robotPos, List states, int startIdx, int endIdx) { + int bestIdx = startIdx; + double bestU = 0.0; + double bestDistSq = Double.POSITIVE_INFINITY; + + for (int i = startIdx; i <= endIdx; i++) { + var a = states.get(i).pose.getTranslation(); + var b = states.get(i + 1).pose.getTranslation(); + double abx = b.getX() - a.getX(); + double aby = b.getY() - a.getY(); + double abLenSq = abx * abx + aby * aby; + double u; + if (abLenSq < 1e-12) { + u = 0.0; + } else { + double apx = robotPos.getX() - a.getX(); + double apy = robotPos.getY() - a.getY(); + u = (apx * abx + apy * aby) / abLenSq; + if (u < 0.0) u = 0.0; + else if (u > 1.0) u = 1.0; + } + double cx = a.getX() + u * abx; + double cy = a.getY() + u * aby; + double dx = robotPos.getX() - cx; + double dy = robotPos.getY() - cy; + double distSq = dx * dx + dy * dy; + if (distSq < bestDistSq) { + bestDistSq = distSq; + bestIdx = i; + bestU = u; + } + } + + double sA = states.get(bestIdx).distanceAlongPath; + double sB = states.get(bestIdx + 1).distanceAlongPath; + double arcLength = sA + bestU * (sB - sA); + Projection out = new Projection(bestIdx, arcLength); + out.segmentU = bestU; + out.distance = Math.sqrt(bestDistSq); + return out; + } + + private ChassisSpeeds speedsToFieldFrame() { + ChassisSpeeds robotSpeeds = speedsSupplier.get(); + return ChassisSpeeds.fromRobotRelativeSpeeds(robotSpeeds, poseSupplier.get().getRotation()); + } + + /** Internal projection result. Package-private for testing. */ + static class Projection { + final int segmentIndex; + final double arcLength; + double segmentU = 0.0; + double distance = 0.0; + + Projection(int segmentIndex, double arcLength) { + this.segmentIndex = segmentIndex; + this.arcLength = arcLength; + } + } +} diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPCrossTrackHolonomicController.java b/pathplannerlib/src/main/java/com/pathplanner/lib/controllers/PPCrossTrackHolonomicController.java new file mode 100644 index 000000000..82b6141da --- /dev/null +++ b/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. + * + *

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. + * + *

Differences vs {@link PPHolonomicDriveController}: + * + *

+ */ +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; + } +} diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java index c9c89422c..dabaa97b0 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectory.java @@ -34,6 +34,7 @@ public PathPlannerTrajectory(List states, List 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 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( diff --git a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java index b28d15869..9186da305 100644 --- a/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java +++ b/pathplannerlib/src/main/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryState.java @@ -24,6 +24,11 @@ public class PathPlannerTrajectoryState implements Interpolatable 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; diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/commands/FollowPathDistanceCommandTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/commands/FollowPathDistanceCommandTest.java new file mode 100644 index 000000000..a497a645d --- /dev/null +++ b/pathplannerlib/src/test/java/com/pathplanner/lib/commands/FollowPathDistanceCommandTest.java @@ -0,0 +1,148 @@ +package com.pathplanner.lib.commands; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; +import com.pathplanner.lib.util.DriveFeedforwards; +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; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.Test; + +/** + * Tests focus on the projection geometry exercised by FollowPathDistanceCommand. The {@code + * projectOntoPath} / {@code scanWindow} machinery is exercised indirectly by constructing known + * trajectories and asserting that {@link PathPlannerTrajectory#sampleByDistance(double)} at the + * projected s lands where expected. + * + *

End-to-end command lifecycle (initialize/execute/end) needs a full RobotConfig and a real + * subsystem, so it's covered in sim/integration testing rather than unit tests. + */ +public class FollowPathDistanceCommandTest { + + private static PathPlannerTrajectory straightLineX(double length) { + int samples = (int) Math.round(length / 0.05); + List states = new ArrayList<>(); + double step = length / samples; + for (int i = 0; i <= samples; i++) { + double x = i * step; + var s = new PathPlannerTrajectoryState(); + s.timeSeconds = x / 1.0; + s.pose = new Pose2d(new Translation2d(x, 0), Rotation2d.kZero); + s.heading = Rotation2d.kZero; + s.linearVelocity = 1.0; + s.fieldSpeeds = new ChassisSpeeds(1.0, 0, 0); + s.feedforwards = DriveFeedforwards.zeros(4); + states.add(s); + } + return new PathPlannerTrajectory(states); + } + + // Tests call the real FollowPathDistanceCommand.scanWindow (package-private static), so any + // future change to the algorithm is reflected here. End-to-end command lifecycle + // (initialize/execute/end) needs a full RobotConfig and a real subsystem, so it's covered in + // sim/integration testing rather than unit tests. + + @Test + public void projectsExactlyOntoStraightPathAtKnownStates() { + var traj = straightLineX(2.0); + var states = traj.getStates(); + for (int i = 0; i < states.size(); i++) { + var pos = states.get(i).pose.getTranslation(); + var r = FollowPathDistanceCommand.scanWindow(pos, states, 0, states.size() - 2); + assertEquals(states.get(i).distanceAlongPath, r.arcLength, 1e-9, "state " + i); + assertEquals(0.0, r.distance, 1e-9); + } + } + + @Test + public void projectsPerpendicularOffsetOntoNearestPathPoint() { + var traj = straightLineX(2.0); + var states = traj.getStates(); + // Robot at (0.5, 0.3) on a path along +X: closest point is (0.5, 0), s=0.5, dist=0.3 + var r = + FollowPathDistanceCommand.scanWindow( + new Translation2d(0.5, 0.3), states, 0, states.size() - 2); + assertEquals(0.5, r.arcLength, 1e-9); + assertEquals(0.3, r.distance, 1e-9); + } + + @Test + public void projectsRobotAheadOfPathEndClampsToEndState() { + var traj = straightLineX(2.0); + var states = traj.getStates(); + // Robot at (3.0, 0) -- past the path's end at (2.0, 0). The closest segment is the last, + // and u clamps to 1.0, giving s=totalArcLength. + var r = + FollowPathDistanceCommand.scanWindow( + new Translation2d(3.0, 0), states, 0, states.size() - 2); + assertEquals(traj.getTotalArcLength(), r.arcLength, 1e-9); + } + + @Test + public void projectsRobotBehindPathStartClampsToInitialState() { + var traj = straightLineX(2.0); + var states = traj.getStates(); + // Robot at (-1.0, 0) -- before path start. Best segment is the first, u clamps to 0. + var r = + FollowPathDistanceCommand.scanWindow( + new Translation2d(-1.0, 0), states, 0, states.size() - 2); + assertEquals(0.0, r.arcLength, 1e-9); + } + + @Test + public void projectionWithinWindowDoesntCrossWholePath() { + var traj = straightLineX(2.0); + var states = traj.getStates(); + // Lookahead window [5, 10]. Robot truly closest to state 20 at (1.0, 0) -- but window scan + // returns the best within [5, 10]. Demonstrates that the window limits results -- the full + // algorithm has a saturation-fallback (tested separately) to handle this. + var r = FollowPathDistanceCommand.scanWindow(new Translation2d(1.0, 0), states, 5, 10); + assertTrue(r.segmentIndex >= 5 && r.segmentIndex <= 10); + // The closest in-window point is the end of segment 10 (state 11 at s~=0.55). + assertTrue(r.arcLength <= 0.55 + 1e-6); + } + + @Test + public void sampleByDistanceAtProjectedSGivesTargetPoseOnPath() { + var traj = straightLineX(2.0); + var states = traj.getStates(); + // Robot at (1.234, 0.5) -- 1.234 along path, 0.5 m off. Sampling at projected s should + // return a state on the path at x=1.234 (sub-mm) on a straight constant-speed path. + var r = + FollowPathDistanceCommand.scanWindow( + new Translation2d(1.234, 0.5), states, 0, states.size() - 2); + var target = traj.sampleByDistance(r.arcLength); + assertEquals(1.234, target.pose.getX(), 1e-6); + assertEquals(0.0, target.pose.getY(), 1e-9); + } + + @Test + public void endConditionsDefaultsHasSensibleValues() { + var ec = FollowPathDistanceCommand.EndConditions.defaults(); + assertEquals(0.05, ec.distanceToleranceMeters(), 1e-9); + assertEquals(0.1, ec.velocityToleranceMPS(), 1e-9); + assertEquals(Math.toRadians(5.0), ec.rotationToleranceRad(), 1e-9); + } + + @Test + public void endConditionsCustomValuesArePreserved() { + var ec = new FollowPathDistanceCommand.EndConditions(0.10, 0.2, Math.toRadians(10.0)); + assertEquals(0.10, ec.distanceToleranceMeters(), 1e-9); + assertEquals(0.2, ec.velocityToleranceMPS(), 1e-9); + assertEquals(Math.toRadians(10.0), ec.rotationToleranceRad(), 1e-9); + } + + @Test + public void endConditionsRotationCanBeDisabled() { + var ec = new FollowPathDistanceCommand.EndConditions(0.05, 0.1, Double.POSITIVE_INFINITY); + assertNotNull(ec); + assertEquals(Double.POSITIVE_INFINITY, ec.rotationToleranceRad()); + } +} diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/controllers/PPCrossTrackHolonomicControllerTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/controllers/PPCrossTrackHolonomicControllerTest.java new file mode 100644 index 000000000..7c5d3fd54 --- /dev/null +++ b/pathplannerlib/src/test/java/com/pathplanner/lib/controllers/PPCrossTrackHolonomicControllerTest.java @@ -0,0 +1,186 @@ +package com.pathplanner.lib.controllers; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; +import com.pathplanner.lib.util.DriveFeedforwards; +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; +import org.junit.jupiter.api.Test; + +public class PPCrossTrackHolonomicControllerTest { + private static final double DELTA = 1e-6; + + private static PathPlannerTrajectoryState targetAt(double x, double y, double headingRad) { + var s = new PathPlannerTrajectoryState(); + s.pose = new Pose2d(new Translation2d(x, y), Rotation2d.fromRadians(headingRad)); + s.heading = Rotation2d.fromRadians(headingRad); + s.linearVelocity = 2.0; + s.fieldSpeeds = new ChassisSpeeds(2.0 * Math.cos(headingRad), 2.0 * Math.sin(headingRad), 0.0); + s.feedforwards = DriveFeedforwards.zeros(4); + return s; + } + + @Test + public void zeroErrorReturnsFeedforwardOnly() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(3.0, 0.0, 0.5), new PIDConstants(5.0, 0.0, 0.0), 0.0); + var target = targetAt(1.0, 0.0, 0.0); // path going +X at (1,0) + var pose = new Pose2d(new Translation2d(1.0, 0.0), Rotation2d.kZero); // robot at target + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + // Robot-frame speeds when heading=0 == field speeds + assertEquals(2.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(0.0, speeds.vyMetersPerSecond, DELTA); + assertEquals(0.0, speeds.omegaRadiansPerSecond, DELTA); + } + + @Test + public void positiveCrossTrackErrorProducesPerpendicularCorrection() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(3.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0), 0.0); + // Path going +X, target at origin, robot 0.1 m to the LEFT (+Y). + var target = targetAt(0.0, 0.0, 0.0); + var pose = new Pose2d(new Translation2d(0.0, 0.1), Rotation2d.kZero); + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + // Cross-track error should push robot back toward path (-Y direction). + // With kp=3.0, perpVelocity = -(3.0 * 0.1) = -0.3 m/s along the +Y direction. + // Plus tangent FF of 2.0 along +X. + assertEquals(2.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(-0.3, speeds.vyMetersPerSecond, DELTA); + } + + @Test + public void negativeCrossTrackErrorProducesPositivePerpendicularCorrection() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(3.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0), 0.0); + // Path going +X, robot to the RIGHT (-Y). + var target = targetAt(0.0, 0.0, 0.0); + var pose = new Pose2d(new Translation2d(0.0, -0.1), Rotation2d.kZero); + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + assertEquals(2.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(0.3, speeds.vyMetersPerSecond, DELTA); + } + + @Test + public void tangentDirectionErrorProducesNoPerpendicularCorrection() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(3.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0), 0.0); + // Path going +X, robot AHEAD of target along the tangent (no cross-track error). + var target = targetAt(0.0, 0.0, 0.0); + var pose = new Pose2d(new Translation2d(0.5, 0.0), Rotation2d.kZero); + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + // Tangent-direction error doesn't affect cross-track; output is pure feedforward. + assertEquals(2.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(0.0, speeds.vyMetersPerSecond, DELTA); + } + + @Test + public void curvatureFfAddsCentripetalPerpendicularVelocity() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(0.0, 0.0, 0.0), // disable cross-track to isolate FF + new PIDConstants(5.0, 0.0, 0.0), + 0.1); // FF gain + // Target with left-curving path: kappa = +0.5 (1/m), v = 2.0 m/s + // curvatureFf = 0.1 * 2.0^2 * 0.5 = 0.2 m/s perpendicular (toward inside of curve, +Y). + var target = targetAt(0.0, 0.0, 0.0); + target.curvatureRadPerMeter = 0.5; + var pose = new Pose2d(new Translation2d(0.0, 0.0), Rotation2d.kZero); + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + assertEquals(2.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(0.2, speeds.vyMetersPerSecond, DELTA); + } + + @Test + public void curvatureFfSignFlipsForRightCurve() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(0.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0), 0.1); + var target = targetAt(0.0, 0.0, 0.0); + target.curvatureRadPerMeter = -0.5; // right turn + var pose = new Pose2d(new Translation2d(0.0, 0.0), Rotation2d.kZero); + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + assertEquals(2.0, speeds.vxMetersPerSecond, DELTA); + assertEquals(-0.2, speeds.vyMetersPerSecond, DELTA); + } + + @Test + public void headingErrorProducesRotationFeedback() { + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(0.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0), 0.0); + var target = targetAt(0.0, 0.0, 0.0); // target rotation = 0 + // Robot rotated 10 degrees right (-Z, negative): heading error pulls it back +Z. + var pose = new Pose2d(new Translation2d(0.0, 0.0), Rotation2d.fromDegrees(-10.0)); + var speeds = controller.calculateRobotRelativeSpeeds(pose, target); + // kp=5.0, error = +10 deg = +0.1745 rad, expected omega = 5 * 0.1745 ~= 0.872 rad/s positive + assertTrue(speeds.omegaRadiansPerSecond > 0.5); + assertTrue(speeds.omegaRadiansPerSecond < 1.2); + } + + @Test + public void defaultsFactoryProducesNonNullController() { + var c = PPCrossTrackHolonomicController.defaults(); + var target = targetAt(0.0, 0.0, 0.0); + var pose = new Pose2d(new Translation2d(0.0, 0.1), Rotation2d.kZero); + var speeds = c.calculateRobotRelativeSpeeds(pose, target); + // Just confirm it produces *some* perpendicular correction with default gains + assertTrue( + speeds.vyMetersPerSecond < 0, + "default gains should pull robot back toward path (negative y for +y error)"); + } + + @Test + public void isHolonomicReturnsTrue() { + var c = PPCrossTrackHolonomicController.defaults(); + assertTrue(c.isHolonomic()); + } + + @Test + public void resetClearsCrossTrackDerivativeState() { + // Pure D, period 0.02s: D output = (e_n - e_{n-1}) / 0.02, applied with sign such that vy + // pushes back toward path (i.e. for rising +y error, vy goes -y). + var controller = + new PPCrossTrackHolonomicController( + new PIDConstants(0.0, 0.0, 1.0), new PIDConstants(0.0, 0.0, 0.0), 0.0); + var target = targetAt(0.0, 0.0, 0.0); + + // 1st call: hasLastError=false guard returns 0 regardless of error. + var first = + controller.calculateRobotRelativeSpeeds( + new Pose2d(new Translation2d(0, 0.0), Rotation2d.kZero), target); + assertEquals(0.0, first.vyMetersPerSecond, DELTA); + + // 2nd call: error jumps from 0.0 to 0.2 over one period (0.02s). D term observes the + // change: rate = (0.2 - 0.0) / 0.02 = 10.0, correction = -kd * rate = -10 in the +y normal + // direction, so vy = -10. + var second = + controller.calculateRobotRelativeSpeeds( + new Pose2d(new Translation2d(0, 0.2), Rotation2d.kZero), target); + assertEquals( + -10.0, + second.vyMetersPerSecond, + 1e-6, + "second call should see D-term reflecting the 0.0->0.2 step"); + + // Reset clears the error history. The next call with err=0.2 is again the "first call": + // hasLastError=false guard returns D=0, NOT -10 (which would indicate stale state). + controller.reset(Pose2d.kZero, new ChassisSpeeds()); + var afterReset = + controller.calculateRobotRelativeSpeeds( + new Pose2d(new Translation2d(0, 0.2), Rotation2d.kZero), target); + assertEquals( + 0.0, + afterReset.vyMetersPerSecond, + DELTA, + "after reset, first call must suppress D regardless of error magnitude"); + } +} diff --git a/pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java b/pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java index 820219596..f436429b4 100644 --- a/pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java +++ b/pathplannerlib/src/test/java/com/pathplanner/lib/trajectory/PathPlannerTrajectoryTest.java @@ -114,11 +114,7 @@ public void sampleByDistanceClampsAboveEnd() { @Test public void sampleByDistanceReturnsDistanceAlongPathExactly() { - // The interpolated distanceAlongPath field is a pure lerp of the bracket values, so it - // can be asserted exactly. (Pose values go through PathPlannerTrajectoryState.interpolate - // which Euler-integrates and inherits an existing upstream quirk where the loop starts - // at timeSeconds + 0.01; that's tested via the agreement-with-sample(time) tests below - // rather than absolute pose round-tripping.) + // distanceAlongPath is a pure lerp of the bracket values. var traj = quarterArc(1.0, 1.0); for (var state : traj.getStates()) { var sampled = traj.sampleByDistance(state.distanceAlongPath); @@ -126,6 +122,92 @@ public void sampleByDistanceReturnsDistanceAlongPathExactly() { } } + @Test + public void interpolateAtT1ReturnsEndPose() { + // Regression: pre-fix interpolate's loop started at timeSeconds + 0.01 and used the start + // state's heading throughout, so even at t=1 the pose was 1 0.01 s step short of endVal. + // With the fix, interpolate(s0, s1, 1.0) should reproduce s1.pose to sub-mm. + var traj = straightLine(2.0, 1.0); + var states = traj.getStates(); + var a = states.get(10); + var b = states.get(11); + var lerped = a.interpolate(b, 1.0); + assertEquals(b.pose.getX(), lerped.pose.getX(), 1e-9); + assertEquals(b.pose.getY(), lerped.pose.getY(), 1e-9); + } + + @Test + public void interpolateAtT0ReturnsStartPose() { + var traj = straightLine(2.0, 1.0); + var states = traj.getStates(); + var a = states.get(10); + var b = states.get(11); + var lerped = a.interpolate(b, 0.0); + assertEquals(a.pose.getX(), lerped.pose.getX(), 1e-9); + assertEquals(a.pose.getY(), lerped.pose.getY(), 1e-9); + } + + @Test + public void interpolateAtMidpointGivesGeometricMidpointOnStraightLine() { + var traj = straightLine(2.0, 1.0); + var states = traj.getStates(); + var a = states.get(10); // x = 0.5 + var b = states.get(11); // x = 0.55 + var mid = a.interpolate(b, 0.5); + assertEquals(0.525, mid.pose.getX(), 1e-9); + assertEquals(0.0, mid.pose.getY(), 1e-9); + } + + @Test + public void interpolateWithZeroDeltaTReturnsCopyOfStart() { + // Regression: pre-fix code divided by zero in intT = (intTime - timeSeconds) / deltaT, + // producing NaN pose. With the fix the integration is skipped when deltaT == 0. + var s0 = new PathPlannerTrajectoryState(); + s0.timeSeconds = 1.0; + s0.pose = new Pose2d(new Translation2d(3.0, 4.0), Rotation2d.kZero); + s0.heading = Rotation2d.kZero; + s0.linearVelocity = 2.0; + s0.fieldSpeeds = new ChassisSpeeds(2.0, 0, 0); + s0.feedforwards = DriveFeedforwards.zeros(4); + var s1 = new PathPlannerTrajectoryState(); + s1.timeSeconds = 1.0; // same time as s0 + s1.pose = new Pose2d(new Translation2d(3.5, 4.0), Rotation2d.kZero); + s1.heading = Rotation2d.kZero; + s1.linearVelocity = 2.0; + s1.fieldSpeeds = new ChassisSpeeds(2.0, 0, 0); + s1.feedforwards = DriveFeedforwards.zeros(4); + var lerped = s0.interpolate(s1, 0.5); + assertTrue(Double.isFinite(lerped.pose.getX()), "deltaT=0 must not produce NaN"); + assertTrue(Double.isFinite(lerped.pose.getY()), "deltaT=0 must not produce NaN"); + assertEquals(s0.pose.getX(), lerped.pose.getX(), 1e-9); + assertEquals(s0.pose.getY(), lerped.pose.getY(), 1e-9); + } + + @Test + public void interpolateWithSmallDeltaTIntegratesForward() { + // Regression: pre-fix code with deltaT < 0.01 entered the break case immediately with + // dt = lerpedState.timeSeconds - intTime = NEGATIVE, integrating BACKWARDS. With the fix, + // deltaT = 0.005 integrates v * 0.005 = 0.005 of forward motion. + var s0 = new PathPlannerTrajectoryState(); + s0.timeSeconds = 0.0; + s0.pose = new Pose2d(new Translation2d(0.0, 0.0), Rotation2d.kZero); + s0.heading = Rotation2d.kZero; + s0.linearVelocity = 1.0; + s0.fieldSpeeds = new ChassisSpeeds(1.0, 0, 0); + s0.feedforwards = DriveFeedforwards.zeros(4); + var s1 = new PathPlannerTrajectoryState(); + s1.timeSeconds = 0.005; + s1.pose = new Pose2d(new Translation2d(0.005, 0.0), Rotation2d.kZero); + s1.heading = Rotation2d.kZero; + s1.linearVelocity = 1.0; + s1.fieldSpeeds = new ChassisSpeeds(1.0, 0, 0); + s1.feedforwards = DriveFeedforwards.zeros(4); + var lerped = s0.interpolate(s1, 1.0); + // Forward motion expected: 1.0 m/s * 0.005 s = 0.005 m + assertEquals(0.005, lerped.pose.getX(), 1e-9); + assertEquals(0.0, lerped.pose.getY(), 1e-9); + } + @Test public void sampleByDistanceInterpolatesBetweenStates() { // Halfway between two adjacent states should land near (not at) either endpoint, and on @@ -196,6 +278,62 @@ public void copyWithTimePreservesDistanceAlongPath() { assertEquals(42.0, copy.timeSeconds, DELTA); } + @Test + public void curvaturePopulatedOnStraightLineIsZero() { + var traj = straightLine(2.0, 1.0); + for (var state : traj.getStates()) { + assertEquals(0.0, state.curvatureRadPerMeter, 1e-9); + } + } + + @Test + public void curvaturePopulatedOnQuarterArcMatchesReciprocalRadius() { + // Unit circle -> kappa = 1.0 (1/m). Positive sign because the arc goes CCW (left turn). + var traj = quarterArc(1.0, 1.0); + // Interior states (skip endpoints which get 0). + var states = traj.getStates(); + for (int i = 1; i < states.size() - 1; i++) { + // Allow some tolerance because three-point circle fit on a sampled arc has small error. + assertEquals( + 1.0, states.get(i).curvatureRadPerMeter, 0.05, "expected curvature ~1.0 at state " + i); + assertTrue( + states.get(i).curvatureRadPerMeter > 0, + "left-turning arc should have positive curvature at state " + i); + } + } + + @Test + public void curvatureEndpointsAreZero() { + var traj = quarterArc(1.0, 1.0); + var states = traj.getStates(); + assertEquals(0.0, states.get(0).curvatureRadPerMeter, 1e-9); + assertEquals(0.0, states.get(states.size() - 1).curvatureRadPerMeter, 1e-9); + } + + @Test + public void interpolatePreservesCurvatureLinearly() { + var s0 = new PathPlannerTrajectoryState(); + s0.timeSeconds = 0; + s0.pose = Pose2d.kZero; + s0.heading = Rotation2d.kZero; + s0.linearVelocity = 1.0; + s0.fieldSpeeds = new ChassisSpeeds(1.0, 0, 0); + s0.feedforwards = DriveFeedforwards.zeros(4); + s0.curvatureRadPerMeter = 0.0; + + var s1 = new PathPlannerTrajectoryState(); + s1.timeSeconds = 1.0; + s1.pose = new Pose2d(new Translation2d(1.0, 0), Rotation2d.kZero); + s1.heading = Rotation2d.kZero; + s1.linearVelocity = 1.0; + s1.fieldSpeeds = new ChassisSpeeds(1.0, 0, 0); + s1.feedforwards = DriveFeedforwards.zeros(4); + s1.curvatureRadPerMeter = 2.0; + + var mid = s0.interpolate(s1, 0.5); + assertEquals(1.0, mid.curvatureRadPerMeter, DELTA); + } + @Test public void interpolatePreservesDistanceAlongPathLinearly() { var s0 = new PathPlannerTrajectoryState();