cleaning and simplifying trajectory generation

comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java

@@ -10,7 +10,9 @@
 import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics;
 import org.team100.lib.subsystems.r3.commands.GoToPosePosition;
 import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
 import org.team100.lib.trajectory.timing.ConstantConstraint;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TorqueConstraint;
 import org.team100.lib.trajectory.timing.YawRateConstraint;
@@ -44,8 +46,9 @@ public MechTrajectories(
         // These finer grains make smoother paths and schedules but
         // take longer to compute, so if it takes too long, make these
         // numbers bigger!
-        m_planner = new TrajectoryPlanner(0.01, 0.1, 0.05, c);
-        // m_planner = new TrajectoryPlanner(0.02, 0.2, 0.1, c);
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c);
+        PathFactory pathFactory = new PathFactory(0.05, 0.01, 0.01, 0.1);
+        m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
     }
 
     /** A command that goes from the start to the end and then finishes. */

comp/src/main/java/org/team100/frc2025/Swerve/Auto/BigLoop.java

@@ -9,19 +9,22 @@
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
+import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TimingConstraintFactory;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
 
 import edu.wpi.first.math.geometry.Pose2d;
 
 /** big looping trajectory for testing */
 public class BigLoop implements Function<Pose2d, Trajectory100> {
     private final TrajectoryPlanner m_planner;
 
-    public BigLoop(
-            LoggerFactory log,
-            SwerveKinodynamics kinodynamics) {
-        m_planner = new TrajectoryPlanner(
-                new TimingConstraintFactory(kinodynamics).auto(log.type(this)));
+    public BigLoop(LoggerFactory log, SwerveKinodynamics kinodynamics) {
+        List<TimingConstraint> constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this));
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        PathFactory pathFactory = new PathFactory();
+        m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
     }
 
     @Override

comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStation.java

@@ -12,6 +12,9 @@
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
+import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TimingConstraintFactory;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -34,8 +37,10 @@ public GoToCoralStation(
             double scale) {
         m_station = station;
         m_scale = scale;
-        m_planner = new TrajectoryPlanner(
-                new TimingConstraintFactory(kinodynamics).auto(log.type(this)));
+        List<TimingConstraint> constraints = new TimingConstraintFactory(kinodynamics).auto(log.type(this));
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        PathFactory pathFactory = new PathFactory();
+        m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
     }
 
     @Override

comp/src/main/java/org/team100/frc2025/robot/AllAutons.java

@@ -1,5 +1,7 @@
 package org.team100.frc2025.robot;
 
+import java.util.List;
+
 import org.team100.lib.config.AnnotatedCommand;
 import org.team100.lib.config.AutonChooser;
 import org.team100.lib.config.ElevatorUtil.ScoringLevel;
@@ -11,6 +13,9 @@
 import org.team100.lib.profile.r3.HolonomicProfileFactory;
 import org.team100.lib.profile.r3.ProfileR3;
 import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
+import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TimingConstraintFactory;
 
 import edu.wpi.first.wpilibj2.command.Command;
@@ -34,8 +39,11 @@ public AllAutons(Machinery machinery) {
                 5);
         final FullStateControllerR3 controller = ControllerFactoryR3
                 .auto2025LooseTolerance(autoLog);
-        final TrajectoryPlanner planner = new TrajectoryPlanner(
-                new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(autoLog));
+        List<TimingConstraint> constraints = new TimingConstraintFactory(machinery.m_swerveKinodynamics)
+                .medium(autoLog);
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        PathFactory pathFactory = new PathFactory();
+        final TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
 
         // WARNING! The glass widget will override the default, so check it!
         // Run the auto in pre-match testing!

comp/src/main/java/org/team100/frc2025/robot/Prewarmer.java

@@ -10,6 +10,9 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.Logging;
 import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
+import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TimingConstraintFactory;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -38,8 +41,10 @@ public static void init(Machinery machinery) {
                 new Pose2d(new Translation2d(1, 0), Rotation2d.kZero),
                 new DirectionSE2(1, 0, 0),
                 1));
-        TrajectoryPlanner planner = new TrajectoryPlanner(
-                new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger));
+        List<TimingConstraint> constraints = new TimingConstraintFactory(machinery.m_swerveKinodynamics).medium(logger);
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        PathFactory pathFactory = new PathFactory();
+        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
         planner.restToRest(waypoints);
 
         // Exercise the drive motors.

comp/src/test/java/org/team100/frc2025/CalgamesArm/TrajectoryJointTest.java

@@ -17,7 +17,9 @@
 import org.team100.lib.subsystems.prr.JointVelocities;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
 import org.team100.lib.trajectory.timing.ConstantConstraint;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.YawRateConstraint;
 
@@ -47,7 +49,9 @@ void homeToL4() {
         List<TimingConstraint> c = List.of(
                 new ConstantConstraint(log, 1, 1),
                 new YawRateConstraint(log, 1, 1));
-        TrajectoryPlanner m_planner = new TrajectoryPlanner(c);
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(c);
+        PathFactory pathFactory = new PathFactory();
+        TrajectoryPlanner m_planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
 
         Trajectory100 t = m_planner.restToRest(List.of(
                 WaypointSE2.irrotational(

lib/src/main/java/org/team100/lib/localization/AprilTagRobotLocalizer.java

@@ -7,6 +7,7 @@
 import org.team100.lib.coherence.Takt;
 import org.team100.lib.experiments.Experiment;
 import org.team100.lib.experiments.Experiments;
+import org.team100.lib.geometry.Metrics;
 import org.team100.lib.logging.Level;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.LoggerFactory.BooleanLogger;
@@ -368,7 +369,7 @@ void estimateRobotPose(
                 continue;
             }
 
-            final double distanceM = distance(m_prevPose, pose);
+            final double distanceM = Metrics.translationalDistance(m_prevPose, pose);
             if (distanceM > VISION_CHANGE_TOLERANCE_M) {
                 // The new estimate is too far from the previous one: it's probably garbage.
                 m_prevPose = pose;
@@ -426,13 +427,4 @@ static double[] visionMeasurementStdDevs(double distanceM) {
                 Double.MAX_VALUE };
     }
 
-    ///////////////////////////////////////
-
-    /** Distance between pose translations. */
-    private static double distance(Pose2d a, Pose2d b) {
-        // the translation distance is a little quicker to calculate and we don't care
-        // about the "twist" curve measurement
-        return a.getTranslation().getDistance(b.getTranslation());
-    }
-
 }