On-The-Fly Path Generation When Already at Finished Pose Crashes Robot Code

[blaze-developer]

When trying to follow an on-the-fly path created like this,

public static Command straightLinePathCmd(Pose2d targetPose) {
    PathConstraints constraints = new PathConstraints(
      4.0,
      4.0,
      DegreesPerSecond.of(540).in(RadiansPerSecond),
      DegreesPerSecondPerSecond.of(720).in(RadiansPerSecondPerSecond)
    );

    List<Waypoint> waypoints = PathPlannerPath.waypointsFromPoses(
      AutoBuilder.getCurrentPose(),
      targetPose
    );

    PathPlannerPath path = new PathPlannerPath(
      waypoints,
      constraints,
      null,
      new GoalEndState(0.0, targetPose.getRotation())
    );

    path.preventFlipping = true;

    return AutoBuilder.followPath(path);
}

It normally works fine. However, when the robot is already at its final position, like when a driver accidentally presses the line-up button again after already lining up, the code crashes.

I tried checking if the list of waypoints was empty, or less than two before running, but no dice. I assume I have a misunderstanding in how all of this works, and I was not sure where to go further with debugging. So I come here, in assumption that this is not the intended behavior.

Error at java.base/java.lang.Thread.getStackTrace(Thread.java:1619): x and y components of Rotation2d are zero
        at java.base/java.lang.Thread.getStackTrace(Thread.java:1619)
        at edu.wpi.first.math.geometry.Rotation2d.<init>(Rotation2d.java:126)
        at edu.wpi.first.math.geometry.Rotation2d.rotateBy(Rotation2d.java:274)
        at edu.wpi.first.math.geometry.Rotation2d.plus(Rotation2d.java:213)
        at edu.wpi.first.math.geometry.Rotation2d.interpolate(Rotation2d.java:380)
        at com.pathplanner.lib.trajectory.PathPlannerTrajectory.cosineInterpolate(PathPlannerTrajectory.java:786)
        at com.pathplanner.lib.trajectory.PathPlannerTrajectory.generateStates(PathPlannerTrajectory.java:246)
        at com.pathplanner.lib.trajectory.PathPlannerTrajectory.<init>(PathPlannerTrajectory.java:69)
        at com.pathplanner.lib.path.PathPlannerPath.generateTrajectory(PathPlannerPath.java:1152)
        at com.pathplanner.lib.commands.FollowPathCommand.initialize(FollowPathCommand.java:131)
        at edu.wpi.first.wpilibj2.command.CommandScheduler.initCommand(CommandScheduler.java:169)
        ... (Generic Command Stack)
        at edu.wpi.first.wpilibj.RobotBase.lambda$startRobot$1(RobotBase.java:490)
        at java.base/java.lang.Thread.run(Thread.java:840)

Error at java.base/jdk.internal.util.Preconditions.outOfBounds(Preconditions.java:64): Unhandled exception: java.lang.IndexOutOfBoundsException: Index -1 out of bounds for length 0
        at java.base/jdk.internal.util.Preconditions.outOfBounds(Preconditions.java:64)
        at java.base/jdk.internal.util.Preconditions.outOfBoundsCheckIndex(Preconditions.java:70)
        at java.base/jdk.internal.util.Preconditions.checkIndex(Preconditions.java:266)
        at java.base/java.util.Objects.checkIndex(Objects.java:361)
        at java.base/java.util.ArrayList.get(ArrayList.java:427)
        at com.pathplanner.lib.trajectory.PathPlannerTrajectory.generateStates(PathPlannerTrajectory.java:264)
        at com.pathplanner.lib.trajectory.PathPlannerTrajectory.<init>(PathPlannerTrajectory.java:69)
        at com.pathplanner.lib.path.PathPlannerPath.generateTrajectory(PathPlannerPath.java:1152)
        at com.pathplanner.lib.commands.FollowPathCommand.initialize(FollowPathCommand.java:131)
        at edu.wpi.first.wpilibj2.command.CommandScheduler.initCommand(CommandScheduler.java:169)
        ... (Generic Command Stack)
        at edu.wpi.first.wpilibj.RobotBase.lambda$startRobot$1(RobotBase.java:490)
        at java.base/java.lang.Thread.run(Thread.java:840)

The index error seems to be in the else portion of this segment of PathPlannerTrajectory.generateStates():

if (i != path.numPoints() - 1) {
        Translation2d headingTranslation =
            path.getPoint(i + 1).position.minus(state.pose.getTranslation());
        if (headingTranslation.getNorm() <= 1e-6) {
          state.heading = Rotation2d.kZero;
        } else {
          state.heading = headingTranslation.getAngle();
        }
} else {
        state.heading = states.get(i - 1).heading;
}

[blaze-developer]

In addition to this, when the robot is extremely close, but not exactly, the robot can sometimes get stuck, not being controllable by normal controls anymore. I assume it gets stuck in a path that never finishes. Upon re-enabling, it is freed.