Viewing and Diagnosing Trajectories

For path visualization, to view a regular trajectory, you can use java/org/aceshigh176/lib/path_optimization/SwerveVisualizer.java.

When you get to the point of having an auto with multiple paths, java/org/aceshigh176/lib/path_optimization/SwerveVisualizerForAuto.java is set up to make a separate window for each path in the trajectory.

        var waypoints =
                List.of(
                        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
                        new Pose2d(100, 0, Rotation2d.fromDegrees(0))
                );
        var headings =
                List.of(
                        Rotation2d.fromDegrees(0),
                        Rotation2d.fromDegrees(0)
                );

        Trajectory<TimedState<Pose2dWithCurvature>, TimedState<Rotation2d>> trajectory = motionPlanner.generateTrajectory(
                false,
                waypoints,
                headings,
                List.of(new MaxVelocityForTurnRadiusConstraint()),
                60,
                60,
                9
        );

The top window is a position graph, which the edges of the robot painted.

The middle window plots:

• green: rotation velocity
• red: rotation accel
• black: curvature

The bottom window:

• black: velocity
• magenta: acceleration
• blue: turn radius
• red: max velocity (for turn radius)
• green: robot heading