From ecb1780db3a9329380a07cbe4d7d46760877ab64 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sat, 27 Dec 2025 19:39:31 -0800
Subject: [PATCH 1/5] trajectory WIP

---
 .../team100/lib/geometry/GeometryUtil.java    |   2 +-
 .../{Pose2dWithMotion.java => PathPoint.java} |  22 +-
 .../team100/lib/logging/LoggerFactory.java    |  16 +-
 .../org/team100/lib/state/ControlSE2.java     |   8 +-
 .../java/org/team100/lib/state/ModelSE2.java  |   8 +-
 .../tank/commands/FixedTrajectory.java        |   2 +-
 .../tank/commands/ToPoseWithTrajectory.java   |   2 +-
 .../java/org/team100/lib/trajectory/README.md |  29 ++-
 .../team100/lib/trajectory/Trajectory100.java |  19 +-
 .../team100/lib/trajectory/path/Path100.java  |  36 +--
 .../lib/trajectory/path/PathFactory.java      |  36 +--
 .../path/spline/HolonomicSpline.java          |  35 +--
 .../timing/CapsizeAccelerationConstraint.java |  10 +-
 .../trajectory/timing/ConstantConstraint.java |   8 +-
 .../trajectory/timing/DiamondConstraint.java  |  12 +-
 .../team100/lib/trajectory/timing/README.md   |   5 +-
 .../timing/SoftRegionConstraint.java          |  69 +++++
 .../timing/SwerveDriveDynamicsConstraint.java |  12 +-
 .../lib/trajectory/timing/TimedState.java     |   8 +-
 .../trajectory/timing/TimingConstraint.java   |  13 +-
 .../trajectory/timing/TorqueConstraint.java   |  12 +-
 .../trajectory/timing/TrajectoryFactory.java  |  24 +-
 .../trajectory/timing/TrajectoryRecycler.java |  20 ++
 .../timing/VelocityLimitRegionConstraint.java |  10 +-
 .../trajectory/timing/YawRateConstraint.java  |   8 +-
 .../TrajectoryVisualization.java              |   2 +-
 .../se2/FullStateControllerR3Test.java        |  40 +--
 .../se2/ReferenceControllerSE2Test.java       |   2 +-
 .../lib/geometry/GeometryUtilTest.java        | 121 +--------
 .../subsystems/swerve/SwerveControlTest.java  |  10 +-
 .../subsystems/swerve/SwerveModelTest.java    |  10 +-
 .../lib/trajectory/ParameterizationTest.java  |   5 +-
 .../lib/trajectory/PathToVectorSeries.java    |  12 +-
 .../lib/trajectory/Trajectory100Test.java     |  18 +-
 .../lib/trajectory/TrajectoryPlannerTest.java |  18 +-
 .../lib/trajectory/TrajectoryPlotter.java     |   1 +
 .../lib/trajectory/TrajectoryTest.java        |  10 +-
 .../trajectory/TrajectoryToVectorSeries.java  |   6 +-
 .../lib/trajectory/path/Path100Test.java      |  20 +-
 .../lib/trajectory/path/PathFactoryTest.java  |  46 ++--
 .../path/spline/HolonomicSplineTest.java      | 246 ++++++++++++------
 .../spline}/SplineToVectorSeries.java         |   6 +-
 ...CentripetalAccelerationConstraintTest.java |  12 +-
 .../timing/ConstantConstraintTest.java        |   6 +-
 .../timing/DiamondConstraintTest.java         |  14 +-
 .../timing/SoftRegionConstraintTest.java      |  77 ++++++
 .../SwerveDriveDynamicsConstraintTest.java    |  12 +-
 .../lib/trajectory/timing/TimedStateTest.java |   8 +-
 .../timing/TorqueConstraintTest.java          |  14 +-
 .../timing/TrajectoryFactoryTest.java         |  32 +--
 .../timing/TrajectoryRecyclerTest.java        |  10 +
 .../timing/TrajectoryVelocityProfileTest.java |  12 +-
 .../VelocityLimitRegionConstraintTest.java    |   6 +-
 .../timing/YawRateConstraintTest.java         |  10 +-
 54 files changed, 706 insertions(+), 506 deletions(-)
 rename lib/src/main/java/org/team100/lib/geometry/{Pose2dWithMotion.java => PathPoint.java} (88%)
 create mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
 create mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
 rename lib/src/test/java/org/team100/lib/trajectory/{ => path/spline}/SplineToVectorSeries.java (93%)
 create mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
 create mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java

diff --git a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java
index b0e217c1..7e9d75a1 100644
--- a/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java
+++ b/lib/src/main/java/org/team100/lib/geometry/GeometryUtil.java
@@ -375,7 +375,7 @@ public static Vector<N3> toVec3(Translation2d t) {
     /**
      * Change in heading per change in position.
      */
-    static double headingRatio(Pose2d p0, Pose2d p1) {
+    public static double headingRatio(Pose2d p0, Pose2d p1) {
         Rotation2d h0 = p0.getRotation();
         Rotation2d h1 = p1.getRotation();
         double d = Metrics.doubleGeodesicDistance(p0, p1);
diff --git a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
similarity index 88%
rename from lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java
rename to lib/src/main/java/org/team100/lib/geometry/PathPoint.java
index e5bea460..beac26d9 100644
--- a/lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java
+++ b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
@@ -4,8 +4,12 @@
 
 import edu.wpi.first.math.MathUtil;
 
-/** WaypointSE2 with heading rate and curvature. */
-public class Pose2dWithMotion {
+/**
+ * Represents a point on a path in SE(2).
+ * 
+ * Includes a WaypointSE2, heading rate, and curvature.
+ */
+public class PathPoint {
     private static final boolean DEBUG = false;
     /** Pose and course. */
     private final WaypointSE2 m_waypoint;
@@ -19,7 +23,7 @@ public class Pose2dWithMotion {
      * @param headingRateRad_M change in heading, per meter traveled
      * @param curvatureRad_M   change in course per meter traveled.
      */
-    public Pose2dWithMotion(
+    public PathPoint(
             WaypointSE2 waypoint,
             double headingRateRad_M,
             double curvatureRad_M) {
@@ -28,7 +32,7 @@ public Pose2dWithMotion(
         m_curvatureRad_M = curvatureRad_M;
     }
 
-    public WaypointSE2 getPose() {
+    public WaypointSE2 waypoint() {
         return m_waypoint;
     }
 
@@ -51,8 +55,8 @@ public double getCurvatureRad_M() {
      * 
      * Not a constant-twist arc.
      */
-    public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) {
-        return new Pose2dWithMotion(
+    public PathPoint interpolate(PathPoint other, double x) {
+        return new PathPoint(
                 GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x),
                 MathUtil.interpolate(m_headingRateRad_M, other.m_headingRateRad_M, x),
                 Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x));
@@ -65,18 +69,18 @@ public Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) {
      * 
      * Always non-negative.
      */
-    public double distanceCartesian(Pose2dWithMotion other) {
+    public double distanceCartesian(PathPoint other) {
         return Metrics.translationalDistance(m_waypoint.pose(), other.m_waypoint.pose());
     }
 
     public boolean equals(Object other) {
-        if (!(other instanceof Pose2dWithMotion)) {
+        if (!(other instanceof PathPoint)) {
             if (DEBUG)
                 System.out.println("wrong type");
             return false;
         }
 
-        Pose2dWithMotion p2dwc = (Pose2dWithMotion) other;
+        PathPoint p2dwc = (PathPoint) other;
         if (!m_waypoint.equals(p2dwc.m_waypoint)) {
             if (DEBUG)
                 System.out.println("wrong waypoint");
diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java
index aba430aa..abb496ae 100644
--- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java
+++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java
@@ -11,7 +11,7 @@
 import org.team100.lib.geometry.DeltaSE2;
 import org.team100.lib.geometry.GlobalVelocityR2;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.localization.Blip24;
 import org.team100.lib.logging.primitive.PrimitiveLogger;
@@ -451,7 +451,7 @@ public Rotation2dLogger rotation2dLogger(Level level, String leaf) {
 
     public class TimedStateLogger {
         private final Level m_level;
-        private final Pose2dWithMotionLogger m_pose2dWithMotionLogger;
+        private final PathPointLogger m_pose2dWithMotionLogger;
         private final DoubleLogger m_timeLogger;
         private final DoubleLogger m_velocityLogger;
         private final DoubleLogger m_accelLogger;
@@ -501,28 +501,28 @@ public PoseWithCurvatureLogger poseWithCurvatureLogger(Level level, String leaf)
         return new PoseWithCurvatureLogger(level, leaf);
     }
 
-    public class Pose2dWithMotionLogger {
+    public class PathPointLogger {
         private final Level m_level;
         private final Pose2dLogger m_pose2dLogger;
         private final Rotation2dLogger m_rotation2dLogger;
 
-        Pose2dWithMotionLogger(Level level, String leaf) {
+        PathPointLogger(Level level, String leaf) {
             m_level = level;
             m_pose2dLogger = pose2dLogger(level, join(leaf, "pose"));
             m_rotation2dLogger = rotation2dLogger(level, join(leaf, "course"));
         }
 
-        public void log(Supplier<Pose2dWithMotion> vals) {
+        public void log(Supplier<PathPoint> vals) {
             if (!allow(m_level))
                 return;
-            WaypointSE2 val = vals.get().getPose();
+            WaypointSE2 val = vals.get().waypoint();
             m_pose2dLogger.log(val::pose);
             m_rotation2dLogger.log(() -> val.course().toRotation());
         }
     }
 
-    public Pose2dWithMotionLogger pose2dWithMotionLogger(Level level, String leaf) {
-        return new Pose2dWithMotionLogger(level, leaf);
+    public PathPointLogger pose2dWithMotionLogger(Level level, String leaf) {
+        return new PathPointLogger(level, leaf);
     }
 
     public class Twist2dLogger {
diff --git a/lib/src/main/java/org/team100/lib/state/ControlSE2.java b/lib/src/main/java/org/team100/lib/state/ControlSE2.java
index 9793855c..66c690a0 100644
--- a/lib/src/main/java/org/team100/lib/state/ControlSE2.java
+++ b/lib/src/main/java/org/team100/lib/state/ControlSE2.java
@@ -127,12 +127,12 @@ public Control100 theta() {
      * Correctly accounts for centripetal acceleration.
      */
     public static ControlSE2 fromTimedState(TimedState timedPose) {
-        double xx = timedPose.state().getPose().pose().getTranslation().getX();
-        double yx = timedPose.state().getPose().pose().getTranslation().getY();
-        double thetax = timedPose.state().getPose().pose().getRotation().getRadians();
+        double xx = timedPose.state().waypoint().pose().getTranslation().getX();
+        double yx = timedPose.state().waypoint().pose().getTranslation().getY();
+        double thetax = timedPose.state().waypoint().pose().getRotation().getRadians();
 
         double velocityM_s = timedPose.velocityM_S();
-        Rotation2d course = timedPose.state().getPose().course().toRotation();
+        Rotation2d course = timedPose.state().waypoint().course().toRotation();
         double xv = course.getCos() * velocityM_s;
         double yv = course.getSin() * velocityM_s;
         double thetav = timedPose.state().getHeadingRateRad_M() * velocityM_s;
diff --git a/lib/src/main/java/org/team100/lib/state/ModelSE2.java b/lib/src/main/java/org/team100/lib/state/ModelSE2.java
index f4dd0a2e..48362339 100644
--- a/lib/src/main/java/org/team100/lib/state/ModelSE2.java
+++ b/lib/src/main/java/org/team100/lib/state/ModelSE2.java
@@ -1,7 +1,7 @@
 package org.team100.lib.state;
 
 import org.team100.lib.geometry.GlobalVelocityR2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
@@ -136,13 +136,13 @@ public Model100 theta() {
      * Transform timed pose into swerve state.
      */
     public static ModelSE2 fromTimedState(TimedState timedPose) {
-        Pose2dWithMotion state = timedPose.state();
-        WaypointSE2 pose = state.getPose();
+        PathPoint state = timedPose.state();
+        WaypointSE2 pose = state.waypoint();
         Translation2d translation = pose.pose().getTranslation();
         double xx = translation.getX();
         double yx = translation.getY();
         double thetax = pose.pose().getRotation().getRadians();
-        Rotation2d course = state.getPose().course().toRotation();
+        Rotation2d course = state.waypoint().course().toRotation();
         double velocityM_s = timedPose.velocityM_S();
         double xv = course.getCos() * velocityM_s;
         double yv = course.getSin() * velocityM_s;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java
index b2fd65f1..901faa00 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java
@@ -63,7 +63,7 @@ public void execute() {
         // next for feedforward (and selecting K)
         TimedState next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S);
         Pose2d currentPose = m_drive.getPose();
-        Pose2d poseReference = current.state().getPose().pose();
+        Pose2d poseReference = current.state().waypoint().pose();
         double velocityReference = next.velocityM_S();
         double omegaReference = next.velocityM_S() * next.state().getHeadingRateRad_M();
         ChassisSpeeds speeds = m_controller.calculate(
diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java
index f21d5e3a..77df5c2d 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java
@@ -70,7 +70,7 @@ public void execute() {
             // next for feedforward (and selecting K)
             TimedState next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S);
             Pose2d currentPose = m_drive.getPose();
-            Pose2d poseReference = current.state().getPose().pose();
+            Pose2d poseReference = current.state().waypoint().pose();
             double velocityReference = next.velocityM_S();
             double omegaReference = next.velocityM_S() * next.state().getHeadingRateRad_M();
             ChassisSpeeds speeds = m_controller.calculate(
diff --git a/lib/src/main/java/org/team100/lib/trajectory/README.md b/lib/src/main/java/org/team100/lib/trajectory/README.md
index c2941c93..be9c93fc 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/README.md
+++ b/lib/src/main/java/org/team100/lib/trajectory/README.md
@@ -5,7 +5,8 @@ This package represents an path in SE(2) and a schedule.
 `Trajectory100` is adapted from 254.  Trajectories are holonomic,
 i.e. handle course and heading separately, so they have four dimensions (x, y, heading, course), and "point" below means four-dimensional holonomic state.
 
-A good entry point to experiment with trajectories is `TrajectoryPlanner`.  Try `restToRest()` between two poses.
+The main entry point is `TrajectoryPlanner`.  Try `restToRest()` with a list of waypoints.
+Very often our trajectories have only two waypoints: the start and end.
 
 The process of constructing a trajectory has three stages.
 
@@ -21,7 +22,7 @@ you want to travel through.  A waypoint includes
 The "scale" parameter above determines the "straightness" of the curve at each knot.
 In our implementation, the curvature at each knot is zero.
 
-3. Construct a list of points (`Pose2dWithMotion`) along the splines, such that straight lines
+3. Construct a list of points (`PathPoint`) along the splines, such that straight lines
 connecting the points ("secant lines") don't deviate too much from the true spline, and aren't
 too far apart from each other. (This uses recursive bisection.)  These points will
 be close together where the curvature is high, and far apart along straighter sections
@@ -29,8 +30,22 @@ of the spline.  Each point is a `WaypointSE2` and also the spatial rate of headi
 along the path.  These steps are performed by `PathFactory`, producing `Path100`.
 
 4. Using a list of kinodynamic constraints (implementations of `TimingConstraint`),
-assign a time for each point.  The resulting list of `TimedState` is created by `TrajectoryFactory`, producing `Trajectory100`.
-
-To use a trajectory, you `sample()` it, with time (in seconds) as the parameter.  The resulting `TimedState` is interpolated from the list of `TimedState` above.
-
-If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains, it will work well enough to set the course and heading to be the same at each waypoint.
\ No newline at end of file
+assign a time for each point.  This process is identical to the what a `TimedProfile` does,
+but instead of a one-dimensional path, the path is a curve embedded in SE(2), so the
+constraints are aware of the extra dimensions.  (It would be possible to unify the
+trajectory scheduler and timed profile code someday.)  The resulting list of
+`TimedState` is created by `TrajectoryFactory`, producing `Trajectory100`.
+
+To use a trajectory, you `sample()` it, with time (in seconds) as the parameter.
+The resulting `TimedState` is interpolated from the list of `TimedState` above.
+
+One thing to keep in mind: the samples are interpolated *linearly* along the secant lines
+from step 2. This means that the actual instantaneous curvature of the path between samples
+is zero almost all the time, but the sampled curvature is not zero.  If you try to compute
+acceleration from adjacent samples (e.g. for feedforward dynamics), you'll not find
+any cross-track acceleration at all.  Instead you should use the curvature, which is
+interpolated correclty, with the velocity, which is also correctly interpolated.
+This pattern is implemented correctly in `ControlSE2.fromTimedState()`.
+
+If you want to use these trajectories for non-holonomic (e.g. "tank") drivetrains,
+it will work well enough to set the course and heading to be the same at each waypoint.
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
index 1c7f59a7..18fd95f1 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
@@ -3,7 +3,7 @@
 import java.util.ArrayList;
 import java.util.List;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.trajectory.timing.TimedState;
 
@@ -106,11 +106,24 @@ public void dump() {
         System.out.println("i, t, v, a, k, x, y");
         for (int i = 0; i < length(); ++i) {
             TimedState ts = getPoint(i);
-            Pose2dWithMotion pwm = ts.state();
-            WaypointSE2 w = pwm.getPose();
+            PathPoint pwm = ts.state();
+            WaypointSE2 w = pwm.waypoint();
             Pose2d p = w.pose();
             System.out.printf("%d, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
                     i, ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(), p.getX(), p.getY());
         }
     }
+
+        /** For cutting-and-pasting into a spreadsheet */
+        public void tdump() {
+            System.out.println("t, v, a, k, x, y");
+            for (double t = 0; t < duration(); t += 0.02) {
+                TimedState ts = sample(t);
+                PathPoint pwm = ts.state();
+                WaypointSE2 w = pwm.waypoint();
+                Pose2d p = w.pose();
+                System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
+                        ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(), p.getX(), p.getY());
+            }
+        }
 }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java
index acc0f7d9..851cd5bf 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/path/Path100.java
@@ -4,7 +4,7 @@
 import java.util.List;
 
 import org.team100.lib.geometry.Metrics;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 
 import edu.wpi.first.math.geometry.Twist2d;
 
@@ -21,14 +21,14 @@ public class Path100 {
     // scheduler can't see
     // TODO: make this a constructor parameter.
     private static final double INTERPOLATION_LIMIT = 0.3;
-    private final List<Pose2dWithMotion> m_points;
+    private final List<PathPoint> m_points;
     /**
      * Translational distance, just the xy plane, not the Twist arc
      * or anything else, just xy distance.
      */
     private final double[] m_distances;
 
-    public Path100(final List<Pose2dWithMotion> states) {
+    public Path100(final List<PathPoint> states) {
         int n = states.size();
         m_points = new ArrayList<>(n);
         m_distances = new double[n];
@@ -39,9 +39,9 @@ public Path100(final List<Pose2dWithMotion> states) {
         m_points.add(states.get(0));
         for (int i = 1; i < n; ++i) {
             m_points.add(states.get(i));
-            Pose2dWithMotion p0 = getPoint(i - 1);
-            Pose2dWithMotion p1 = getPoint(i);
-            double dist = Metrics.translationalDistance(p0.getPose().pose(), p1.getPose().pose());
+            PathPoint p0 = getPoint(i - 1);
+            PathPoint p1 = getPoint(i);
+            double dist = Metrics.translationalDistance(p0.waypoint().pose(), p1.waypoint().pose());
             m_distances[i] = m_distances[i - 1] + dist;
         }
     }
@@ -54,7 +54,7 @@ public int length() {
         return m_points.size();
     }
 
-    public Pose2dWithMotion getPoint(int index) {
+    public PathPoint getPoint(int index) {
         if (m_points.isEmpty())
             return null;
         return m_points.get(index);
@@ -75,7 +75,7 @@ public double getMaxDistance() {
      * 
      * @param distance in meters, always a non-negative number.
      */
-    public Pose2dWithMotion sample(double distance) {
+    public PathPoint sample(double distance) {
         if (distance >= getMaxDistance()) {
             // off the end
             return getPoint(length() - 1);
@@ -86,19 +86,19 @@ public Pose2dWithMotion sample(double distance) {
         }
         for (int i = 1; i < length(); ++i) {
             // walk through the points to bracket the desired distance
-            Pose2dWithMotion p0 = getPoint(i - 1);
-            Pose2dWithMotion p1 = getPoint(i);
+            PathPoint p0 = getPoint(i - 1);
+            PathPoint p1 = getPoint(i);
             double d0 = m_distances[i - 1];
             double d1 = m_distances[i];
             double d = d1 - d0;
             if (d1 >= distance) {
                 // Found the bracket.
                 double s = (distance - d0) / d;
-                Pose2dWithMotion lerp = p0.interpolate(p1, s);
+                PathPoint lerp = p0.interpolate(p1, s);
                 // disallow corners
-                Twist2d t0 = p0.getPose().course().minus(lerp.getPose().course());
+                Twist2d t0 = p0.waypoint().course().minus(lerp.waypoint().course());
                 double l0 = Metrics.l2Norm(t0);
-                Twist2d t1 = p1.getPose().course().minus(lerp.getPose().course());
+                Twist2d t1 = p1.waypoint().course().minus(lerp.waypoint().course());
                 double l1 = Metrics.l2Norm(t1);
                 if (Math.max(l0, l1) > INTERPOLATION_LIMIT)
                     System.out.printf(
@@ -111,8 +111,8 @@ public Pose2dWithMotion sample(double distance) {
     }
 
     /** Just returns the list of points with no further sampling. */
-    public Pose2dWithMotion[] resample() {
-        return m_points.toArray(Pose2dWithMotion[]::new);
+    public PathPoint[] resample() {
+        return m_points.toArray(PathPoint[]::new);
     }
 
     @Override
@@ -138,7 +138,7 @@ public String toString() {
      * Samples the entire path evenly by distance. Since the spline parameterizer
      * now contains a pathwise distance limit, you shouldn't need this anymore.
      */
-    public Pose2dWithMotion[] resample(double step) {
+    public PathPoint[] resample(double step) {
         double maxDistance = getMaxDistance();
         if (maxDistance == 0)
             throw new IllegalArgumentException("max distance must be greater than zero");
@@ -146,12 +146,12 @@ public Pose2dWithMotion[] resample(double step) {
         if (DEBUG)
             System.out.printf("resample max distance %f step %f num states %d f %f\n",
                     maxDistance, step, num_states, maxDistance / step);
-        Pose2dWithMotion[] samples = new Pose2dWithMotion[num_states];
+        PathPoint[] samples = new PathPoint[num_states];
         for (int i = 0; i < num_states; ++i) {
             // the dtheta and curvature come from here and are never changed.
             // the values here are just interpolated from the original values.
             double d = Math.min(i * step, maxDistance);
-            Pose2dWithMotion state = sample(d);
+            PathPoint state = sample(d);
             if (state == null) continue; 
             if (DEBUG)
                 System.out.printf("RESAMPLE: i=%d d=%f state=%s\n", i, d, state);
diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java
index 2549b87c..0c03ee17 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/path/PathFactory.java
@@ -5,7 +5,7 @@
 
 import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.geometry.Metrics;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.trajectory.path.spline.HolonomicSpline;
 
@@ -69,7 +69,7 @@ public Path100 fromWaypoints(List<WaypointSE2> waypoints) {
     ///
     ///
     ///
-  
+
     /**
      * Make a list of splines with the waypoints as knots.
      */
@@ -82,7 +82,7 @@ private List<HolonomicSpline> splinesFromWaypoints(List<WaypointSE2> waypoints)
     }
 
     /**
-     * Converts a spline into a list of Pose2dWithMotion.
+     * Converts a spline into a list of PathPoint.
      * 
      * The points are chosen so that the secant line between the points is within
      * the specified tolerance (dx, dy, dtheta) of the actual spline.
@@ -90,9 +90,9 @@ private List<HolonomicSpline> splinesFromWaypoints(List<WaypointSE2> waypoints)
      * The trajectory scheduler consumes these points, interpolating between them
      * with straight lines.
      */
-    List<Pose2dWithMotion> samplesFromSpline(HolonomicSpline spline) {
-        List<Pose2dWithMotion> result = new ArrayList<>();
-        result.add(spline.getPose2dWithMotion(0.0));
+    List<PathPoint> samplesFromSpline(HolonomicSpline spline) {
+        List<PathPoint> result = new ArrayList<>();
+        result.add(spline.getPathPoint(0.0));
         getSegmentArc(spline, result, 0, 1);
         return result;
     }
@@ -104,16 +104,16 @@ public Path100 fromSplines(List<? extends HolonomicSpline> splines) {
     /**
      * For testing only. Do not call this directly
      */
-    public List<Pose2dWithMotion> samplesFromSplines(List<? extends HolonomicSpline> splines) {
-        List<Pose2dWithMotion> result = new ArrayList<>();
+    public List<PathPoint> samplesFromSplines(List<? extends HolonomicSpline> splines) {
+        List<PathPoint> result = new ArrayList<>();
         if (splines.isEmpty())
             return result;
-        result.add(splines.get(0).getPose2dWithMotion(0.0));
+        result.add(splines.get(0).getPathPoint(0.0));
         for (int i = 0; i < splines.size(); i++) {
             HolonomicSpline s = splines.get(i);
             if (DEBUG)
                 System.out.printf("SPLINE:\n%d\n%s\n", i, s);
-            List<Pose2dWithMotion> samples = samplesFromSpline(s);
+            List<PathPoint> samples = samplesFromSpline(s);
             // the sample at the end of the previous spline is the same as the one for the
             // beginning of the next, so don't include it twice.
             samples.remove(0);
@@ -131,13 +131,13 @@ public List<Pose2dWithMotion> samplesFromSplines(List<? extends HolonomicSpline>
      */
     private void getSegmentArc(
             HolonomicSpline spline,
-            List<Pose2dWithMotion> rv,
+            List<PathPoint> rv,
             double s0,
             double s1) {
-        Pose2d p0 = spline.getPose2d(s0);
+        Pose2d p0 = spline.getPathPoint(s0).waypoint().pose();
         double shalf = (s0 + s1) / 2;
-        Pose2d phalf = spline.getPose2d(shalf);
-        Pose2d p1 = spline.getPose2d(s1);
+        Pose2d phalf = spline.getPathPoint(shalf).waypoint().pose();
+        Pose2d p1 = spline.getPathPoint(s1).waypoint().pose();
 
         // twist from p0 to p1
         Twist2d twist_full = p0.log(p1);
@@ -149,9 +149,9 @@ private void getSegmentArc(
         Transform2d error = phalf_predicted.minus(phalf);
 
         // also prohibit large changes in direction between points
-        Pose2dWithMotion p20 = spline.getPose2dWithMotion(s0);
-        Pose2dWithMotion p21 = spline.getPose2dWithMotion(s1);
-        Twist2d p2t = p20.getPose().course().minus(p21.getPose().course());
+        PathPoint p20 = spline.getPathPoint(s0);
+        PathPoint p21 = spline.getPathPoint(s1);
+        Twist2d p2t = p20.waypoint().course().minus(p21.waypoint().course());
 
         // note the extra conditions to avoid points too far apart.
         // checks both translational and l2 norms
@@ -167,7 +167,7 @@ private void getSegmentArc(
             getSegmentArc(spline, rv, shalf, s1);
         } else {
             // midpoint is close enough, so add the endpoint
-            rv.add(spline.getPose2dWithMotion(s1));
+            rv.add(spline.getPathPoint(s1));
         }
     }
 }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java
index 56644668..579396fb 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java
@@ -2,7 +2,7 @@
 
 import org.team100.lib.geometry.DirectionSE2;
 import org.team100.lib.geometry.Metrics;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -61,6 +61,9 @@ public class HolonomicSpline {
     public HolonomicSpline(WaypointSE2 p0, WaypointSE2 p1) {
         // Translation distance in the xy plane.
         double distance = Metrics.translationalDistance(p0.pose(), p1.pose());
+        if (distance < 1e-6)
+            throw new IllegalArgumentException("splines must cover xy distance");
+
         if (DEBUG)
             System.out.printf("distance %f\n", distance);
         double scale0 = p0.scale() * distance;
@@ -109,17 +112,21 @@ public String toString() {
     }
 
     /**
-     * TODO: eliminate the waypoint here, for sure eliminate the scale.
+     * TODO: remove the "1" scale here
      * 
      * @param s [0,1]
      */
-    public Pose2dWithMotion getPose2dWithMotion(double s) {
-        return new Pose2dWithMotion(
-                new WaypointSE2(getPose2d(s), getCourse(s), 1), // <<< TODO: remove the "1"
+    public PathPoint getPathPoint(double s) {
+        return new PathPoint(
+                new WaypointSE2(
+                        new Pose2d(new Translation2d(x(s), y(s)), getHeading(s)),
+                        getCourse(s), 1),
                 getDHeadingDs(s),
                 getCurvature(s));
     }
 
+    ////////////////////////////////////////////////////////////////////////
+
     /**
      * Direction of motion in SE(2). Includes both cartesian dimensions and also the
      * rotation dimension. This is exactly a unit-length twist in the motion
@@ -129,15 +136,11 @@ private DirectionSE2 getCourse(double s) {
         double dx = dx(s);
         double dy = dy(s);
         double dtheta = dtheta(s);
+        if (DEBUG)
+            System.out.printf("%f %f %f\n", dx, dy, dtheta);
         return new DirectionSE2(dx, dy, dtheta);
     }
 
-    public Pose2d getPose2d(double s) {
-        return new Pose2d(new Translation2d(x(s), y(s)), getHeading(s));
-    }
-
-    ////////////////////////////////////////////////////////////////////////
-
     private double getDHeading(double s) {
         return m_heading.getVelocity(s);
     }
@@ -148,12 +151,12 @@ private double getDHeading(double s) {
      * 
      * TODO: elsewhere this is combined with R2 pathwise velocity, so this is wrong.
      */
-    public double getDHeadingDs(double s) {
+    double getDHeadingDs(double s) {
         return getDHeading(s) / getVelocity(s);
     }
 
     /** x at s */
-    public double x(double s) {
+    double x(double s) {
         return m_x.getPosition(s);
     }
 
@@ -169,7 +172,7 @@ private Rotation2d getHeading(double s) {
     }
 
     /** dx/ds */
-    public double dx(double s) {
+    double dx(double s) {
         return m_x.getVelocity(s);
     }
 
@@ -184,7 +187,7 @@ public double dx(double s) {
     }
 
     /** d^2x/ds^2 */
-    public double ddx(double s) {
+    double ddx(double s) {
         return m_x.getAcceleration(s);
     }
 
@@ -221,7 +224,7 @@ private double getVelocity(double s) {
      * Note the denominator is distance in this case, not the parameter, p.
      * but the argument to this function *is* the parameter, s. :-)
      */
-    public double getCurvature(double s) {
+    double getCurvature(double s) {
         double dx = dx(s);
         double dy = dy(s);
         double ddx = ddx(s);
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java
index 3dc01d46..51ba571a 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.tuning.Mutable;
@@ -48,14 +48,14 @@ public CapsizeAccelerationConstraint(LoggerFactory parent, double centripetal, d
      * If the curvature is zero, this will return infinity.
      */
     @Override
-    public double maxV(final Pose2dWithMotion state) {
+    public double maxV(final PathPoint state) {
         double radius = 1 / Math.abs(state.getCurvatureRad_M());
         // abs is used here to make sure sqrt is happy.
         return Math.sqrt(Math.abs(m_maxCentripetalAccel * m_scale.getAsDouble() * radius));
     }
 
     @Override
-    public double maxAccel(Pose2dWithMotion state, double velocity) {
+    public double maxAccel(PathPoint state, double velocity) {
         double alongsq = alongSq(state, velocity);
         if (alongsq < 0) {
             if (DEBUG)
@@ -66,7 +66,7 @@ public double maxAccel(Pose2dWithMotion state, double velocity) {
     }
 
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         double alongsq = alongSq(state, velocity);
         if (alongsq < 0) {
             if (DEBUG)
@@ -90,7 +90,7 @@ public double maxDecel(Pose2dWithMotion state, double velocity) {
      * so
      * along = sqrt(total^2 - v^4/r^2)
      */
-    private double alongSq(Pose2dWithMotion state, double velocity) {
+    private double alongSq(PathPoint state, double velocity) {
         double radius = 1 / Math.abs(state.getCurvatureRad_M());
         double actualCentripetalAccel = velocity * velocity / radius;
         if (DEBUG)
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java
index 80007975..9b0c4ecb 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/ConstantConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.tuning.Mutable;
@@ -21,17 +21,17 @@ public ConstantConstraint(LoggerFactory log, double vScale, double aScale, Swerv
     }
 
     @Override
-    public double maxV(Pose2dWithMotion state) {
+    public double maxV(PathPoint state) {
         return m_maxVelocity.getAsDouble();
     }
 
     @Override
-    public double maxAccel(Pose2dWithMotion state, double velocityM_S) {
+    public double maxAccel(PathPoint state, double velocityM_S) {
         return m_maxAccel.getAsDouble();
     }
     
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         return -m_maxAccel.getAsDouble();
     }
 }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java
index 8166670b..2cba0d64 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/DiamondConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.tuning.Mutable;
 
@@ -33,9 +33,9 @@ public DiamondConstraint(LoggerFactory parent, double maxVX, double maxVY, doubl
     }
 
     @Override
-    public double maxV(Pose2dWithMotion state) {
-        Rotation2d course = state.getPose().course().toRotation();
-        Rotation2d heading = state.getPose().pose().getRotation();
+    public double maxV(PathPoint state) {
+        Rotation2d course = state.waypoint().course().toRotation();
+        Rotation2d heading = state.waypoint().pose().getRotation();
         Rotation2d strafe = course.minus(heading);
         // a rhombus is a superellipse with exponent 1
         // https://en.wikipedia.org/wiki/Superellipse
@@ -45,13 +45,13 @@ public double maxV(Pose2dWithMotion state) {
     }
 
     @Override
-    public double maxAccel(Pose2dWithMotion state, double velocityM_S) {
+    public double maxAccel(PathPoint state, double velocityM_S) {
         // TODO: this should also have a diamond shape
         return m_maxAccel.getAsDouble();
     }
 
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         return -m_maxAccel.getAsDouble();
     }
 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/README.md b/lib/src/main/java/org/team100/lib/trajectory/timing/README.md
index 8448596b..944b073b 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/README.md
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/README.md
@@ -1,3 +1,6 @@
 # lib.trajectory.timing
 
-This package helps to create trajectory schedules.
\ No newline at end of file
+This package helps to create trajectory schedules.
+
+The main entry point is `TrajectoryFactory`, which uses a list of
+`TimingConstraint` to construct a schedule through a path.
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
new file mode 100644
index 00000000..c875b23a
--- /dev/null
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
@@ -0,0 +1,69 @@
+package org.team100.lib.trajectory.timing;
+
+import org.team100.lib.geometry.PathPoint;
+import org.team100.lib.util.Math100;
+
+import edu.wpi.first.math.geometry.Translation2d;
+
+/**
+ * Limits acceleration near a point.
+ * 
+ * There's a minimum at the point, and a maximum at the edge of a circle
+ * surrounding it, interpolated using the sqrt of the distance.  This
+ * provides roughly constant jerk.
+ * 
+ * Because the path is discretized by length, and because acceleration is
+ * constant between points, the interpolated samples have a rather jerky
+ * acceleration curve when viewed on the time axis -- it's not really a
+ * smooth jerk limit.
+ * 
+ * https://docs.google.com/spreadsheets/d/1sbB-zTBUjRRlWHaWXe-V1ZDhAZCFwItVVO1x3LmZ4B4/edit?gid=691127145#gid=691127145
+ */
+public class SoftRegionConstraint implements TimingConstraint {
+    private final Translation2d m_center;
+    private final double m_radius;
+    private final double m_min;
+    private final double m_max;
+
+    /**
+     * @param center of the soft region
+     * @param radius of the soft region
+     * @param min    accel/decel at the center
+     * @param max    accel/decel at the edge
+     */
+    public SoftRegionConstraint(
+            Translation2d center, double radius, double min, double max) {
+        m_center = center;
+        m_radius = radius;
+        m_min = min;
+        m_max = max;
+    }
+
+    @Override
+    public double maxV(PathPoint state) {
+        return Double.POSITIVE_INFINITY;
+    }
+
+    @Override
+    public double maxAccel(PathPoint state, double velocityM_S) {
+        double s = near(state);
+        if (s < 1)
+            return Math100.interpolate(m_min, m_max, Math.sqrt(s));
+        return Double.POSITIVE_INFINITY;
+    }
+
+    @Override
+    public double maxDecel(PathPoint state, double velocityM_S) {
+        double s = near(state);
+        if (s < 1)
+            return -1.0 * Math100.interpolate(m_min, m_max, Math.sqrt(s));
+        return Double.NEGATIVE_INFINITY;
+    }
+
+    /** distance to the center compared with the radius */
+    private double near(PathPoint state) {
+        Translation2d translation = state.waypoint().pose().getTranslation();
+        return translation.getDistance(m_center) / m_radius;
+    }
+
+}
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java
index 62745536..d3bac858 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.subsystems.swerve.kinodynamics.limiter.SwerveUtil;
@@ -42,11 +42,11 @@ public SwerveDriveDynamicsConstraint(
      * speed allowed (m/s) that maintains the target spatial heading rate.
      */
     @Override
-    public double maxV(Pose2dWithMotion state) {
+    public double maxV(PathPoint state) {
         // First check instantaneous velocity and compute a limit based on drive
         // velocity.
-        Rotation2d course = state.getPose().course().toRotation();
-        Rotation2d heading = state.getPose().pose().getRotation();
+        Rotation2d course = state.waypoint().course().toRotation();
+        Rotation2d heading = state.waypoint().pose().getRotation();
         Rotation2d course_local = course.minus(heading);
         double vx = course_local.getCos();
         double vy = course_local.getSin();
@@ -79,7 +79,7 @@ public double maxV(Pose2dWithMotion state) {
      * @see SwerveUtil.getAccelLimit()
      */
     @Override
-    public double maxAccel(Pose2dWithMotion state, double velocity) {
+    public double maxAccel(PathPoint state, double velocity) {
         if (Double.isNaN(velocity))
             throw new IllegalArgumentException();
         double maxAccel = SwerveUtil.minAccel(m_limits, 1, 1, velocity);
@@ -92,7 +92,7 @@ public double maxAccel(Pose2dWithMotion state, double velocity) {
     }
 
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         // min accel is stronger than max accel
         return -1.0 * maxA();
     }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
index 269870ff..f638a184 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.util.Math100;
 
 /**
@@ -11,7 +11,7 @@
  */
 public class TimedState {
     private static final boolean DEBUG = false;
-    private final Pose2dWithMotion m_state;
+    private final PathPoint m_state;
     /** Time we achieve this state. */
     private final double m_timeS;
     /** Instantaneous pathwise velocity, m/s. */
@@ -23,7 +23,7 @@ public class TimedState {
     private final double m_accelM_S_S;
 
     public TimedState(
-            Pose2dWithMotion state,
+            PathPoint state,
             double t,
             double velocity,
             double acceleration) {
@@ -33,7 +33,7 @@ public TimedState(
         m_accelM_S_S = acceleration;
     }
 
-    public Pose2dWithMotion state() {
+    public PathPoint state() {
         return m_state;
     }
 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java
index 2d7a18d4..16007f26 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java
@@ -1,12 +1,17 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 
 /**
  * Timing constraints govern the assignment of a schedule to a path, creating a
  * trajectory. Different implementations focus on different aspects, e.g.
  * tippiness, wheel slip, etc. Different maneuvers may want different
  * constraints, e.g. some should be slow and precise, others fast and risky.
+ * 
+ * Note that this interface doesn't support jerk limiting in a simple way.
+ * If you want to limit jerk at the start and/or end of a trajectory,
+ * you can implement that using the acceleration and deceleration limits,
+ * with a constraint that is aware of the endpoint locations.
  */
 public interface TimingConstraint {
     /**
@@ -14,19 +19,19 @@ public interface TimingConstraint {
      * 
      * Always positive.
      */
-    double maxV(Pose2dWithMotion state);
+    double maxV(PathPoint state);
 
     /**
      * Maximum allowed pathwise acceleration, m/s^2.
      * 
      * Always positive.
      */
-    double maxAccel(Pose2dWithMotion state, double velocityM_S);
+    double maxAccel(PathPoint state, double velocityM_S);
 
     /**
      * Maximum allowed pathwise deceleration, m/s^2.
      * 
      * Always negative.
      */
-    double maxDecel(Pose2dWithMotion state, double velocityM_S);
+    double maxDecel(PathPoint state, double velocityM_S);
 }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java
index 2bd4aa25..42f734b1 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TorqueConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -46,24 +46,24 @@ public TorqueConstraint(double maxTorque) {
     }
 
     @Override
-    public double maxV(Pose2dWithMotion state) {
+    public double maxV(PathPoint state) {
         // Do not constrain velocity.
         return Double.POSITIVE_INFINITY;
     }
 
     @Override
     public double maxAccel(
-            Pose2dWithMotion state, double velocityM_S) {
+            PathPoint state, double velocityM_S) {
         return getA(state);
     }
 
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         return -getA(state);
     }
 
-    private double getA(Pose2dWithMotion state) {
-        WaypointSE2 pose = state.getPose();
+    private double getA(PathPoint state) {
+        WaypointSE2 pose = state.waypoint();
         Rotation2d course = pose.course().toRotation();
         // acceleration unit vector
         Translation2d u = new Translation2d(1.0, course);
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
index 3b61bceb..663d2768 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
@@ -3,7 +3,7 @@
 import java.util.ArrayList;
 import java.util.List;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.path.Path100;
 import org.team100.lib.util.Math100;
@@ -30,7 +30,7 @@ public TrajectoryFactory(List<TimingConstraint> constraints) {
      * Samples the path, then assigns a time to each sample.
      */
     public Trajectory100 fromPath(Path100 path, double start_vel, double end_vel) {
-        Pose2dWithMotion[] samples = getSamples(path);
+        PathPoint[] samples = getSamples(path);
         return fromSamples(samples, start_vel, end_vel);
     }
 
@@ -41,7 +41,7 @@ public Trajectory100 fromPath(Path100 path, double start_vel, double end_vel) {
     /**
      * Return an array of poses from the path.
      */
-    private Pose2dWithMotion[] getSamples(Path100 path) {
+    private PathPoint[] getSamples(Path100 path) {
         return path.resample();
     }
 
@@ -51,7 +51,7 @@ private Pose2dWithMotion[] getSamples(Path100 path) {
      * Output is these same samples with time.
      */
     private Trajectory100 fromSamples(
-            Pose2dWithMotion[] samples,
+            PathPoint[] samples,
             double start_vel,
             double end_vel) {
         double[] distances = distances(samples);
@@ -66,7 +66,7 @@ private Trajectory100 fromSamples(
      * Creates a list of timed states.
      */
     private List<TimedState> timedStates(
-            Pose2dWithMotion[] samples, double[] velocities, double[] accels, double[] runningTime) {
+            PathPoint[] samples, double[] velocities, double[] accels, double[] runningTime) {
         int n = samples.length;
         List<TimedState> timedStates = new ArrayList<>(n);
         for (int i = 0; i < n; ++i) {
@@ -113,7 +113,7 @@ private double[] accels(double[] distances, double[] velocities) {
      * constraints.
      */
     private double[] velocities(
-            Pose2dWithMotion[] samples, double start_vel, double end_vel, double[] distances) {
+            PathPoint[] samples, double start_vel, double end_vel, double[] distances) {
         double velocities[] = new double[samples.length];
         forward(samples, start_vel, distances, velocities);
         backward(samples, end_vel, distances, velocities);
@@ -129,7 +129,7 @@ private double[] velocities(
      * referencing the state at i.
      */
     private void forward(
-            Pose2dWithMotion[] samples, double start_vel, double[] distances, double[] velocities) {
+            PathPoint[] samples, double start_vel, double[] distances, double[] velocities) {
         int n = samples.length;
         velocities[0] = start_vel;
         for (int i = 0; i < n - 1; ++i) {
@@ -161,7 +161,7 @@ private void forward(
      * smoothly smooth enough so it shouldn't matter much in practice.
      */
     private void backward(
-            Pose2dWithMotion[] samples, double end_vel, double[] distances, double[] velocities) {
+            PathPoint[] samples, double end_vel, double[] distances, double[] velocities) {
         int n = samples.length;
         velocities[n - 1] = end_vel;
         for (int i = n - 2; i >= 0; --i) {
@@ -182,7 +182,7 @@ private void backward(
     /**
      * Computes the length of each arc and accumulates.
      */
-    private double[] distances(Pose2dWithMotion[] samples) {
+    private double[] distances(PathPoint[] samples) {
         int n = samples.length;
         double distances[] = new double[n];
         for (int i = 1; i < n; ++i) {
@@ -196,7 +196,7 @@ private double[] distances(Pose2dWithMotion[] samples) {
      * Returns the lowest (i.e. closest to zero) velocity constraint from the list
      * of constraints. Always positive or zero.
      */
-    private double maxVelocity(Pose2dWithMotion sample) {
+    private double maxVelocity(PathPoint sample) {
         double minVelocity = HIGH_V;
         for (TimingConstraint constraint : m_constraints) {
             minVelocity = Math.min(minVelocity, constraint.maxV(sample));
@@ -208,7 +208,7 @@ private double maxVelocity(Pose2dWithMotion sample) {
      * Returns the lowest (i.e. closest to zero) acceleration constraint from the
      * list of constraints. Always positive or zero.
      */
-    private double maxAccel(Pose2dWithMotion sample, double velocity) {
+    private double maxAccel(PathPoint sample, double velocity) {
         double minAccel = HIGH_ACCEL;
         for (TimingConstraint constraint : m_constraints) {
             minAccel = Math.min(minAccel, constraint.maxAccel(sample, velocity));
@@ -220,7 +220,7 @@ private double maxAccel(Pose2dWithMotion sample, double velocity) {
      * Returns the highest (i.e. closest to zero) deceleration constraint from the
      * list of constraints. Always negative or zero.
      */
-    private double maxDecel(Pose2dWithMotion sample, double velocity) {
+    private double maxDecel(PathPoint sample, double velocity) {
         double maxDecel = -HIGH_ACCEL;
         for (TimingConstraint constraint : m_constraints) {
             maxDecel = Math.max(maxDecel, constraint.maxDecel(sample, velocity));
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
new file mode 100644
index 00000000..18592236
--- /dev/null
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
@@ -0,0 +1,20 @@
+package org.team100.lib.trajectory.timing;
+
+/**
+ * Resamples a trajectory with constant time between samples.
+ * 
+ * The idea is to prevent overworking the fast parts (where few samples are
+ * required) and underworking the slow parts (where more samples are required),
+ * as the spatial sampling tends to do. This allows the overall
+ * trajectory-making process to be faster, but have higher resolution where it
+ * matters.
+ * 
+ * The sampling goes all the way back to the source spline, to avoid
+ * interpolation error, so the trajectory needs to include it.
+ * 
+ * The new samples are used to reschedule a new trajectory which may have a
+ * different duration.
+ */
+public class TrajectoryRecycler {
+
+}
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java
index 485d443c..36b03525 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 
 import edu.wpi.first.math.geometry.Translation2d;
 
@@ -24,8 +24,8 @@ public VelocityLimitRegionConstraint(
     }
 
     @Override
-    public double maxV(Pose2dWithMotion state) {
-        final Translation2d translation = state.getPose().pose().getTranslation();
+    public double maxV(PathPoint state) {
+        final Translation2d translation = state.waypoint().pose().getTranslation();
         if (translation.getX() <= m_max.getX() && translation.getX() >= m_min.getX() &&
                 translation.getY() <= m_max.getY() && translation.getY() >= m_min.getY()) {
             return m_limit;
@@ -34,12 +34,12 @@ public double maxV(Pose2dWithMotion state) {
     }
 
     @Override
-    public double maxAccel(Pose2dWithMotion state, double velocity) {
+    public double maxAccel(PathPoint state, double velocity) {
         return Double.POSITIVE_INFINITY;
     }
 
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         return Double.NEGATIVE_INFINITY;
     }
 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java
index 358b00ef..80de1e07 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/YawRateConstraint.java
@@ -1,6 +1,6 @@
 package org.team100.lib.trajectory.timing;
 
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.tuning.Mutable;
@@ -37,7 +37,7 @@ public YawRateConstraint(LoggerFactory log, SwerveKinodynamics limits, double sc
     }
 
     @Override
-    public double maxV(Pose2dWithMotion state) {
+    public double maxV(PathPoint state) {
         // Heading rate in rad/m
         double heading_rate = state.getHeadingRateRad_M();
         // rad/s / rad/m => m/s.
@@ -45,7 +45,7 @@ public double maxV(Pose2dWithMotion state) {
     }
 
     @Override
-    public double maxAccel(Pose2dWithMotion state, double velocity) {
+    public double maxAccel(PathPoint state, double velocity) {
         // TODO: this is wrong
         // Heading rate in rad/m
         double heading_rate = state.getHeadingRateRad_M();
@@ -54,7 +54,7 @@ public double maxAccel(Pose2dWithMotion state, double velocity) {
     }
 
     @Override
-    public double maxDecel(Pose2dWithMotion state, double velocity) {
+    public double maxDecel(PathPoint state, double velocity) {
         // TODO: this is wrong
         // Heading rate in rad/m
         double heading_rate = state.getHeadingRateRad_M();
diff --git a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java
index b7307056..85dc5cf0 100644
--- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java
+++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java
@@ -32,7 +32,7 @@ private static double[] fromTrajectory100(Trajectory100 m_trajectory) {
         double[] arr = new double[m_trajectory.length() * 3];
         int ndx = 0;
         for (TimedState p : m_trajectory.getPoints()) {
-            WaypointSE2 pose = p.state().getPose();
+            WaypointSE2 pose = p.state().waypoint();
             arr[ndx + 0] = pose.pose().getTranslation().getX();
             arr[ndx + 1] = pose.pose().getTranslation().getY();
             arr[ndx + 2] = pose.pose().getRotation().getDegrees();
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java
index 19e051df..331c90ac 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java
@@ -8,7 +8,7 @@
 import org.team100.lib.controller.se2.ControllerFactorySE2;
 import org.team100.lib.controller.se2.FullStateControllerSE2;
 import org.team100.lib.geometry.DeltaSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
@@ -181,7 +181,7 @@ void testErrZero() {
         ModelSE2 measurement = new ModelSE2();
         ModelSE2 currentReference = ModelSE2
                 .fromTimedState(new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -198,7 +198,7 @@ void testErrXAhead() {
                 0.01, 0.02);
         ModelSE2 measurement = new ModelSE2(new Pose2d(1, 0, new Rotation2d()));
         ModelSE2 currentReference = ModelSE2
-                .fromTimedState(new TimedState(new Pose2dWithMotion(
+                .fromTimedState(new TimedState(new PathPoint(
                         WaypointSE2.irrotational(
                                 new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                         0, 0), 0, 0, 0));
@@ -214,7 +214,7 @@ void testErrXBehind() {
                 0.01, 0.02);
         ModelSE2 measurement = new ModelSE2(new Pose2d(0, 0, new Rotation2d()));
         ModelSE2 currentReference = ModelSE2
-                .fromTimedState(new TimedState(new Pose2dWithMotion(
+                .fromTimedState(new TimedState(new PathPoint(
                         WaypointSE2.irrotational(
                                 new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2),
                         0, 0), 0, 0, 0));
@@ -231,7 +231,7 @@ void testErrXAheadWithRotation() {
                 0.01, 0.02);
         ModelSE2 measurement = new ModelSE2(new Pose2d(1, 0, new Rotation2d(1)));
         ModelSE2 currentReference = ModelSE2
-                .fromTimedState(new TimedState(new Pose2dWithMotion(
+                .fromTimedState(new TimedState(new PathPoint(
                         WaypointSE2.irrotational(
                                 new Pose2d(0, 0, new Rotation2d(1)), 0, 1.2),
                         0, 0), 0, 0, 0));
@@ -250,7 +250,7 @@ void testErrorAhead() {
         // motion is in a straight line, down the x axis
 
         // setpoint is also at the origin
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
@@ -275,7 +275,7 @@ void testErrorSideways() {
         ModelSE2 measurement = new ModelSE2(new Pose2d(0, 0, Rotation2d.kCCW_Pi_2));
         // motion is in a straight line, down the x axis
         // setpoint is +x, facing down y
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(1, 0, new Rotation2d(Math.PI / 2)), 0, 1.2),
                 0, 0);
@@ -301,7 +301,7 @@ void testVelocityErrorZero() {
                 new VelocitySE2(1, 0, 0));
         // motion is in a straight line, down the x axis
         // setpoint is also at the origin
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0);
         double t = 0;
         // moving
@@ -327,7 +327,7 @@ void testVelocityErrorAhead() {
                 new VelocitySE2(0, 1, 0));
         // motion is in a straight line, down the x axis
         // at the origin
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
@@ -351,7 +351,7 @@ void testFeedForwardAhead() {
                 0.02);
         // motion is in a straight line, down the x axis
         // setpoint is also at the origin
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
@@ -375,7 +375,7 @@ void testFeedForwardSideways() {
                 0.02);
         // motion is in a straight line, down the x axis
         // setpoint is the same
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(Math.PI / 2)), 0, 1.2),
                 0, 0);
@@ -399,7 +399,7 @@ void testFeedForwardTurning() {
                 0.02);
         // motion is tangential to the x axis but turning left
         // setpoint is also at the origin
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 1, 1);
@@ -428,7 +428,7 @@ void testFeedbackAhead() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -462,7 +462,7 @@ void testFeedbackAheadPlusY() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -496,7 +496,7 @@ void testFeedbackAheadPlusTheta() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -533,7 +533,7 @@ void testFeedbackSideways() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -566,7 +566,7 @@ void testFeedbackSidewaysPlusY() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -599,7 +599,7 @@ void testFullFeedbackAhead() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -635,7 +635,7 @@ void testFullFeedbackSideways() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
@@ -676,7 +676,7 @@ void testFullFeedbackSidewaysWithRotation() {
         // motion is in a straight line, down the x axis
         // no curvature
         double curvatureRad_M = 0;
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 setpointPose,
                 0,
                 curvatureRad_M);
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
index 470a9fbc..44cbdb21 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
@@ -182,7 +182,7 @@ void testFieldRelativeTrajectory() {
         VelocityReferenceControllerSE2 referenceController = new VelocityReferenceControllerSE2(
                 logger, drive, swerveController, reference);
 
-        Pose2d pose = trajectory.sample(0).state().getPose().pose();
+        Pose2d pose = trajectory.sample(0).state().waypoint().pose();
         VelocitySE2 velocity = VelocitySE2.ZERO;
 
         double mDt = 0.02;
diff --git a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java
index 327c3bed..1aa39ca1 100644
--- a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java
+++ b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java
@@ -20,126 +20,7 @@ class GeometryUtilTest {
     private static final boolean DEBUG = false;
     private static final double DELTA = 0.001;
 
-    @Test
-    void testCurvature() {
-        HolonomicSpline spline = new HolonomicSpline(
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(),
-                                new Rotation2d()),
-                        new DirectionSE2(1, 0, 0), 1),
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(1, 1),
-                                new Rotation2d()),
-                        new DirectionSE2(0, 1, 0), 1));
-        // verify one point
-        {
-            double splineCurvature = spline.getCurvature(0.5);
-            assertEquals(0.950, splineCurvature, DELTA);
-            Pose2d p0 = spline.getPose2d(0.49);
-            Pose2d p1 = spline.getPose2d(0.5);
-            Pose2d p2 = spline.getPose2d(0.51);
-            double mengerCurvature = GeometryUtil.mengerCurvature(
-                    p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
-            assertEquals(0.950, mengerCurvature, DELTA);
-        }
-        // verify all the points
-        double DS = 0.01;
-        for (double s = DS; s <= 1 - DS; s += DS) {
-            double splineCurvature = spline.getCurvature(s);
-            Pose2d p0 = spline.getPose2d(s - DS);
-            Pose2d p1 = spline.getPose2d(s);
-            Pose2d p2 = spline.getPose2d(s + DS);
-            double mengerCurvature = GeometryUtil.mengerCurvature(
-                    p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
-            if (DEBUG)
-                System.out.printf("%f %f %f %f\n", s, splineCurvature, mengerCurvature,
-                        splineCurvature - mengerCurvature);
-            // error scales with ds.
-            assertEquals(splineCurvature, mengerCurvature, 0.001);
-        }
-    }
-
-    @Test
-    void testCurvature2() {
-        // no curve
-        HolonomicSpline spline = new HolonomicSpline(
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(),
-                                new Rotation2d()),
-                        new DirectionSE2(1, 0, 0), 1),
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(1, 0),
-                                new Rotation2d()),
-                        new DirectionSE2(1, 0, 0), 1));
-        double splineCurvature = spline.getCurvature(0.5);
-        assertEquals(0, splineCurvature, DELTA);
-        Pose2d p0 = spline.getPose2d(0.49);
-        Pose2d p1 = spline.getPose2d(0.5);
-        Pose2d p2 = spline.getPose2d(0.51);
-        double mengerCurvature = GeometryUtil.mengerCurvature(
-                p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
-        assertEquals(0, mengerCurvature, DELTA);
-    }
-
-    @Test
-    void testCurvature3() {
-        // turn in place
-        WaypointSE2 w0 = new WaypointSE2(
-                new Pose2d(new Translation2d(), new Rotation2d()),
-                new DirectionSE2(0, 0, 1), 1);
-        WaypointSE2 w1 = new WaypointSE2(
-                new Pose2d(new Translation2d(), new Rotation2d(1)),
-                new DirectionSE2(0, 0, 1), 1);
-        HolonomicSpline spline = new HolonomicSpline(w0, w1);
-
-        double splineCurvature = spline.getCurvature(0.5);
-        assertEquals(0, splineCurvature, DELTA);
-        Pose2d p0 = spline.getPose2d(0.49);
-        Pose2d p1 = spline.getPose2d(0.5);
-        Pose2d p2 = spline.getPose2d(0.51);
-        double mengerCurvature = GeometryUtil.mengerCurvature(
-                p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
-        assertEquals(0, mengerCurvature, DELTA);
-    }
-
-    @Test
-    void testHeadingRate() {
-        // note spline rotation rate is not constant, to make it more interesting
-        HolonomicSpline spline = new HolonomicSpline(
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(),
-                                new Rotation2d()),
-                        new DirectionSE2(1, 0, 0), 1),
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(1, 0),
-                                new Rotation2d(1)),
-                        new DirectionSE2(1, 0, 1), 1));
-        {
-            double splineHR = spline.getDHeadingDs(0.5);
-            assertEquals(0.811, splineHR, DELTA);
-            Pose2d p0 = spline.getPose2d(0.49);
-            Pose2d p1 = spline.getPose2d(0.51);
-            double discreteHR = GeometryUtil.headingRatio(p0, p1);
-            assertEquals(0.811, discreteHR, DELTA);
-        }
-        double DS = 0.001;
-        for (double s = DS; s <= 1 - DS; s += DS) {
-            double splineHR = spline.getDHeadingDs(s);
-            Pose2d p0 = spline.getPose2d(s - DS);
-            Pose2d p1 = spline.getPose2d(s + DS);
-            double discreteHR = GeometryUtil.headingRatio(p0, p1);
-            if (DEBUG)
-                System.out.printf("%f %f %f %f\n", s, splineHR, discreteHR, splineHR - discreteHR);
-            // error scales with ds
-            assertEquals(splineHR, discreteHR, 0.00001);
-        }
-    }
+   
 
     @Test
     void testProject() {
diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java
index 05cb3d3e..58e6db3c 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveControlTest.java
@@ -3,7 +3,7 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.state.ControlSE2;
@@ -29,7 +29,7 @@ void testTransform() {
     void testTimedState() {
         ControlSE2 s = ControlSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -46,7 +46,7 @@ void testTimedState() {
     void testTimedState2() {
         ControlSE2 s = ControlSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -63,7 +63,7 @@ void testTimedState2() {
     void testTimedState3() {
         ControlSE2 s = ControlSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -81,7 +81,7 @@ void testTimedState3() {
     void testTimedState4() {
         ControlSE2 s = ControlSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 1),
diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java
index 9652eee1..8e7952b9 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/SwerveModelTest.java
@@ -5,7 +5,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.state.ModelSE2;
 import org.team100.lib.trajectory.timing.TimedState;
@@ -30,7 +30,7 @@ void testTransform() {
     void testTimedState() {
         ModelSE2 s = ModelSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -47,7 +47,7 @@ void testTimedState() {
     void testTimedState2() {
         ModelSE2 s = ModelSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -64,7 +64,7 @@ void testTimedState2() {
     void testTimedState3() {
         ModelSE2 s = ModelSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 0),
@@ -82,7 +82,7 @@ void testTimedState3() {
     void testTimedState4() {
         ModelSE2 s = ModelSE2.fromTimedState(
                 new TimedState(
-                        new Pose2dWithMotion(
+                        new PathPoint(
                                 WaypointSE2.irrotational(
                                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                                 0, 1),
diff --git a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java
index b9b0d564..189a5406 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/ParameterizationTest.java
@@ -9,13 +9,14 @@
 import org.jfree.data.xy.XYSeries;
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.DirectionSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
 import org.team100.lib.trajectory.path.PathFactory;
 import org.team100.lib.trajectory.path.spline.HolonomicSpline;
+import org.team100.lib.trajectory.path.spline.SplineToVectorSeries;
 import org.team100.lib.trajectory.timing.ConstantConstraint;
 import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.TimingConstraint;
@@ -115,7 +116,7 @@ void testPoses() {
                         new DirectionSE2(1, 0, 0), 1));
 
         PathFactory pathFactory = new PathFactory(0.1, 0.02, 0.2, 0.1);
-        List<Pose2dWithMotion> poses = pathFactory.samplesFromSplines(List.of(spline));
+        List<PathPoint> poses = pathFactory.samplesFromSplines(List.of(spline));
 
         XYSeries sx = PathToVectorSeries.x("spline", poses);
         XYDataset dataSet = TrajectoryPlotter.collect(sx);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java
index 6e0cb7b4..a5c627eb 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/PathToVectorSeries.java
@@ -4,7 +4,7 @@
 
 import org.jfree.data.xy.VectorSeries;
 import org.jfree.data.xy.XYSeries;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.trajectory.path.Path100;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -27,9 +27,9 @@ public VectorSeries convert(String name, Path100 path) {
         for (double d = 0; d < l; d += dl) {
             if (DEBUG)
                 System.out.printf("%f\n", d);
-            Pose2dWithMotion pwm;
+            PathPoint pwm;
             pwm = path.sample(d);
-            Pose2d p = pwm.getPose().pose();
+            Pose2d p = pwm.waypoint().pose();
             double x = p.getX();
             double y = p.getY();
             Rotation2d heading = p.getRotation();
@@ -40,11 +40,11 @@ public VectorSeries convert(String name, Path100 path) {
         return s;
     }
 
-    public static XYSeries x(String name, List<Pose2dWithMotion> poses) {
+    public static XYSeries x(String name, List<PathPoint> poses) {
         XYSeries series = new XYSeries(name);
         for (int i = 0; i < poses.size(); ++i) {
-            Pose2dWithMotion pose = poses.get(i);
-            series.add(i, pose.getPose().pose().getX());
+            PathPoint pose = poses.get(i);
+            series.add(i, pose.waypoint().pose().getX());
         }
         return series;
     }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
index be6d9e56..409fb5e7 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
@@ -53,16 +53,16 @@ void testPreviewAndAdvance() {
         Trajectory100 trajectory = planner.restToRest(waypoints);
 
         TimedState sample = trajectory.sample(0);
-        assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
 
         sample = trajectory.sample(1);
-        assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
 
         sample = trajectory.sample(2);
-        assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
 
         sample = trajectory.sample(3);
-        assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
     }
 
     @Test
@@ -90,11 +90,11 @@ void testSample() {
 
         assertEquals(1.415, trajectory.duration(), DELTA);
         TimedState sample = trajectory.sample(0);
-        assertEquals(0, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
         sample = trajectory.sample(1);
-        assertEquals(0.828, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.828, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
         sample = trajectory.sample(2);
-        assertEquals(1, sample.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
     }
 
     /**
@@ -150,7 +150,7 @@ void testSampleThoroughly() {
     }
 
     private void check(Trajectory100 trajectory, double t, double x) {
-        assertEquals(x, trajectory.sample(t).state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(x, trajectory.sample(t).state().waypoint().pose().getTranslation().getX(), DELTA);
     }
 
     /**
@@ -180,7 +180,7 @@ void testSamplePerformance() {
         long start = System.nanoTime();
         for (int rep = 0; rep < reps; ++rep) {
             for (int t = 0; t < times; ++t) {
-                spline.getPose2dWithMotion(0.1 * t);
+                spline.getPathPoint(0.1 * t);
             }
         }
         long end = System.nanoTime();
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
index 452d59fc..17c68cb7 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
@@ -8,7 +8,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.DirectionSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
@@ -22,11 +22,11 @@
 import org.team100.lib.trajectory.path.PathFactory;
 import org.team100.lib.trajectory.timing.CapsizeAccelerationConstraint;
 import org.team100.lib.trajectory.timing.ConstantConstraint;
-import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.SwerveDriveDynamicsConstraint;
 import org.team100.lib.trajectory.timing.TimedState;
 import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TimingConstraintFactory;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.YawRateConstraint;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -61,7 +61,7 @@ void testLinear() {
         // start at zero velocity
         assertEquals(0, tp.velocityM_S(), DELTA);
         TimedState p = t.getPoint(8);
-        assertEquals(0.5, p.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.5, p.state().waypoint().pose().getTranslation().getX(), DELTA);
         assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
     }
 
@@ -93,7 +93,7 @@ void testLinearRealistic() {
         TimedState tp = t.getPoint(0);
         assertEquals(0, tp.velocityM_S(), DELTA);
         TimedState p = t.getPoint(8);
-        assertEquals(0.5, p.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.5, p.state().waypoint().pose().getTranslation().getX(), DELTA);
         assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
     }
 
@@ -135,7 +135,7 @@ void testPerformance() {
         }
         assertEquals(33, t.length());
         TimedState p = t.getPoint(12);
-        assertEquals(0.605, p.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.605, p.state().waypoint().pose().getTranslation().getX(), DELTA);
         assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
     }
 
@@ -295,8 +295,8 @@ void testVariableConstraint() {
 
         class ConditionalTimingConstraint implements TimingConstraint {
             @Override
-            public double maxV(Pose2dWithMotion state) {
-                double x = state.getPose().pose().getTranslation().getX();
+            public double maxV(PathPoint state) {
+                double x = state.waypoint().pose().getTranslation().getX();
                 if (x < 1.5) {
                     return 2.0;
                 }
@@ -308,12 +308,12 @@ public double maxV(Pose2dWithMotion state) {
             }
 
             @Override
-            public double maxAccel(Pose2dWithMotion state, double velocity) {
+            public double maxAccel(PathPoint state, double velocity) {
                 return 2;
             }
 
             @Override
-            public double maxDecel(Pose2dWithMotion state, double velocity) {
+            public double maxDecel(PathPoint state, double velocity) {
                 return -1;
             }
         }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java
index 37c3ed39..9bf23b0a 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlotter.java
@@ -23,6 +23,7 @@
 import org.jfree.data.xy.XYSeriesCollection;
 import org.team100.lib.trajectory.path.Path100;
 import org.team100.lib.trajectory.path.spline.HolonomicSpline;
+import org.team100.lib.trajectory.path.spline.SplineToVectorSeries;
 
 public class TrajectoryPlotter {
     public static final boolean SHOW = false;
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java
index 8b9036d5..d4dc588d 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java
@@ -118,8 +118,8 @@ void testDheading() {
                     "t, intrinsic_heading_dt, heading_dt, intrinsic_ca, extrinsic_ca, extrinsic v, intrinsic v, dcourse, dcourse1");
         for (double t = 0.04; t < duration; t += 0.04) {
             TimedState p1 = trajectory.sample(t);
-            Rotation2d heading0 = p0.state().getPose().pose().getRotation();
-            Rotation2d heading1 = p1.state().getPose().pose().getRotation();
+            Rotation2d heading0 = p0.state().waypoint().pose().getRotation();
+            Rotation2d heading1 = p1.state().waypoint().pose().getRotation();
             double dheading = heading1.minus(heading0).getRadians();
             // compute time derivative of heading two ways:
             // this just compares the poses and uses the known time step
@@ -129,9 +129,9 @@ void testDheading() {
             double intrinsicDheadingDt = p0.state().getHeadingRateRad_M() * p0.velocityM_S();
             // curvature is used to compute centripetal acceleration
             // ca = v^2*curvature
-            DirectionSE2 course0 = p0.state().getPose().course();
-            DirectionSE2 course1 = p1.state().getPose().course();
-            p1.state().getPose().pose().log(p0.state().getPose().pose());
+            DirectionSE2 course0 = p0.state().waypoint().course();
+            DirectionSE2 course1 = p1.state().waypoint().course();
+            p1.state().waypoint().pose().log(p0.state().waypoint().pose());
             double dcourse1 = Metrics.translationalNorm(course1.minus(course0));
             double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians();
             double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.state().getCurvatureRad_M();
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java
index f01e0d70..f601e099 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java
@@ -27,7 +27,7 @@ public VectorSeries convert(String name, Trajectory100 t) {
         double dt = duration / POINTS;
         for (double time = 0; time < duration; time += dt) {
             TimedState p = t.sample(time);
-            WaypointSE2 pp = p.state().getPose();
+            WaypointSE2 pp = p.state().waypoint();
             double x = pp.pose().getTranslation().getX();
             double y = pp.pose().getTranslation().getY();
             Rotation2d heading = pp.pose().getRotation();
@@ -51,7 +51,7 @@ public static XYSeries x(String name, Trajectory100 trajectory) {
         double dt = duration / POINTS;
         for (double t = 0; t <= duration + 0.0001; t += dt) {
             TimedState p = trajectory.sample(t);
-            WaypointSE2 pp = p.state().getPose();
+            WaypointSE2 pp = p.state().waypoint();
             double x = pp.pose().getTranslation().getX();
             series.add(t, x);
         }
@@ -69,7 +69,7 @@ public static XYSeries xdot(String name, Trajectory100 trajectory) {
         double dt = duration / POINTS;
         for (double t = 0; t <= duration + 0.0001; t += dt) {
             TimedState p = trajectory.sample(t);
-            Rotation2d course = p.state().getPose().course().toRotation();
+            Rotation2d course = p.state().waypoint().course().toRotation();
             double velocityM_s = p.velocityM_S();
             System.out.println(velocityM_s);
             System.out.println(course);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java
index 336fab9b..150c472f 100755
--- a/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/path/Path100Test.java
@@ -9,7 +9,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.GeometryUtil;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.trajectory.path.spline.HolonomicSpline;
 
@@ -24,20 +24,20 @@ class Path100Test {
             GeometryUtil.fromDegrees(60),
             GeometryUtil.fromDegrees(90));
 
-    private static final List<Pose2dWithMotion> WAYPOINTS = Arrays.asList(
-            new Pose2dWithMotion(
+    private static final List<PathPoint> WAYPOINTS = Arrays.asList(
+            new PathPoint(
                     WaypointSE2.irrotational(
                             new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                     0, 0),
-            new Pose2dWithMotion(
+            new PathPoint(
                     WaypointSE2.irrotational(
                             new Pose2d(24, 0, new Rotation2d(Math.toRadians(30))), 0, 1.2),
                     0, 0),
-            new Pose2dWithMotion(
+            new PathPoint(
                     WaypointSE2.irrotational(
                             new Pose2d(36, 12, new Rotation2d(Math.toRadians(60))), 0, 1.2),
                     0, 0),
-            new Pose2dWithMotion(
+            new PathPoint(
                     WaypointSE2.irrotational(
                             new Pose2d(60, 12, new Rotation2d(Math.toRadians(90))), 0, 1.2),
                     0, 0));
@@ -66,10 +66,10 @@ void testStateAccessors() {
         assertEquals(WAYPOINTS.get(2), traj.getPoint(2));
         assertEquals(WAYPOINTS.get(3), traj.getPoint(3));
 
-        assertEquals(HEADINGS.get(0), traj.getPoint(0).getPose().pose().getRotation());
-        assertEquals(HEADINGS.get(1), traj.getPoint(1).getPose().pose().getRotation());
-        assertEquals(HEADINGS.get(2), traj.getPoint(2).getPose().pose().getRotation());
-        assertEquals(HEADINGS.get(3), traj.getPoint(3).getPose().pose().getRotation());
+        assertEquals(HEADINGS.get(0), traj.getPoint(0).waypoint().pose().getRotation());
+        assertEquals(HEADINGS.get(1), traj.getPoint(1).waypoint().pose().getRotation());
+        assertEquals(HEADINGS.get(2), traj.getPoint(2).waypoint().pose().getRotation());
+        assertEquals(HEADINGS.get(3), traj.getPoint(3).waypoint().pose().getRotation());
     }
 
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java
index e4001be7..8be5ec07 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/path/PathFactoryTest.java
@@ -9,7 +9,7 @@
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.DirectionSE2;
 import org.team100.lib.geometry.GeometryUtil;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.testing.Timeless;
 import org.team100.lib.trajectory.TrajectoryPlotter;
@@ -47,13 +47,13 @@ void testCorner() {
         Path100 path = pathFactory.fromWaypoints(waypoints);
 
         assertEquals(54, path.length());
-        Pose2dWithMotion p = path.getPoint(0);
-        assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        PathPoint p = path.getPoint(0);
+        assertEquals(0, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
         p = path.getPoint(8);
-        assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        assertEquals(0.5, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
     }
 
@@ -73,13 +73,13 @@ void testLinear() {
         PathFactory pathFactory = new PathFactory(0.1, 0.01, 0.01, 0.1);
         Path100 path = pathFactory.fromWaypoints(waypoints);
         assertEquals(17, path.length());
-        Pose2dWithMotion p = path.getPoint(0);
-        assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        PathPoint p = path.getPoint(0);
+        assertEquals(0, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
         p = path.getPoint(8);
-        assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        assertEquals(0.5, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
     }
 
@@ -171,21 +171,21 @@ void test() {
         HolonomicSpline s = new HolonomicSpline(p1, p2);
 
         PathFactory pathFactory = new PathFactory(0.1, 0.05, 0.05, 0.1);
-        List<Pose2dWithMotion> samples = pathFactory.samplesFromSpline(s);
+        List<PathPoint> samples = pathFactory.samplesFromSpline(s);
 
         double arclength = 0;
-        Pose2dWithMotion cur_pose = samples.get(0);
-        for (Pose2dWithMotion sample : samples) {
+        PathPoint cur_pose = samples.get(0);
+        for (PathPoint sample : samples) {
             Twist2d twist = GeometryUtil.slog(
                     GeometryUtil.transformBy(
                             GeometryUtil.inverse(
-                                    cur_pose.getPose().pose()),
-                            sample.getPose().pose()));
+                                    cur_pose.waypoint().pose()),
+                            sample.waypoint().pose()));
             arclength += Math.hypot(twist.dx, twist.dy);
             cur_pose = sample;
         }
 
-        WaypointSE2 pose = cur_pose.getPose();
+        WaypointSE2 pose = cur_pose.waypoint();
         assertEquals(15.0, pose.pose().getTranslation().getX(), 0.001);
         assertEquals(10.0, pose.pose().getTranslation().getY(), 0.001);
         assertEquals(78.690, pose.course().toRotation().getDegrees(), 0.001);
@@ -207,11 +207,11 @@ void testDx() {
                         new DirectionSE2(0, 1, 0), 1));
         List<HolonomicSpline> splines = List.of(s0);
         PathFactory pathFactory = new PathFactory(0.1, 0.001, 0.001, 0.001);
-        List<Pose2dWithMotion> motion = pathFactory.samplesFromSplines(splines);
-        for (Pose2dWithMotion p : motion) {
+        List<PathPoint> motion = pathFactory.samplesFromSplines(splines);
+        for (PathPoint p : motion) {
             if (DEBUG)
-                System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(),
-                        p.getPose().pose().getTranslation().getY());
+                System.out.printf("%5.3f %5.3f\n", p.waypoint().pose().getTranslation().getX(),
+                        p.waypoint().pose().getTranslation().getY());
         }
     }
 
@@ -252,8 +252,8 @@ void testPerformance() {
             System.out.printf("duration per iteration ms: %5.3f\n", totalDurationMs / iterations);
         }
         assertEquals(33, t.length());
-        Pose2dWithMotion p = t.getPoint(4);
-        assertEquals(0.211, p.getPose().pose().getTranslation().getX(), DELTA);
+        PathPoint p = t.getPoint(4);
+        assertEquals(0.211, p.waypoint().pose().getTranslation().getX(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
     }
 
diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java
index 3177cd5a..c3a6e7ee 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/HolonomicSplineTest.java
@@ -1,14 +1,16 @@
 package org.team100.lib.trajectory.path.spline;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
 import java.util.ArrayList;
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.geometry.Metrics;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
@@ -22,8 +24,8 @@
 import org.team100.lib.trajectory.path.PathFactory;
 import org.team100.lib.trajectory.timing.CapsizeAccelerationConstraint;
 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.TrajectoryFactory;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -64,20 +66,7 @@ void testCurvature() {
                         new DirectionSE2(0, 1, 0), 1));
         assertEquals(0.950, s.getCurvature(0.5), DELTA);
 
-        // rotation in place yields zero curvature since there is no x/y motion, so
-        // curvature has no meaning.
-        s = new HolonomicSpline(
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(),
-                                new Rotation2d()),
-                        new DirectionSE2(0, 0, 1), 1),
-                new WaypointSE2(
-                        new Pose2d(
-                                new Translation2d(),
-                                new Rotation2d(1)),
-                        new DirectionSE2(0, 0, 1), 1));
-        assertEquals(0, s.getCurvature(0.5), DELTA);
+
     }
 
     @Test
@@ -105,17 +94,17 @@ void testLinear() {
         TrajectoryPlotter plotter = new TrajectoryPlotter(0.1);
         plotter.plot("rotation", List.of(s));
 
-        Translation2d t = s.getPose2d(0).getTranslation();
+        Translation2d t = s.getPathPoint(0).waypoint().pose().getTranslation();
         assertEquals(0, t.getX(), DELTA);
-        t = s.getPose2d(1).getTranslation();
+        t = s.getPathPoint(1).waypoint().pose().getTranslation();
         assertEquals(1, t.getX(), DELTA);
-        Pose2dWithMotion p = s.getPose2dWithMotion(0);
-        assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        PathPoint p = s.getPathPoint(0);
+        assertEquals(0, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
-        p = s.getPose2dWithMotion(1);
-        assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        p = s.getPathPoint(1);
+        assertEquals(1, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
     }
 
@@ -136,17 +125,17 @@ void testLinear2() {
         TrajectoryPlotter plotter = new TrajectoryPlotter(0.1);
         plotter.plot("rotation", List.of(s));
 
-        Translation2d t = s.getPose2d(0).getTranslation();
+        Translation2d t = s.getPathPoint(0).waypoint().pose().getTranslation();
         assertEquals(0, t.getX(), DELTA);
-        t = s.getPose2d(1).getTranslation();
+        t = s.getPathPoint(1).waypoint().pose().getTranslation();
         assertEquals(2, t.getX(), DELTA);
-        Pose2dWithMotion p = s.getPose2dWithMotion(0);
-        assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        PathPoint p = s.getPathPoint(0);
+        assertEquals(0, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
-        p = s.getPose2dWithMotion(1);
-        assertEquals(2, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        p = s.getPathPoint(1);
+        assertEquals(2, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
     }
 
@@ -172,26 +161,26 @@ void testRotationSoft() {
 
         // now that the magic numbers apply to the rotational scaling, the heading rate
         // is constant.
-        Translation2d t = s.getPose2d(0).getTranslation();
+        Translation2d t = s.getPathPoint(0).waypoint().pose().getTranslation();
         assertEquals(0, t.getX(), DELTA);
-        t = s.getPose2d(1).getTranslation();
+        t = s.getPathPoint(1).waypoint().pose().getTranslation();
         assertEquals(1, t.getX(), DELTA);
 
-        Pose2dWithMotion p = s.getPose2dWithMotion(0);
-        assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        PathPoint p = s.getPathPoint(0);
+        assertEquals(0, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         // initial rotation rate is zero
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
 
-        p = s.getPose2dWithMotion(0.5);
-        assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA);
+        p = s.getPathPoint(0.5);
+        assertEquals(0.5, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.5, p.waypoint().pose().getRotation().getRadians(), DELTA);
         // high rotation rate in the middle
         assertEquals(0.915, p.getHeadingRateRad_M(), DELTA);
 
-        p = s.getPose2dWithMotion(1);
-        assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(1, p.getPose().pose().getRotation().getRadians(), DELTA);
+        p = s.getPathPoint(1);
+        assertEquals(1, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, p.waypoint().pose().getRotation().getRadians(), DELTA);
         // rotation rate is zero at the end
         assertEquals(0, p.getHeadingRateRad_M(), DELTA);
 
@@ -220,24 +209,24 @@ void testRotationFast() {
 
         // now that the magic numbers apply to the rotational scaling, the heading rate
         // is constant.
-        Translation2d t = s.getPose2d(0).getTranslation();
+        Translation2d t = s.getPathPoint(0).waypoint().pose().getTranslation();
         assertEquals(0, t.getX(), DELTA);
-        t = s.getPose2d(1).getTranslation();
+        t = s.getPathPoint(1).waypoint().pose().getTranslation();
         assertEquals(1, t.getX(), DELTA);
 
-        Pose2dWithMotion p = s.getPose2dWithMotion(0);
-        assertEquals(0, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.getPose().pose().getRotation().getRadians(), DELTA);
+        PathPoint p = s.getPathPoint(0);
+        assertEquals(0, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0.707, p.getHeadingRateRad_M(), DELTA);
 
-        p = s.getPose2dWithMotion(0.5);
-        assertEquals(0.5, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(0.5, p.getPose().pose().getRotation().getRadians(), DELTA);
+        p = s.getPathPoint(0.5);
+        assertEquals(0.5, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.5, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0.707, p.getHeadingRateRad_M(), DELTA);
 
-        p = s.getPose2dWithMotion(1);
-        assertEquals(1, p.getPose().pose().getTranslation().getX(), DELTA);
-        assertEquals(1, p.getPose().pose().getRotation().getRadians(), DELTA);
+        p = s.getPathPoint(1);
+        assertEquals(1, p.waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, p.waypoint().pose().getRotation().getRadians(), DELTA);
         assertEquals(0.707, p.getHeadingRateRad_M(), DELTA);
 
     }
@@ -260,11 +249,11 @@ void testRotation2() {
         plotter.plot("rotation", List.of(s));
     }
 
-    /** Turning in place splines work. */
+    /** Turning in place splines do not work. */
     @Test
     void spin() {
         double scale = 0.9;
-        HolonomicSpline s0 = new HolonomicSpline(
+        assertThrows(IllegalArgumentException.class, () -> new HolonomicSpline(
                 new WaypointSE2(
                         new Pose2d(
                                 new Translation2d(0, 0),
@@ -274,11 +263,7 @@ void spin() {
                         new Pose2d(
                                 new Translation2d(0, 0),
                                 Rotation2d.kCCW_90deg),
-                        new DirectionSE2(0, 0, 1), scale));
-
-        List<HolonomicSpline> splines = new ArrayList<>();
-        splines.add(s0);
-        TrajectoryPlotter.plot(splines, 0.1);
+                        new DirectionSE2(0, 0, 1), scale)));
     }
 
     /**
@@ -326,18 +311,18 @@ void testDheading() {
                 new Pose2d(new Translation2d(0, 1), Rotation2d.kCW_90deg),
                 new DirectionSE2(-1, 0, 1), scale);
         HolonomicSpline spline = new HolonomicSpline(w0, w1);
-        Pose2dWithMotion p0 = spline.getPose2dWithMotion(0.0);
+        PathPoint p0 = spline.getPathPoint(0.0);
         if (DEBUG)
             System.out.println(
                     "s, p0_heading_rate, p0_curvature, distance, post_hoc_heading_rate, post_hoc_curvature, post_hoc_heading_rate2, post_hoc_curvature2");
         for (double s = 0.01; s <= 1.0; s += 0.01) {
-            Pose2dWithMotion p1 = spline.getPose2dWithMotion(s);
+            PathPoint p1 = spline.getPathPoint(s);
             double cartesianDistance = p1.distanceCartesian(p0);
-            Rotation2d heading0 = p0.getPose().pose().getRotation();
-            Rotation2d heading1 = p1.getPose().pose().getRotation();
+            Rotation2d heading0 = p0.waypoint().pose().getRotation();
+            Rotation2d heading1 = p1.waypoint().pose().getRotation();
             double dheading = heading1.minus(heading0).getRadians();
-            DirectionSE2 course0 = p0.getPose().course();
-            DirectionSE2 course1 = p1.getPose().course();
+            DirectionSE2 course0 = p0.waypoint().course();
+            DirectionSE2 course1 = p1.waypoint().course();
             double curve = Metrics.translationalNorm(course1.minus(course0));
             // this value matches the intrinsic one since it just uses
             // cartesian distance in the denominator.
@@ -361,7 +346,7 @@ private void checkCircle(List<HolonomicSpline> splines, double rangeError, doubl
             System.out.println("s, x, y, k, dh");
         for (HolonomicSpline spline : splines) {
             for (double s = 0; s < 0.99; s += 0.01) {
-                Pose2d p = spline.getPose2d(s);
+                Pose2d p = spline.getPathPoint(s).waypoint().pose();
                 // the position should be on the circle
                 double range = p.getTranslation().getNorm();
                 actualRangeError = Math.max(actualRangeError, Math.abs(1.0 - range));
@@ -541,19 +526,19 @@ void testEntryVelocity() {
         plotter.plot("splines", splines);
 
         PathFactory pathFactory = new PathFactory(0.1, 0.05, 0.05, 0.05);
-        List<Pose2dWithMotion> motion = pathFactory.samplesFromSplines(splines);
+        List<PathPoint> motion = pathFactory.samplesFromSplines(splines);
         if (DEBUG) {
-            for (Pose2dWithMotion p : motion) {
-                System.out.printf("%5.3f %5.3f\n", p.getPose().pose().getTranslation().getX(),
-                        p.getPose().pose().getTranslation().getY());
+            for (PathPoint p : motion) {
+                System.out.printf("%5.3f %5.3f\n", p.waypoint().pose().getTranslation().getX(),
+                        p.waypoint().pose().getTranslation().getY());
             }
         }
         Path100 path = new Path100(motion);
         if (DEBUG) {
             for (int i = 0; i < path.length(); ++i) {
                 System.out.printf("%5.3f %5.3f\n",
-                        path.getPoint(i).getPose().pose().getTranslation().getX(),
-                        path.getPoint(i).getPose().pose().getTranslation().getY());
+                        path.getPoint(i).waypoint().pose().getTranslation().getX(),
+                        path.getPoint(i).waypoint().pose().getTranslation().getY());
             }
         }
 
@@ -574,4 +559,117 @@ void testEntryVelocity() {
         if (DEBUG)
             System.out.printf("trajectory %s\n", trajectory);
     }
+
+    @Test
+    void testHeadingRate() {
+        // note spline rotation rate is not constant, to make it more interesting
+        HolonomicSpline spline = new HolonomicSpline(
+                new WaypointSE2(
+                        new Pose2d(
+                                new Translation2d(),
+                                new Rotation2d()),
+                        new DirectionSE2(1, 0, 0), 1),
+                new WaypointSE2(
+                        new Pose2d(
+                                new Translation2d(1, 0),
+                                new Rotation2d(1)),
+                        new DirectionSE2(1, 0, 1), 1));
+        {
+            double splineHR = spline.getDHeadingDs(0.5);
+            assertEquals(0.811, splineHR, DELTA);
+            Pose2d p0 = spline.getPathPoint(0.49).waypoint().pose();
+            Pose2d p1 = spline.getPathPoint(0.51).waypoint().pose();
+            double discreteHR = GeometryUtil.headingRatio(p0, p1);
+            assertEquals(0.811, discreteHR, DELTA);
+        }
+        double DS = 0.001;
+        for (double s = DS; s <= 1 - DS; s += DS) {
+            double splineHR = spline.getDHeadingDs(s);
+            Pose2d p0 = spline.getPathPoint(s - DS).waypoint().pose();
+            Pose2d p1 = spline.getPathPoint(s + DS).waypoint().pose();
+            double discreteHR = GeometryUtil.headingRatio(p0, p1);
+            if (DEBUG)
+                System.out.printf("%f %f %f %f\n", s, splineHR, discreteHR, splineHR - discreteHR);
+            // error scales with ds
+            assertEquals(splineHR, discreteHR, 0.00001);
+        }
+    }
+
+    @Test
+    void testCurvature1() {
+        HolonomicSpline spline = new HolonomicSpline(
+                new WaypointSE2(
+                        new Pose2d(
+                                new Translation2d(),
+                                new Rotation2d()),
+                        new DirectionSE2(1, 0, 0), 1),
+                new WaypointSE2(
+                        new Pose2d(
+                                new Translation2d(1, 1),
+                                new Rotation2d()),
+                        new DirectionSE2(0, 1, 0), 1));
+        // verify one point
+        {
+            double splineCurvature = spline.getCurvature(0.5);
+            assertEquals(0.950, splineCurvature, DELTA);
+            Pose2d p0 = spline.getPathPoint(0.49).waypoint().pose();
+            Pose2d p1 = spline.getPathPoint(0.50).waypoint().pose();
+            Pose2d p2 = spline.getPathPoint(0.51).waypoint().pose();
+            double mengerCurvature = GeometryUtil.mengerCurvature(
+                    p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
+            assertEquals(0.950, mengerCurvature, DELTA);
+        }
+        // verify all the points
+        double DS = 0.01;
+        for (double s = DS; s <= 1 - DS; s += DS) {
+            double splineCurvature = spline.getCurvature(s);
+            Pose2d p0 = spline.getPathPoint(s - DS).waypoint().pose();
+            Pose2d p1 = spline.getPathPoint(s).waypoint().pose();
+            Pose2d p2 = spline.getPathPoint(s + DS).waypoint().pose();
+            double mengerCurvature = GeometryUtil.mengerCurvature(
+                    p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
+            if (DEBUG)
+                System.out.printf("%f %f %f %f\n", s, splineCurvature, mengerCurvature,
+                        splineCurvature - mengerCurvature);
+            // error scales with ds.
+            assertEquals(splineCurvature, mengerCurvature, 0.001);
+        }
+    }
+
+    @Test
+    void testCurvature2() {
+        // no curve
+        HolonomicSpline spline = new HolonomicSpline(
+                new WaypointSE2(
+                        new Pose2d(
+                                new Translation2d(),
+                                new Rotation2d()),
+                        new DirectionSE2(1, 0, 0), 1),
+                new WaypointSE2(
+                        new Pose2d(
+                                new Translation2d(1, 0),
+                                new Rotation2d()),
+                        new DirectionSE2(1, 0, 0), 1));
+        double splineCurvature = spline.getCurvature(0.5);
+        assertEquals(0, splineCurvature, DELTA);
+        Pose2d p0 = spline.getPathPoint(0.49).waypoint().pose();
+        Pose2d p1 = spline.getPathPoint(0.50).waypoint().pose();
+        Pose2d p2 = spline.getPathPoint(0.51).waypoint().pose();
+        double mengerCurvature = GeometryUtil.mengerCurvature(
+                p0.getTranslation(), p1.getTranslation(), p2.getTranslation());
+        assertEquals(0, mengerCurvature, DELTA);
+    }
+
+    @Test
+    void testCurvature3() {
+        // turn in place
+        WaypointSE2 w0 = new WaypointSE2(
+                new Pose2d(new Translation2d(), new Rotation2d()),
+                new DirectionSE2(0, 0, 1), 1);
+        WaypointSE2 w1 = new WaypointSE2(
+                new Pose2d(new Translation2d(), new Rotation2d(1)),
+                new DirectionSE2(0, 0, 1), 1);
+        assertThrows(IllegalArgumentException.class, () -> new HolonomicSpline(w0, w1));
+    }
+
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineToVectorSeries.java
similarity index 93%
rename from lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java
rename to lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineToVectorSeries.java
index 1f1c0c4b..6247a623 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/SplineToVectorSeries.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineToVectorSeries.java
@@ -1,10 +1,9 @@
-package org.team100.lib.trajectory;
+package org.team100.lib.trajectory.path.spline;
 
 import java.util.List;
 
 import org.jfree.data.xy.VectorSeries;
 import org.jfree.data.xy.XYSeries;
-import org.team100.lib.trajectory.path.spline.HolonomicSpline;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -28,7 +27,8 @@ public VectorSeries convert(String name, List<HolonomicSpline> splines) {
         VectorSeries series = new VectorSeries(name);
         for (HolonomicSpline spline : splines) {
             for (double s = 0; s <= 1.001; s += DS) {
-                Pose2d p = spline.getPose2d(s);
+                Pose2d p = spline.getPathPoint(s).waypoint().pose();
+                System.out.println(p);
                 double x = p.getX();
                 double y = p.getY();
                 Rotation2d heading = p.getRotation();
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java
index 27566bed..84b25222 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/CentripetalAccelerationConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
@@ -28,7 +28,7 @@ void testSimple() {
                 logger,
                 SwerveKinodynamicsFactory.forTest(logger),
                 CENTRIPETAL_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 1);
@@ -47,7 +47,7 @@ void testSimpleMoving() {
                 logger,
                 SwerveKinodynamicsFactory.forTest(logger),
                 CENTRIPETAL_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 1);
@@ -66,7 +66,7 @@ void testSimpleOverspeed() {
                 logger,
                 SwerveKinodynamicsFactory.forTest(logger),
                 CENTRIPETAL_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 1);
@@ -84,7 +84,7 @@ void testSimple2() {
                 logger,
                 SwerveKinodynamicsFactory.forTest2(logger),
                 CENTRIPETAL_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 1);
@@ -101,7 +101,7 @@ void testStraightLine() {
                 logger,
                 SwerveKinodynamicsFactory.forTest2(logger),
                 CENTRIPETAL_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java
index 089586dc..3eb52150 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/ConstantConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
@@ -20,7 +20,7 @@ public class ConstantConstraintTest implements Timeless {
     @Test
     void testVelocity() {
         ConstantConstraint c = new ConstantConstraint(logger, 2, 3);
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
@@ -30,7 +30,7 @@ void testVelocity() {
     @Test
     void testAccel() {
         ConstantConstraint c = new ConstantConstraint(logger, 2, 3);
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java
index 64cfd2d3..30d45421 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/DiamondConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
@@ -21,19 +21,19 @@ public class DiamondConstraintTest implements Timeless {
     void testSquare() {
         // here the two speeds are the same
         DiamondConstraint c = new DiamondConstraint(logger, 1, 1, 4);
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
         // moving purely in x, get the x number
         assertEquals(1, c.maxV(state), DELTA);
-        state = new Pose2dWithMotion(
+        state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2),
                 0, 0);
         // moving purely in y, get the y number
         assertEquals(1, c.maxV(state), DELTA);
-        state = new Pose2dWithMotion(
+        state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2),
                 0, 0);
@@ -44,19 +44,19 @@ void testSquare() {
     @Test
     void testVelocity() {
         DiamondConstraint c = new DiamondConstraint(logger, 2, 3, 4);
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
         // moving purely in x, get the x number
         assertEquals(2, c.maxV(state), DELTA);
-        state = new Pose2dWithMotion(
+        state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 2, 1.2),
                 0, 0);
         // moving purely in y, get the y number
         assertEquals(3, c.maxV(state), DELTA);
-        state = new Pose2dWithMotion(
+        state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), Math.PI / 4, 1.2),
                 0, 0);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
new file mode 100644
index 00000000..75195f64
--- /dev/null
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
@@ -0,0 +1,77 @@
+package org.team100.lib.trajectory.timing;
+
+import java.util.List;
+
+import org.junit.jupiter.api.Test;
+import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.WaypointSE2;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.logging.TestLoggerFactory;
+import org.team100.lib.logging.primitive.TestPrimitiveLogger;
+import org.team100.lib.trajectory.Trajectory100;
+import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.path.PathFactory;
+
+import com.ctre.phoenix6.Utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+public class SoftRegionConstraintTest {
+    private static final double DELTA = 0.001;
+    private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger());
+
+    @Test
+    void testStart() {
+        WaypointSE2 w0 = new WaypointSE2(
+                new Pose2d(
+                        new Translation2d(),
+                        new Rotation2d()),
+                new DirectionSE2(1, 0, 0), 1.2);
+        WaypointSE2 w1 = new WaypointSE2(
+                new Pose2d(
+                        new Translation2d(1, 0),
+                        new Rotation2d()),
+                new DirectionSE2(1, 0, 0), 1.2);
+        List<WaypointSE2> waypoints = List.of(w0, w1);
+        // note that the first accel (10) matches the max soft accel (10)
+        List<TimingConstraint> constraints = List.of(
+                new ConstantConstraint(logger, 10, 2),
+                new SoftRegionConstraint(w0.pose().getTranslation(), 0.1, 0.5, 10));
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        PathFactory pathFactory = new PathFactory();
+        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
+        Trajectory100 trajectory = planner.restToRest(waypoints);
+        trajectory.dump();
+    }
+
+    @Test
+    void testBoth() {
+        // this forces the ctre output to the top
+        Utils.isSimulation();
+        WaypointSE2 w0 = new WaypointSE2(
+                new Pose2d(
+                        new Translation2d(),
+                        new Rotation2d()),
+                new DirectionSE2(1, 0, 0), 1.2);
+        WaypointSE2 w1 = new WaypointSE2(
+                new Pose2d(
+                        new Translation2d(1, 0),
+                        new Rotation2d()),
+                new DirectionSE2(1, 0, 0), 1.2);
+        List<WaypointSE2> waypoints = List.of(w0, w1);
+        // note that the first accel (10) matches the max soft accel (10)
+        List<TimingConstraint> constraints = List.of(
+                new ConstantConstraint(logger, 10, 10),
+                new SoftRegionConstraint(w0.pose().getTranslation(), 0.1, 0.5, 10),
+                new SoftRegionConstraint(w1.pose().getTranslation(), 0.1, 0.5, 10));
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        // note finer path than usual in order to see the detail
+        PathFactory pathFactory = new PathFactory(0.01, 0.01, 0.01, 0.1);
+        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
+        Trajectory100 trajectory = planner.restToRest(waypoints);
+        trajectory.tdump();
+    }
+
+}
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java
index 38afbc7c..4ee9a557 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SwerveDriveDynamicsConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
@@ -24,14 +24,14 @@ void testVelocity() {
         SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1);
 
         // motionless
-        double m = c.maxV(new Pose2dWithMotion(
+        double m = c.maxV(new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0));
         assertEquals(5, m, DELTA);
 
         // moving in +x, no curvature, no rotation
-        m = c.maxV(new Pose2dWithMotion(
+        m = c.maxV(new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0));
@@ -39,7 +39,7 @@ void testVelocity() {
         assertEquals(5, m, DELTA);
 
         // moving in +x, 5 rad/meter
-        m = c.maxV(new Pose2dWithMotion(
+        m = c.maxV(new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 5, 0));
@@ -51,7 +51,7 @@ void testVelocity() {
         // so radius to center is 0.25 * sqrt(2) = 0.356
         // traveling 1 m/s, there are 4 m/s available for the fastest wheel
         // which means 11.314 rad/s, and also 11.314 rad/m since we're going 1 m/s.
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 11.313708, 0);
@@ -69,7 +69,7 @@ void testAccel() {
         SwerveDriveDynamicsConstraint c = new SwerveDriveDynamicsConstraint(logger, kinodynamics, 1, 1);
         // this is constant
         Pose2d p = new Pose2d(0, 0, new Rotation2d(0));
-        Pose2dWithMotion p2 = new Pose2dWithMotion(
+        PathPoint p2 = new PathPoint(
                 WaypointSE2.irrotational(p, 0, 1.2), 0, 0);
         assertEquals(-20, c.maxDecel(p2, 0), DELTA);
         assertEquals(10, c.maxAccel(p2, 0), DELTA);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
index 4699826e..8da514d1 100755
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,7 +16,7 @@ class TimedStateTest {
     void test() {
         // At (0,0,0), t=0, v=0, acceleration=1
         TimedState start_state = new TimedState(
-                new Pose2dWithMotion(
+                new PathPoint(
                         WaypointSE2.irrotational(
                                 new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                         0, 0),
@@ -24,7 +24,7 @@ void test() {
 
         // At (.5,0,0), t=1, v=1, acceleration=0
         TimedState end_state = new TimedState(
-                new Pose2dWithMotion(
+                new PathPoint(
                         WaypointSE2.irrotational(
                                 new Pose2d(0.5, 0, new Rotation2d(0)), 0, 1.2),
                         0, 0),
@@ -40,6 +40,6 @@ void test() {
         assertEquals(start_state.acceleration(), intermediate_state.acceleration(), EPSILON);
         assertEquals(0.5, intermediate_state.velocityM_S(), EPSILON);
         // close to the start state by distance
-        assertEquals(0.125, intermediate_state.state().getPose().pose().getTranslation().getX(), EPSILON);
+        assertEquals(0.125, intermediate_state.state().waypoint().pose().getTranslation().getX(), EPSILON);
     }
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java
index 09c7e106..1f7cf3ff 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TorqueConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,7 +16,7 @@ public class TorqueConstraintTest {
     void testRadial() {
         TorqueConstraint jc = new TorqueConstraint(6);
         // moving +x at (1,0,0)
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(1, 0, new Rotation2d(0)), 0, 1.2),
                 0, 0);
@@ -29,7 +29,7 @@ void testRadial() {
     void testTangential() {
         TorqueConstraint jc = new TorqueConstraint(6);
         // at (1,0,0), moving (0,1,0)
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2),
                 0, 0);
@@ -42,7 +42,7 @@ void testTangential() {
     void testInclined() {
         TorqueConstraint jc = new TorqueConstraint(6);
         // moving +x+y at (1,0,0)
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 4, 1.2),
                 0, 0);
@@ -55,7 +55,7 @@ void testInclined() {
     void testFar() {
         TorqueConstraint jc = new TorqueConstraint(6);
         // moving +y at (2,0,0)
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(2, 0, new Rotation2d(0)), Math.PI / 2, 1.2),
                 0, 0);
@@ -68,7 +68,7 @@ void testFar() {
     void testFar2() {
         TorqueConstraint jc = new TorqueConstraint(6);
         // moving +y at (3,0,0)
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(3, 0, new Rotation2d(0)), Math.PI / 2, 1.2),
                 0, 0);
@@ -81,7 +81,7 @@ void testFar2() {
     void testRealistic() {
         TorqueConstraint jc = new TorqueConstraint(30);
         // moving +y at (1,0,0)
-        Pose2dWithMotion state = new Pose2dWithMotion(
+        PathPoint state = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(1, 0, new Rotation2d(0)), Math.PI / 2, 1.2),
                 0, 0);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java
index a157bf72..340f5d9e 100755
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java
@@ -12,7 +12,7 @@
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.DirectionSE2;
 import org.team100.lib.geometry.GeometryUtil;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
@@ -33,14 +33,14 @@ public class TrajectoryFactoryTest {
     private static final double DELTA = 0.01;
     private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger());
 
-    public static final List<Pose2dWithMotion> WAYPOINTS = Arrays.asList(
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+    public static final List<PathPoint> WAYPOINTS = Arrays.asList(
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0),
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(24.0, 0.0, new Rotation2d(0)), 0, 1.2), 0, 0),
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(36, 12, new Rotation2d(0)), 0, 1.2), 0, 0),
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(60, 12, new Rotation2d(0)), 0, 1.2), 0, 0));
 
     public static final List<Rotation2d> HEADINGS = List.of(
@@ -104,12 +104,12 @@ public void checkTrajectory(
     @Test
     void testJustTurningInPlace() {
         Path100 path = new Path100(Arrays.asList(
-                new Pose2dWithMotion(
+                new PathPoint(
                         new WaypointSE2(
                                 new Pose2d(0, 0, new Rotation2d(0)),
                                 new DirectionSE2(0, 0, 1), 1),
                         1, 0),
-                new Pose2dWithMotion(
+                new PathPoint(
                         new WaypointSE2(
                                 new Pose2d(0, 0, new Rotation2d(Math.PI)),
                                 new DirectionSE2(0, 0, 1), 1),
@@ -190,8 +190,8 @@ void testConditionalVelocityConstraint() {
 
         class ConditionalTimingConstraint implements TimingConstraint {
             @Override
-            public double maxV(Pose2dWithMotion state) {
-                if (state.getPose().pose().getTranslation().getX() >= 24.0) {
+            public double maxV(PathPoint state) {
+                if (state.waypoint().pose().getTranslation().getX() >= 24.0) {
                     return 5.0;
                 } else {
                     return Double.POSITIVE_INFINITY;
@@ -199,12 +199,12 @@ public double maxV(Pose2dWithMotion state) {
             }
 
             @Override
-            public double maxAccel(Pose2dWithMotion state, double velocity) {
+            public double maxAccel(PathPoint state, double velocity) {
                 return Double.POSITIVE_INFINITY;
             }
 
             @Override
-            public double maxDecel(Pose2dWithMotion state, double velocity) {
+            public double maxDecel(PathPoint state, double velocity) {
                 return Double.NEGATIVE_INFINITY;
             }
         }
@@ -221,18 +221,18 @@ void testConditionalAccelerationConstraint() {
 
         class ConditionalTimingConstraint implements TimingConstraint {
             @Override
-            public double maxV(Pose2dWithMotion state) {
+            public double maxV(PathPoint state) {
                 return Double.POSITIVE_INFINITY;
             }
 
             @Override
-            public double maxAccel(Pose2dWithMotion state,
+            public double maxAccel(PathPoint state,
                     double velocity) {
                 return 10.0 / velocity;
             }
 
             @Override
-            public double maxDecel(Pose2dWithMotion state, double velocity) {
+            public double maxDecel(PathPoint state, double velocity) {
                 return -10.0;
             }
         }
@@ -278,7 +278,7 @@ void testPerformance() {
         }
         assertEquals(33, t.length());
         TimedState p = t.getPoint(12);
-        assertEquals(0.605, p.state().getPose().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.605, p.state().waypoint().pose().getTranslation().getX(), DELTA);
         assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
 
     }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
new file mode 100644
index 00000000..ad33f4c7
--- /dev/null
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
@@ -0,0 +1,10 @@
+package org.team100.lib.trajectory.timing;
+
+import org.junit.jupiter.api.Test;
+
+public class TrajectoryRecyclerTest {
+    @Test
+    void test0() {
+    }
+
+}
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java
index 412928a2..7b3bb491 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryVelocityProfileTest.java
@@ -5,7 +5,7 @@
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
@@ -30,15 +30,15 @@ public class TrajectoryVelocityProfileTest implements Timeless {
     private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger());
 
     // A five-meter straight line.
-    private static final Pose2dWithMotion[] WAYPOINTS = new Pose2dWithMotion[] {
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+    private static final PathPoint[] WAYPOINTS = new PathPoint[] {
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2), 0, 0),
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(2.5, 0, new Rotation2d(0)), 0, 1.2), 0, 0),
-            new Pose2dWithMotion(WaypointSE2.irrotational(
+            new PathPoint(WaypointSE2.irrotational(
                     new Pose2d(5, 0, new Rotation2d(0)), 0, 1.2), 0, 0) };
 
-    private static List<WaypointSE2> waypointList = Arrays.asList(WAYPOINTS).stream().map(p -> p.getPose()).toList();
+    private static List<WaypointSE2> waypointList = Arrays.asList(WAYPOINTS).stream().map(p -> p.waypoint()).toList();
     private static PathFactory pathFactory = new PathFactory(0.1, 0.1, 0.1, 0.1);
     private static Path100 path = pathFactory.fromWaypoints(waypointList);
 
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java
index d3793c8b..435be62e 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/VelocityLimitRegionConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -18,7 +18,7 @@ void testOutside() {
         // towards +x, 1 rad/m, 1 rad/s limit => 1 m/s
         VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint(
                 new Translation2d(), new Translation2d(1, 1), 1);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(-1, -1, new Rotation2d(0)), 0, 1.2),
                 0, // spatial, so rad/m
@@ -33,7 +33,7 @@ void testInside() {
         // towards +x, 1 rad/m, 1 rad/s limit => 1 m/s
         VelocityLimitRegionConstraint c = new VelocityLimitRegionConstraint(
                 new Translation2d(), new Translation2d(1, 1), 1);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0.5, 0.5, new Rotation2d(0)), 0, 1.2),
                 0, // spatial, so rad/m
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java
index a3bdc77b..9de0fa28 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/YawRateConstraintTest.java
@@ -4,7 +4,7 @@
 
 import org.junit.jupiter.api.Test;
 import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.geometry.Pose2dWithMotion;
+import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
@@ -27,7 +27,7 @@ void testNormal() {
         // the linear constraint but it's ok)
         YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest(logger),
                 YAW_RATE_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 1, // spatial, so rad/m
                 0);
@@ -41,7 +41,7 @@ void testVelocity2() {
         // towards +x, 1 rad/m, 2 rad/s limit => 2 m/s
         YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest2(logger),
                 YAW_RATE_SCALE);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 1, // spatial, so rad/m
@@ -56,7 +56,7 @@ void testAccel() {
         YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forTest(logger),
                 YAW_RATE_SCALE);
         // driving and spinning
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 1,
@@ -73,7 +73,7 @@ void testAccel2() {
         double scale = 0.1;
         YawRateConstraint c = new YawRateConstraint(logger, SwerveKinodynamicsFactory.forRealisticTest(logger),
                 scale);
-        Pose2dWithMotion p = new Pose2dWithMotion(
+        PathPoint p = new PathPoint(
                 WaypointSE2.irrotational(
                         new Pose2d(0, 0, new Rotation2d(0)), 0, 1.2),
                 1, // spatial, so rad/m

From d7715847043e55daf24ee0a7faa35bfa35cd88f2 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sun, 28 Dec 2025 11:32:20 -0800
Subject: [PATCH 2/5] trajectory recycling WIP

---
 .../org/team100/lib/geometry/PathPoint.java   | 41 ++++++++++++
 .../team100/lib/logging/LoggerFactory.java    |  2 +-
 .../org/team100/lib/state/ControlSE2.java     | 14 ++--
 .../java/org/team100/lib/state/ModelSE2.java  |  2 +-
 .../tank/commands/FixedTrajectory.java        |  4 +-
 .../tank/commands/ToPoseWithTrajectory.java   |  4 +-
 .../team100/lib/trajectory/Trajectory100.java | 31 +++++----
 .../lib/trajectory/TrajectoryPlanner.java     |  6 +-
 .../path/spline/HolonomicSpline.java          |  1 +
 .../lib/trajectory/timing/TimedState.java     | 18 ++---
 .../trajectory/timing/TrajectoryFactory.java  |  4 +-
 .../trajectory/timing/TrajectoryRecycler.java | 24 +++++++
 .../TrajectoryVisualization.java              |  2 +-
 .../se2/ReferenceControllerSE2Test.java       |  2 +-
 .../lib/trajectory/Trajectory100Test.java     | 16 ++---
 .../lib/trajectory/TrajectoryPlannerTest.java | 12 ++--
 .../lib/trajectory/TrajectoryTest.java        | 14 ++--
 .../trajectory/TrajectoryToVectorSeries.java  |  6 +-
 .../lib/trajectory/timing/TimedStateTest.java |  2 +-
 .../timing/TrajectoryFactoryTest.java         | 14 ++--
 .../timing/TrajectoryRecyclerTest.java        | 66 +++++++++++++++++++
 21 files changed, 212 insertions(+), 73 deletions(-)

diff --git a/lib/src/main/java/org/team100/lib/geometry/PathPoint.java b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
index beac26d9..1b64a1e4 100644
--- a/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
+++ b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
@@ -1,5 +1,6 @@
 package org.team100.lib.geometry;
 
+import org.team100.lib.trajectory.path.spline.HolonomicSpline;
 import org.team100.lib.util.Math100;
 
 import edu.wpi.first.math.MathUtil;
@@ -13,6 +14,10 @@ public class PathPoint {
     private static final boolean DEBUG = false;
     /** Pose and course. */
     private final WaypointSE2 m_waypoint;
+    /** The source of this point (for resampling) */
+    private final HolonomicSpline m_spline;
+    /** The parameter value of this point (for resampling) */
+    private final double m_s;
     /** Change in heading per meter of motion, rad/m. */
     private final double m_headingRateRad_M;
     /** Change in course per change in distance, rad/m. */
@@ -25,9 +30,24 @@ public class PathPoint {
      */
     public PathPoint(
             WaypointSE2 waypoint,
+            HolonomicSpline spline,
+            double s,
             double headingRateRad_M,
             double curvatureRad_M) {
         m_waypoint = waypoint;
+        m_spline = spline;
+        m_s = s;
+        m_headingRateRad_M = headingRateRad_M;
+        m_curvatureRad_M = curvatureRad_M;
+    }
+
+    public PathPoint(
+            WaypointSE2 waypoint,
+            double headingRateRad_M,
+            double curvatureRad_M) {
+        m_waypoint = waypoint;
+        m_spline = null;
+        m_s = 0;
         m_headingRateRad_M = headingRateRad_M;
         m_curvatureRad_M = curvatureRad_M;
     }
@@ -56,8 +76,29 @@ public double getCurvatureRad_M() {
      * Not a constant-twist arc.
      */
     public PathPoint interpolate(PathPoint other, double x) {
+        HolonomicSpline spline = null;
+        double s = 0;
+        if (m_spline == other.m_spline) {
+            // ok to interpolate using this spline
+            spline = m_spline;
+            s = Math100.interpolate(m_s, other.m_s, x);
+        } else {
+            // which one to use?
+            // one of the endpoints should be the spline endpoint
+            // which is always the zero (not the 1)
+            if (other.m_s < 1e-6) {
+                // other one is the start, so use this one
+                spline = m_spline;
+                s = Math100.interpolate(m_s, 1, x);
+            } else {
+                spline = other.m_spline;
+                s = Math100.interpolate(other.m_s, 1, x);
+            }
+        }
         return new PathPoint(
                 GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x),
+                spline,
+                s,
                 MathUtil.interpolate(m_headingRateRad_M, other.m_headingRateRad_M, x),
                 Math100.interpolate(m_curvatureRad_M, other.m_curvatureRad_M, x));
     }
diff --git a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java
index abb496ae..00aac4f5 100644
--- a/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java
+++ b/lib/src/main/java/org/team100/lib/logging/LoggerFactory.java
@@ -468,7 +468,7 @@ public void log(Supplier<TimedState> vals) {
             if (!allow(m_level))
                 return;
             TimedState val = vals.get();
-            m_pose2dWithMotionLogger.log(val::state);
+            m_pose2dWithMotionLogger.log(val::point);
             m_timeLogger.log(val::getTimeS);
             m_velocityLogger.log(val::velocityM_S);
             m_accelLogger.log(val::acceleration);
diff --git a/lib/src/main/java/org/team100/lib/state/ControlSE2.java b/lib/src/main/java/org/team100/lib/state/ControlSE2.java
index 66c690a0..053994b6 100644
--- a/lib/src/main/java/org/team100/lib/state/ControlSE2.java
+++ b/lib/src/main/java/org/team100/lib/state/ControlSE2.java
@@ -127,23 +127,23 @@ public Control100 theta() {
      * Correctly accounts for centripetal acceleration.
      */
     public static ControlSE2 fromTimedState(TimedState timedPose) {
-        double xx = timedPose.state().waypoint().pose().getTranslation().getX();
-        double yx = timedPose.state().waypoint().pose().getTranslation().getY();
-        double thetax = timedPose.state().waypoint().pose().getRotation().getRadians();
+        double xx = timedPose.point().waypoint().pose().getTranslation().getX();
+        double yx = timedPose.point().waypoint().pose().getTranslation().getY();
+        double thetax = timedPose.point().waypoint().pose().getRotation().getRadians();
 
         double velocityM_s = timedPose.velocityM_S();
-        Rotation2d course = timedPose.state().waypoint().course().toRotation();
+        Rotation2d course = timedPose.point().waypoint().course().toRotation();
         double xv = course.getCos() * velocityM_s;
         double yv = course.getSin() * velocityM_s;
-        double thetav = timedPose.state().getHeadingRateRad_M() * velocityM_s;
+        double thetav = timedPose.point().getHeadingRateRad_M() * velocityM_s;
 
         double accelM_s_s = timedPose.acceleration();
         double xa = course.getCos() * accelM_s_s;
         double ya = course.getSin() * accelM_s_s;
-        double thetaa = timedPose.state().getHeadingRateRad_M() * accelM_s_s;
+        double thetaa = timedPose.point().getHeadingRateRad_M() * accelM_s_s;
 
         // centripetal accel = v^2/r = v^2 * curvature
-        double curvRad_M = timedPose.state().getCurvatureRad_M();
+        double curvRad_M = timedPose.point().getCurvatureRad_M();
         double centripetalAccelM_s_s = velocityM_s * velocityM_s * curvRad_M;
         double xCa = -1.0 * course.getSin() * centripetalAccelM_s_s;
         double yCa = course.getCos() * centripetalAccelM_s_s;
diff --git a/lib/src/main/java/org/team100/lib/state/ModelSE2.java b/lib/src/main/java/org/team100/lib/state/ModelSE2.java
index 48362339..b611ffcf 100644
--- a/lib/src/main/java/org/team100/lib/state/ModelSE2.java
+++ b/lib/src/main/java/org/team100/lib/state/ModelSE2.java
@@ -136,7 +136,7 @@ public Model100 theta() {
      * Transform timed pose into swerve state.
      */
     public static ModelSE2 fromTimedState(TimedState timedPose) {
-        PathPoint state = timedPose.state();
+        PathPoint state = timedPose.point();
         WaypointSE2 pose = state.waypoint();
         Translation2d translation = pose.pose().getTranslation();
         double xx = translation.getX();
diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java
index 901faa00..c3536e8b 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/FixedTrajectory.java
@@ -63,9 +63,9 @@ public void execute() {
         // next for feedforward (and selecting K)
         TimedState next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S);
         Pose2d currentPose = m_drive.getPose();
-        Pose2d poseReference = current.state().waypoint().pose();
+        Pose2d poseReference = current.point().waypoint().pose();
         double velocityReference = next.velocityM_S();
-        double omegaReference = next.velocityM_S() * next.state().getHeadingRateRad_M();
+        double omegaReference = next.velocityM_S() * next.point().getHeadingRateRad_M();
         ChassisSpeeds speeds = m_controller.calculate(
                 currentPose, poseReference, velocityReference, omegaReference);
         m_drive.setVelocity(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
diff --git a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java
index 77df5c2d..fbd2c79d 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/tank/commands/ToPoseWithTrajectory.java
@@ -70,9 +70,9 @@ public void execute() {
             // next for feedforward (and selecting K)
             TimedState next = m_trajectory.sample(t + TimedRobot100.LOOP_PERIOD_S);
             Pose2d currentPose = m_drive.getPose();
-            Pose2d poseReference = current.state().waypoint().pose();
+            Pose2d poseReference = current.point().waypoint().pose();
             double velocityReference = next.velocityM_S();
-            double omegaReference = next.velocityM_S() * next.state().getHeadingRateRad_M();
+            double omegaReference = next.velocityM_S() * next.point().getHeadingRateRad_M();
             ChassisSpeeds speeds = m_controller.calculate(
                     currentPose, poseReference, velocityReference, omegaReference);
             m_drive.setVelocity(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
index 18fd95f1..96e451c3 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
@@ -6,6 +6,7 @@
 import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.trajectory.timing.TimedState;
+import org.team100.lib.trajectory.timing.TimingConstraint;
 
 import edu.wpi.first.math.geometry.Pose2d;
 
@@ -14,16 +15,20 @@
  */
 public class Trajectory100 {
     private final List<TimedState> m_points;
+    /** Constraints used for this trajectory, for resampling */
+    public final List<TimingConstraint> m_constraints;
     private final double m_duration;
 
     public Trajectory100() {
         m_points = new ArrayList<>();
+        m_constraints = new ArrayList<>();
         m_duration = 0;
     }
 
     /** First timestamp must be zero. */
-    public Trajectory100(final List<TimedState> states) {
+    public Trajectory100(List<TimedState> states, List<TimingConstraint> constraints) {
         m_points = states;
+        m_constraints = constraints;
         m_duration = m_points.get(m_points.size() - 1).getTimeS();
     }
 
@@ -106,7 +111,7 @@ public void dump() {
         System.out.println("i, t, v, a, k, x, y");
         for (int i = 0; i < length(); ++i) {
             TimedState ts = getPoint(i);
-            PathPoint pwm = ts.state();
+            PathPoint pwm = ts.point();
             WaypointSE2 w = pwm.waypoint();
             Pose2d p = w.pose();
             System.out.printf("%d, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
@@ -114,16 +119,16 @@ public void dump() {
         }
     }
 
-        /** For cutting-and-pasting into a spreadsheet */
-        public void tdump() {
-            System.out.println("t, v, a, k, x, y");
-            for (double t = 0; t < duration(); t += 0.02) {
-                TimedState ts = sample(t);
-                PathPoint pwm = ts.state();
-                WaypointSE2 w = pwm.waypoint();
-                Pose2d p = w.pose();
-                System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
-                        ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(), p.getX(), p.getY());
-            }
+    /** For cutting-and-pasting into a spreadsheet */
+    public void tdump() {
+        System.out.println("t, v, a, k, x, y");
+        for (double t = 0; t < duration(); t += 0.02) {
+            TimedState ts = sample(t);
+            PathPoint pwm = ts.point();
+            WaypointSE2 w = pwm.waypoint();
+            Pose2d p = w.pose();
+            System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
+                    ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(), p.getX(), p.getY());
         }
+    }
 }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java
index c568d0cd..403a2811 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/TrajectoryPlanner.java
@@ -39,13 +39,15 @@ public Trajectory100 restToRest(List<WaypointSE2> waypoints) {
 
     /////////////////////////////////////////////////////////////////////////////////////
     ///
-    ///
+    /// DANGER ZONE
+    /// 
+    /// Only use this if you know what you're doing.
 
     /**
      * Makes a trajectory through the supplied waypoints, with start and end
      * velocities.
      */
-    Trajectory100 generateTrajectory(
+    public Trajectory100 generateTrajectory(
             List<WaypointSE2> waypoints, double start_vel, double end_vel) {
         try {
             // Create a path from splines.
diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java
index 579396fb..f23fc3d7 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/HolonomicSpline.java
@@ -121,6 +121,7 @@ public PathPoint getPathPoint(double s) {
                 new WaypointSE2(
                         new Pose2d(new Translation2d(x(s), y(s)), getHeading(s)),
                         getCourse(s), 1),
+                this, s,
                 getDHeadingDs(s),
                 getCurvature(s));
     }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
index f638a184..1e5b1e8b 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
@@ -11,7 +11,7 @@
  */
 public class TimedState {
     private static final boolean DEBUG = false;
-    private final PathPoint m_state;
+    private final PathPoint m_point;
     /** Time we achieve this state. */
     private final double m_timeS;
     /** Instantaneous pathwise velocity, m/s. */
@@ -27,14 +27,14 @@ public TimedState(
             double t,
             double velocity,
             double acceleration) {
-        m_state = state;
+        m_point = state;
         m_timeS = t;
         m_velocityM_S = velocity;
         m_accelM_S_S = acceleration;
     }
 
-    public PathPoint state() {
-        return m_state;
+    public PathPoint point() {
+        return m_point;
     }
 
     /** Time we achieve this state. */
@@ -55,7 +55,7 @@ public double acceleration() {
     @Override
     public String toString() {
         return String.format("state %s, time %5.3f, vel %5.3f, acc %5.3f",
-                m_state,
+                m_point,
                 m_timeS,
                 m_velocityM_S,
                 m_accelM_S_S);
@@ -72,7 +72,7 @@ public TimedState interpolate(TimedState other, double delta_t) {
         double vLerp = m_velocityM_S + m_accelM_S_S * delta_t;
         double pathwiseDistance = m_velocityM_S * delta_t + 0.5 * m_accelM_S_S * delta_t * delta_t;
 
-        double distanceBetween = m_state.distanceCartesian(other.m_state);
+        double distanceBetween = m_point.distanceCartesian(other.m_point);
         double interpolant = pathwiseDistance / distanceBetween;
         if (Double.isNaN(interpolant)) {
             interpolant = 1.0;
@@ -81,7 +81,7 @@ public TimedState interpolate(TimedState other, double delta_t) {
         if (DEBUG)
             System.out.printf("tlerp %f\n", tLerp);
         return new TimedState(
-                m_state.interpolate(other.m_state, interpolant),
+                m_point.interpolate(other.m_point, interpolant),
                 tLerp,
                 vLerp,
                 m_accelM_S_S);
@@ -89,7 +89,7 @@ public TimedState interpolate(TimedState other, double delta_t) {
 
     /** Translation only, ignores rotation */
     public double distanceCartesian(TimedState other) {
-        return m_state.distanceCartesian(other.m_state);
+        return m_point.distanceCartesian(other.m_point);
     }
 
     @Override
@@ -100,7 +100,7 @@ public boolean equals(final Object other) {
             return false;
         }
         TimedState ts = (TimedState) other;
-        if (!m_state.equals(ts.m_state)) {
+        if (!m_point.equals(ts.m_point)) {
             if (DEBUG)
                 System.out.println("wrong state");
             return false;
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
index 663d2768..c2a13a55 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
@@ -50,7 +50,7 @@ private PathPoint[] getSamples(Path100 path) {
      * 
      * Output is these same samples with time.
      */
-    private Trajectory100 fromSamples(
+    public Trajectory100 fromSamples(
             PathPoint[] samples,
             double start_vel,
             double end_vel) {
@@ -59,7 +59,7 @@ private Trajectory100 fromSamples(
         double[] accels = accels(distances, velocities);
         double[] runningTime = runningTime(distances, velocities, accels);
         List<TimedState> timedStates = timedStates(samples, velocities, accels, runningTime);
-        return new Trajectory100(timedStates);
+        return new Trajectory100(timedStates, m_constraints);
     }
 
     /**
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
index 18592236..e61a9f24 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
@@ -1,5 +1,11 @@
 package org.team100.lib.trajectory.timing;
 
+import java.util.ArrayList;
+import java.util.List;
+
+import org.team100.lib.geometry.PathPoint;
+import org.team100.lib.trajectory.Trajectory100;
+
 /**
  * Resamples a trajectory with constant time between samples.
  * 
@@ -17,4 +23,22 @@
  */
 public class TrajectoryRecycler {
 
+    public static Trajectory100 recycle(Trajectory100 original, double dt) {
+        double start_vel = original.sample(0).velocityM_S();
+        double end_vel = original.getLastPoint().velocityM_S();
+
+        List<PathPoint> points = new ArrayList<>();
+        // a little past the end to make sure we get the last point.
+        for (double t = 0; t < original.duration() + dt; t += dt) {
+            TimedState sample = original.sample(t);
+            // these contain discretization errors
+            // TODO: go back to the original spline for these
+            PathPoint point = sample.point();
+            points.add(point);
+        }
+        return new TrajectoryFactory(original.m_constraints)
+                .fromSamples(points.toArray(new PathPoint[0]), start_vel, end_vel);
+
+    }
+
 }
diff --git a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java
index 85dc5cf0..fe1bf60c 100644
--- a/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java
+++ b/lib/src/main/java/org/team100/lib/visualization/TrajectoryVisualization.java
@@ -32,7 +32,7 @@ private static double[] fromTrajectory100(Trajectory100 m_trajectory) {
         double[] arr = new double[m_trajectory.length() * 3];
         int ndx = 0;
         for (TimedState p : m_trajectory.getPoints()) {
-            WaypointSE2 pose = p.state().waypoint();
+            WaypointSE2 pose = p.point().waypoint();
             arr[ndx + 0] = pose.pose().getTranslation().getX();
             arr[ndx + 1] = pose.pose().getTranslation().getY();
             arr[ndx + 2] = pose.pose().getRotation().getDegrees();
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
index 44cbdb21..69db4b64 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
@@ -182,7 +182,7 @@ void testFieldRelativeTrajectory() {
         VelocityReferenceControllerSE2 referenceController = new VelocityReferenceControllerSE2(
                 logger, drive, swerveController, reference);
 
-        Pose2d pose = trajectory.sample(0).state().waypoint().pose();
+        Pose2d pose = trajectory.sample(0).point().waypoint().pose();
         VelocitySE2 velocity = VelocitySE2.ZERO;
 
         double mDt = 0.02;
diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
index 409fb5e7..21ba9e62 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
@@ -53,16 +53,16 @@ void testPreviewAndAdvance() {
         Trajectory100 trajectory = planner.restToRest(waypoints);
 
         TimedState sample = trajectory.sample(0);
-        assertEquals(0, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
 
         sample = trajectory.sample(1);
-        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
 
         sample = trajectory.sample(2);
-        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
 
         sample = trajectory.sample(3);
-        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
     }
 
     @Test
@@ -90,11 +90,11 @@ void testSample() {
 
         assertEquals(1.415, trajectory.duration(), DELTA);
         TimedState sample = trajectory.sample(0);
-        assertEquals(0, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
         sample = trajectory.sample(1);
-        assertEquals(0.828, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.828, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
         sample = trajectory.sample(2);
-        assertEquals(1, sample.state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(1, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
     }
 
     /**
@@ -150,7 +150,7 @@ void testSampleThoroughly() {
     }
 
     private void check(Trajectory100 trajectory, double t, double x) {
-        assertEquals(x, trajectory.sample(t).state().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(x, trajectory.sample(t).point().waypoint().pose().getTranslation().getX(), DELTA);
     }
 
     /**
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
index 17c68cb7..eb997df8 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
@@ -61,8 +61,8 @@ void testLinear() {
         // start at zero velocity
         assertEquals(0, tp.velocityM_S(), DELTA);
         TimedState p = t.getPoint(8);
-        assertEquals(0.5, p.state().waypoint().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
+        assertEquals(0.5, p.point().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.point().getHeadingRateRad_M(), DELTA);
     }
 
     @Test
@@ -93,8 +93,8 @@ void testLinearRealistic() {
         TimedState tp = t.getPoint(0);
         assertEquals(0, tp.velocityM_S(), DELTA);
         TimedState p = t.getPoint(8);
-        assertEquals(0.5, p.state().waypoint().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
+        assertEquals(0.5, p.point().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.point().getHeadingRateRad_M(), DELTA);
     }
 
     /**
@@ -135,8 +135,8 @@ void testPerformance() {
         }
         assertEquals(33, t.length());
         TimedState p = t.getPoint(12);
-        assertEquals(0.605, p.state().waypoint().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
+        assertEquals(0.605, p.point().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.point().getHeadingRateRad_M(), DELTA);
     }
 
     @Test
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java
index d4dc588d..43a68358 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryTest.java
@@ -118,23 +118,23 @@ void testDheading() {
                     "t, intrinsic_heading_dt, heading_dt, intrinsic_ca, extrinsic_ca, extrinsic v, intrinsic v, dcourse, dcourse1");
         for (double t = 0.04; t < duration; t += 0.04) {
             TimedState p1 = trajectory.sample(t);
-            Rotation2d heading0 = p0.state().waypoint().pose().getRotation();
-            Rotation2d heading1 = p1.state().waypoint().pose().getRotation();
+            Rotation2d heading0 = p0.point().waypoint().pose().getRotation();
+            Rotation2d heading1 = p1.point().waypoint().pose().getRotation();
             double dheading = heading1.minus(heading0).getRadians();
             // compute time derivative of heading two ways:
             // this just compares the poses and uses the known time step
             double dheadingDt = dheading / 0.04;
             // this uses the intrinsic heading rate and the velocity
             // rad/m * m/s = rad/s
-            double intrinsicDheadingDt = p0.state().getHeadingRateRad_M() * p0.velocityM_S();
+            double intrinsicDheadingDt = p0.point().getHeadingRateRad_M() * p0.velocityM_S();
             // curvature is used to compute centripetal acceleration
             // ca = v^2*curvature
-            DirectionSE2 course0 = p0.state().waypoint().course();
-            DirectionSE2 course1 = p1.state().waypoint().course();
-            p1.state().waypoint().pose().log(p0.state().waypoint().pose());
+            DirectionSE2 course0 = p0.point().waypoint().course();
+            DirectionSE2 course1 = p1.point().waypoint().course();
+            p1.point().waypoint().pose().log(p0.point().waypoint().pose());
             double dcourse1 = Metrics.translationalNorm(course1.minus(course0));
             double dcourse = course1.toRotation().minus(course0.toRotation()).getRadians();
-            double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.state().getCurvatureRad_M();
+            double intrinsicCa = p0.velocityM_S() * p0.velocityM_S() * p0.point().getCurvatureRad_M();
 
             if (DEBUG)
                 System.out.printf("%5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java
index f601e099..28ba11f1 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryToVectorSeries.java
@@ -27,7 +27,7 @@ public VectorSeries convert(String name, Trajectory100 t) {
         double dt = duration / POINTS;
         for (double time = 0; time < duration; time += dt) {
             TimedState p = t.sample(time);
-            WaypointSE2 pp = p.state().waypoint();
+            WaypointSE2 pp = p.point().waypoint();
             double x = pp.pose().getTranslation().getX();
             double y = pp.pose().getTranslation().getY();
             Rotation2d heading = pp.pose().getRotation();
@@ -51,7 +51,7 @@ public static XYSeries x(String name, Trajectory100 trajectory) {
         double dt = duration / POINTS;
         for (double t = 0; t <= duration + 0.0001; t += dt) {
             TimedState p = trajectory.sample(t);
-            WaypointSE2 pp = p.state().waypoint();
+            WaypointSE2 pp = p.point().waypoint();
             double x = pp.pose().getTranslation().getX();
             series.add(t, x);
         }
@@ -69,7 +69,7 @@ public static XYSeries xdot(String name, Trajectory100 trajectory) {
         double dt = duration / POINTS;
         for (double t = 0; t <= duration + 0.0001; t += dt) {
             TimedState p = trajectory.sample(t);
-            Rotation2d course = p.state().waypoint().course().toRotation();
+            Rotation2d course = p.point().waypoint().course().toRotation();
             double velocityM_s = p.velocityM_S();
             System.out.println(velocityM_s);
             System.out.println(course);
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
index 8da514d1..aa9ed599 100755
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
@@ -40,6 +40,6 @@ void test() {
         assertEquals(start_state.acceleration(), intermediate_state.acceleration(), EPSILON);
         assertEquals(0.5, intermediate_state.velocityM_S(), EPSILON);
         // close to the start state by distance
-        assertEquals(0.125, intermediate_state.state().waypoint().pose().getTranslation().getX(), EPSILON);
+        assertEquals(0.125, intermediate_state.point().waypoint().pose().getTranslation().getX(), EPSILON);
     }
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java
index 340f5d9e..2af5827b 100755
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryFactoryTest.java
@@ -79,14 +79,14 @@ public void checkTrajectory(
         TimedState prev_state = null;
         for (TimedState state : traj.getPoints()) {
             for (final TimingConstraint constraint : constraints) {
-                assertTrue(state.velocityM_S() - EPSILON <= constraint.maxV(state.state()));
+                assertTrue(state.velocityM_S() - EPSILON <= constraint.maxV(state.point()));
                 assertTrue(state.acceleration() - EPSILON <= constraint.maxAccel(
-                        state.state(), state.velocityM_S()),
+                        state.point(), state.velocityM_S()),
                         String.format("%f %f", state.acceleration(), constraint.maxAccel(
-                                state.state(), state.velocityM_S())));
-                assertTrue(state.acceleration() + EPSILON >= constraint.maxDecel(state.state(), state.velocityM_S()),
+                                state.point(), state.velocityM_S())));
+                assertTrue(state.acceleration() + EPSILON >= constraint.maxDecel(state.point(), state.velocityM_S()),
                         String.format("%f %f", state.acceleration(),
-                                constraint.maxDecel(state.state(), state.velocityM_S())));
+                                constraint.maxDecel(state.point(), state.velocityM_S())));
             }
             if (prev_state != null) {
                 assertEquals(state.velocityM_S(),
@@ -278,8 +278,8 @@ void testPerformance() {
         }
         assertEquals(33, t.length());
         TimedState p = t.getPoint(12);
-        assertEquals(0.605, p.state().waypoint().pose().getTranslation().getX(), DELTA);
-        assertEquals(0, p.state().getHeadingRateRad_M(), DELTA);
+        assertEquals(0.605, p.point().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0, p.point().getHeadingRateRad_M(), DELTA);
 
     }
 
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
index ad33f4c7..d90e0b60 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
@@ -1,10 +1,76 @@
 package org.team100.lib.trajectory.timing;
 
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.List;
+
 import org.junit.jupiter.api.Test;
+import org.team100.lib.geometry.DirectionSE2;
+import org.team100.lib.geometry.WaypointSE2;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.logging.TestLoggerFactory;
+import org.team100.lib.logging.primitive.TestPrimitiveLogger;
+import org.team100.lib.trajectory.Trajectory100;
+import org.team100.lib.trajectory.TrajectoryPlanner;
+import org.team100.lib.trajectory.examples.TrajectoryExamples;
+import org.team100.lib.trajectory.path.PathFactory;
+
+import com.ctre.phoenix6.Utils;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 public class TrajectoryRecyclerTest {
+    LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger());
+
     @Test
     void test0() {
+        // this forces the ctre output to the top
+        Utils.isSimulation();
+        List<TimingConstraint> constraints = List.of(new ConstantConstraint(log, 1, 1));
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        PathFactory pathFactory = new PathFactory();
+        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
+        WaypointSE2 w0 = new WaypointSE2(
+                new Pose2d(0, 0, new Rotation2d()),
+                new DirectionSE2(1, 0, 0), 1);
+        WaypointSE2 w1 = new WaypointSE2(
+                new Pose2d(1, 0, new Rotation2d()),
+                new DirectionSE2(1, 0, 0), 1);
+        List<WaypointSE2> waypoints = List.of(w0, w1);
+        Trajectory100 original = planner.restToRest(waypoints);
+        original.dump();
+        Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.1);
+        recycled.dump();
+    }
+
+    /**
+     * see TrajectoryPlannerTest.test2d2()
+     * Straight, curve, straight.
+     */
+    @Test
+    void test2d2() {
+        // this forces the ctre output to the top
+        Utils.isSimulation();
+        List<WaypointSE2> waypoints = List.of(
+                new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
+                new WaypointSE2(new Pose2d(1, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
+                new WaypointSE2(new Pose2d(2, 1, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3),
+                new WaypointSE2(new Pose2d(2, 2, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3));
+
+        List<TimingConstraint> constraints = List.of(
+                new ConstantConstraint(log, 1, 1),
+                new CapsizeAccelerationConstraint(log, 0.5, 1));
+        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
+        // coarser than before
+        PathFactory pathFactory = new PathFactory(0.5, 0.2, 0.2, 1);
+        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
+
+        Trajectory100 original = planner.generateTrajectory(waypoints, 1, 1);
+        original.dump();
+        Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.1);
+        recycled.dump();
+
     }
 
 }

From 3be3c75583bc156dbfd4adc54ef2bbc2e386f2b0 Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sun, 28 Dec 2025 17:06:16 -0800
Subject: [PATCH 3/5] trajectory recycler and soft start/end kinda work now

---
 .../org/team100/lib/geometry/PathPoint.java   | 26 +++++++++++-
 .../team100/lib/trajectory/Trajectory100.java | 36 +++++++++++++++--
 .../timing/CapsizeAccelerationConstraint.java | 20 ++++++----
 .../timing/SoftRegionConstraint.java          | 13 +++---
 .../lib/trajectory/timing/TimedState.java     | 40 +++++++++++++++++++
 .../trajectory/timing/TrajectoryFactory.java  | 35 ++++++++++++++--
 .../trajectory/timing/TrajectoryRecycler.java | 11 ++++-
 .../se2/ReferenceControllerSE2Test.java       |  6 +--
 .../se2/TrajectoryReferenceTest.java          |  8 ++--
 .../DriveWithTrajectoryListFunctionTest.java  |  2 +-
 .../test/DriveWithTrajectoryTest.java         |  8 ++--
 .../lib/trajectory/Trajectory100Test.java     | 20 +++++-----
 .../lib/trajectory/TrajectoryPlannerTest.java |  4 +-
 .../timing/SoftRegionConstraintTest.java      | 13 +++---
 .../lib/trajectory/timing/TimedStateTest.java |  4 +-
 .../timing/TrajectoryRecyclerTest.java        | 30 ++++++++------
 16 files changed, 209 insertions(+), 67 deletions(-)

diff --git a/lib/src/main/java/org/team100/lib/geometry/PathPoint.java b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
index 1b64a1e4..4cb30d51 100644
--- a/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
+++ b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
@@ -70,16 +70,27 @@ public double getCurvatureRad_M() {
         return m_curvatureRad_M;
     }
 
+    public double getS() {
+        return m_s;
+    }
+
     /**
      * Linear interpolation of each component separately.
      * 
+     * TODO: this is wrong for the spline parameter, it's the distance.
+     * 
      * Not a constant-twist arc.
      */
     public PathPoint interpolate(PathPoint other, double x) {
+        if (DEBUG)
+            System.out.printf("this s %f other s %f\n",
+                    m_s, other.m_s);
         HolonomicSpline spline = null;
         double s = 0;
         if (m_spline == other.m_spline) {
             // ok to interpolate using this spline
+            if (DEBUG)
+                System.out.println("same spline");
             spline = m_spline;
             s = Math100.interpolate(m_s, other.m_s, x);
         } else {
@@ -88,13 +99,26 @@ public PathPoint interpolate(PathPoint other, double x) {
             // which is always the zero (not the 1)
             if (other.m_s < 1e-6) {
                 // other one is the start, so use this one
+                if (DEBUG)
+                    System.out.println("use this spline");
                 spline = m_spline;
                 s = Math100.interpolate(m_s, 1, x);
             } else {
+                if (DEBUG)
+                    System.out.println("use the other spline");
                 spline = other.m_spline;
-                s = Math100.interpolate(other.m_s, 1, x);
+                s = Math100.interpolate(0, other.m_s, x);
             }
         }
+        if (DEBUG)
+            System.out.printf("s0 %f s1 %f x %f s %f\n",
+                    m_s, other.m_s, x, s);
+        // sample the spline again instead of interpolating.
+        if (spline != null)
+            return spline.getPathPoint(s);
+        // TODO: remove this way
+        if (DEBUG)
+            System.out.println("no spline");
         return new PathPoint(
                 GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x),
                 spline,
diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
index 96e451c3..cade34d6 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
@@ -65,6 +65,35 @@ public TimedState sample(double timeS) {
         throw new IllegalStateException("impossible trajectory: " + toString());
     }
 
+    /** Uses non-constant acceleration, for resampling */
+    public TimedState sample2(double timeS) {
+        // This scans the whole trajectory for every sample, but most of the time
+        // is the interpolation; I tried a TreeMap index and it only saved a few
+        // nanoseconds per call.
+        if (isEmpty())
+            throw new IllegalStateException("can't sample an empty trajectory");
+        if (timeS >= m_duration) {
+            return getLastPoint();
+        }
+        if (timeS <= 0) {
+            return getPoint(0);
+        }
+
+        for (int i = 1; i < length(); ++i) {
+            final TimedState ceil = getPoint(i);
+            if (ceil.getTimeS() >= timeS) {
+                final TimedState floor = getPoint(i - 1);
+                double span = ceil.getTimeS() - floor.getTimeS();
+                if (Math.abs(span) <= 1e-12) {
+                    return ceil;
+                }
+                double delta_t = timeS - floor.getTimeS();
+                return floor.interpolate2(ceil, delta_t);
+            }
+        }
+        throw new IllegalStateException("impossible trajectory: " + toString());
+    }
+
     /** Time is at or beyond the trajectory duration. */
     public boolean isDone(double timeS) {
         return timeS >= duration();
@@ -108,14 +137,15 @@ public String toString() {
 
     /** For cutting-and-pasting into a spreadsheet */
     public void dump() {
-        System.out.println("i, t, v, a, k, x, y");
+        System.out.println("i, s, t, v, a, k, x, y");
         for (int i = 0; i < length(); ++i) {
             TimedState ts = getPoint(i);
             PathPoint pwm = ts.point();
             WaypointSE2 w = pwm.waypoint();
             Pose2d p = w.pose();
-            System.out.printf("%d, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
-                    i, ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(), p.getX(), p.getY());
+            System.out.printf("%d, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f, %5.3f\n",
+                    i, pwm.getS(), ts.getTimeS(), ts.velocityM_S(), ts.acceleration(), pwm.getCurvatureRad_M(),
+                    p.getX(), p.getY());
         }
     }
 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java
index 51ba571a..4ef1bf7d 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/CapsizeAccelerationConstraint.java
@@ -51,7 +51,10 @@ public CapsizeAccelerationConstraint(LoggerFactory parent, double centripetal, d
     public double maxV(final PathPoint state) {
         double radius = 1 / Math.abs(state.getCurvatureRad_M());
         // abs is used here to make sure sqrt is happy.
-        return Math.sqrt(Math.abs(m_maxCentripetalAccel * m_scale.getAsDouble() * radius));
+        double maxV = Math.sqrt(Math.abs(m_maxCentripetalAccel * m_scale.getAsDouble() * radius));
+        if (DEBUG)
+            System.out.printf("maxV %f\n", maxV);
+        return maxV;
     }
 
     @Override
@@ -62,7 +65,10 @@ public double maxAccel(PathPoint state, double velocity) {
                 System.out.println("too fast for the curvature, can't speed up");
             return 0;
         }
-        return Math.sqrt(alongsq);
+        double maxA = Math.sqrt(alongsq);
+        if (DEBUG)
+            System.out.printf("maxA %f\n", maxA);
+        return maxA;
     }
 
     @Override
@@ -73,10 +79,10 @@ public double maxDecel(PathPoint state, double velocity) {
                 System.out.println("too fast for the curvature, slowing down is ok");
             return m_maxDecel * m_scale.getAsDouble();
         }
-        double decel = -Math.sqrt(alongsq);
+        double maxD = -Math.sqrt(alongsq);
         if (DEBUG)
-            System.out.printf("decel %f\n", decel);
-        return decel;
+            System.out.printf("maxD %f\n", maxD);
+        return maxD;
     }
 
     /**
@@ -94,8 +100,8 @@ private double alongSq(PathPoint state, double velocity) {
         double radius = 1 / Math.abs(state.getCurvatureRad_M());
         double actualCentripetalAccel = velocity * velocity / radius;
         if (DEBUG)
-            System.out.printf("radius %f actual centripetal %f\n",
-                    radius, actualCentripetalAccel);
+            System.out.printf("radius %f velocity %f actual centripetal accel %f\n",
+                    radius, velocity, actualCentripetalAccel);
         return m_maxCentripetalAccel * m_scale.getAsDouble() * m_maxCentripetalAccel * m_scale.getAsDouble()
                 - actualCentripetalAccel * actualCentripetalAccel;
     }
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
index c875b23a..8568aad6 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
@@ -9,13 +9,10 @@
  * Limits acceleration near a point.
  * 
  * There's a minimum at the point, and a maximum at the edge of a circle
- * surrounding it, interpolated using the sqrt of the distance.  This
- * provides roughly constant jerk.
+ * surrounding it, interpolated using the cube root of the distance.  This
+ * provides roughly constant jerk (i.e. constant growth rate of acceleration).
  * 
- * Because the path is discretized by length, and because acceleration is
- * constant between points, the interpolated samples have a rather jerky
- * acceleration curve when viewed on the time axis -- it's not really a
- * smooth jerk limit.
+ * To increase resolution of the slow part, you should use the TrajectoryRecycler.
  * 
  * https://docs.google.com/spreadsheets/d/1sbB-zTBUjRRlWHaWXe-V1ZDhAZCFwItVVO1x3LmZ4B4/edit?gid=691127145#gid=691127145
  */
@@ -48,7 +45,7 @@ public double maxV(PathPoint state) {
     public double maxAccel(PathPoint state, double velocityM_S) {
         double s = near(state);
         if (s < 1)
-            return Math100.interpolate(m_min, m_max, Math.sqrt(s));
+            return Math100.interpolate(m_min, m_max, Math.cbrt(s));
         return Double.POSITIVE_INFINITY;
     }
 
@@ -56,7 +53,7 @@ public double maxAccel(PathPoint state, double velocityM_S) {
     public double maxDecel(PathPoint state, double velocityM_S) {
         double s = near(state);
         if (s < 1)
-            return -1.0 * Math100.interpolate(m_min, m_max, Math.sqrt(s));
+            return -1.0 * Math100.interpolate(m_min, m_max, Math.cbrt(s));
         return Double.NEGATIVE_INFINITY;
     }
 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
index 1e5b1e8b..bd5642fb 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
@@ -68,6 +68,10 @@ public String toString() {
      * Acceleration of this state is constant through the whole arc.
      */
     public TimedState interpolate(TimedState other, double delta_t) {
+        if (delta_t < 0)
+            throw new IllegalArgumentException("delta_t must be non-negative");
+        if (DEBUG)
+            System.out.println("lerp");
         double tLerp = m_timeS + delta_t;
         double vLerp = m_velocityM_S + m_accelM_S_S * delta_t;
         double pathwiseDistance = m_velocityM_S * delta_t + 0.5 * m_accelM_S_S * delta_t * delta_t;
@@ -78,9 +82,45 @@ public TimedState interpolate(TimedState other, double delta_t) {
             interpolant = 1.0;
         }
 
+        // TODO: pass t interpolant, not just spatial one
+        double s = delta_t / (other.m_timeS - m_timeS);
+        if (DEBUG)
+            System.out.printf("t0 %f t1 %f delta t %f s %f\n",
+                    m_timeS, other.m_timeS, delta_t, s);
+
+        if (DEBUG)
+            System.out.printf("tlerp %f\n", tLerp);
+        return new TimedState(
+                // m_point.interpolate(other.m_point, interpolant),
+                m_point.interpolate(other.m_point, s),
+                tLerp,
+                vLerp,
+                m_accelM_S_S);
+    }
+
+    /** Tries to take variation in acceleration into account. For resampling. */
+    public TimedState interpolate2(TimedState other, double delta_t) {
+        if (delta_t < 0)
+            throw new IllegalArgumentException("delta_t must be non-negative");
+        if (DEBUG)
+            System.out.println("lerp");
+        double tLerp = m_timeS + delta_t;
+        double vLerp = m_velocityM_S + m_accelM_S_S * delta_t;
+        double dt = other.m_timeS - m_timeS;
+        double j = (other.m_accelM_S_S - m_accelM_S_S) / dt;
+        double pathwiseDistance = m_velocityM_S * delta_t + 0.5 * m_accelM_S_S * delta_t * delta_t
+                + 0.1666 * j * delta_t * delta_t * delta_t;
+
+        double distanceBetween = m_velocityM_S * dt + 0.5 * m_accelM_S_S * dt * dt + 0.1666 * j * dt * dt * dt;
+        double interpolant = pathwiseDistance / distanceBetween;
+        if (Double.isNaN(interpolant)) {
+            interpolant = 1.0;
+        }
+
         if (DEBUG)
             System.out.printf("tlerp %f\n", tLerp);
         return new TimedState(
+                // m_point.interpolate(other.m_point, interpolant),
                 m_point.interpolate(other.m_point, interpolant),
                 tLerp,
                 vLerp,
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
index c2a13a55..a1943716 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryFactory.java
@@ -133,22 +133,33 @@ private void forward(
         int n = samples.length;
         velocities[0] = start_vel;
         for (int i = 0; i < n - 1; ++i) {
+            if (DEBUG)
+                System.out.printf("FWD i %d\n", i);
             double arclength = distances[i + 1] - distances[i];
             if (Math.abs(arclength) < EPSILON) {
+                if (DEBUG)
+                    System.out.printf("i %d zero arc\n", i);
                 // zero-length arcs have the same state at both ends
                 velocities[i + 1] = velocities[i];
                 break;
             }
             // velocity constraint depends only on state
             double maxVelocity = maxVelocity(samples[i + 1]);
+            if (DEBUG)
+                System.out.printf("maxV i %d %f\n", i + 1, maxVelocity);
             // start with the maximum velocity
             velocities[i + 1] = maxVelocity;
             // reduce velocity to fit under the acceleration constraint
             double impliedAccel = Math100.accel(velocities[i], velocities[i + 1], arclength);
             double maxAccel = maxAccel(samples[i], velocities[i]);
-            if (impliedAccel > maxAccel + EPSILON) {
+            if (impliedAccel > maxAccel/* + EPSILON */) {
                 velocities[i + 1] = Math100.v1(velocities[i], maxAccel, arclength);
+                if (DEBUG)
+                    System.out.printf("adjust vi+1 %f\n", velocities[i + 1]);
             }
+            if (DEBUG)
+                System.out.printf("FWD i %d vi %f vi+1 %f maxA %f impliedA %f\n",
+                        i, velocities[i], velocities[i + 1], maxAccel, impliedAccel);
         }
     }
 
@@ -165,17 +176,35 @@ private void backward(
         int n = samples.length;
         velocities[n - 1] = end_vel;
         for (int i = n - 2; i >= 0; --i) {
+            if (DEBUG)
+                System.out.printf("BACK i %d\n", i);
             double arclength = distances[i + 1] - distances[i];
             if (Math.abs(arclength) < EPSILON) {
                 // already handled this case
                 break;
             }
+
+            double maxVelocity = maxVelocity(samples[i]);
+            if (DEBUG)
+                System.out.printf("maxV i %d %f\n", i, maxVelocity);
+
             double impliedAccel = Math100.accel(velocities[i], velocities[i + 1], arclength);
             // Apply the decel constraint at the end of the segment since it is feasible.
-            double maxDecel = maxDecel(samples[i + 1], velocities[i + 1]);
-            if (impliedAccel < maxDecel - EPSILON) {
+            double maxDecel = maxDecel(samples[i], velocities[i + 1]);
+            if (impliedAccel < maxDecel/* - EPSILON */) {
                 velocities[i] = Math100.v0(velocities[i + 1], maxDecel, arclength);
+                if (DEBUG)
+                    System.out.printf("adjust vi %f\n", velocities[i]);
             }
+            if (Math.abs(maxVelocity) < velocities[i]) {
+                velocities[i] = Math.signum(velocities[i]) * maxVelocity;
+                if (DEBUG)
+                    System.out.println("fix v one more time");
+            }
+
+            if (DEBUG)
+                System.out.printf("BACK i %d vi %f vi+1 %f max %f implied %f\n",
+                        i, velocities[i], velocities[i + 1], maxDecel, impliedAccel);
         }
     }
 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
index e61a9f24..0a1bc525 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
@@ -20,8 +20,15 @@
  * 
  * The new samples are used to reschedule a new trajectory which may have a
  * different duration.
+ * 
+ * This does the right thing when acceleration is constant: samples are further
+ * apart when moving fast.
+ * 
+ * But when acceleration is rising, this tends to oversample (because sampling
+ * assumes the lower acceleration will continue for longer).
  */
 public class TrajectoryRecycler {
+    private static final boolean DEBUG = false;
 
     public static Trajectory100 recycle(Trajectory100 original, double dt) {
         double start_vel = original.sample(0).velocityM_S();
@@ -30,11 +37,13 @@ public static Trajectory100 recycle(Trajectory100 original, double dt) {
         List<PathPoint> points = new ArrayList<>();
         // a little past the end to make sure we get the last point.
         for (double t = 0; t < original.duration() + dt; t += dt) {
-            TimedState sample = original.sample(t);
+            TimedState sample = original.sample2(t);
             // these contain discretization errors
             // TODO: go back to the original spline for these
             PathPoint point = sample.point();
             points.add(point);
+            if (DEBUG)
+                System.out.printf("recycle t %f %s\n", t, point);
         }
         return new TrajectoryFactory(original.m_constraints)
                 .fromSamples(points.toArray(new PathPoint[0]), start_vel, end_vel);
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
index 69db4b64..4702edf4 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
@@ -81,21 +81,21 @@ void testTrajectoryStart() {
 
         stepTime();
         c.execute();
-        assertEquals(0.139, drive.m_setpoint.x(), DELTA);
+        assertEquals(0.171, drive.m_setpoint.x(), DELTA);
         assertEquals(0, drive.m_setpoint.y(), DELTA);
         assertEquals(0, drive.m_setpoint.theta(), DELTA);
 
         // more normal driving
         stepTime();
         c.execute();
-        assertEquals(0.179, drive.m_setpoint.x(), DELTA);
+        assertEquals(0.217, drive.m_setpoint.x(), DELTA);
         assertEquals(0, drive.m_setpoint.y(), DELTA);
         assertEquals(0, drive.m_setpoint.theta(), DELTA);
 
         // etc
         stepTime();
         c.execute();
-        assertEquals(0.221, drive.m_setpoint.x(), DELTA);
+        assertEquals(0.264, drive.m_setpoint.x(), DELTA);
         assertEquals(0, drive.m_setpoint.y(), DELTA);
         assertEquals(0, drive.m_setpoint.theta(), DELTA);
     }
diff --git a/lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java b/lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java
index 81e409e4..34bea4a7 100644
--- a/lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java
+++ b/lib/src/test/java/org/team100/lib/reference/se2/TrajectoryReferenceTest.java
@@ -51,7 +51,7 @@ void testSimple() {
             assertEquals(0, c.pose().getX(), DELTA);
             ControlSE2 n = r.next();
             assertEquals(0.033, n.velocity().x(), DELTA);
-            assertEquals(0, n.pose().getX(), DELTA);
+            assertEquals(0.004, n.pose().getX(), DELTA);
         }
         // no time step, nothing changes
         {
@@ -61,17 +61,17 @@ void testSimple() {
             ControlSE2 n = r.next();
             assertEquals(0.033, n.velocity().x(), DELTA);
             // x is very small but not zero
-            assertEquals(0.0003266, n.pose().getX(), 0.0000001);
+            assertEquals(0.004, n.pose().getX(), DELTA);
         }
         // stepping time gets the next references
         stepTime();
         {
             ModelSE2 c = r.current();
             assertEquals(0.033, c.velocity().x(), DELTA);
-            assertEquals(0, c.pose().getX(), DELTA);
+            assertEquals(0.004, c.pose().getX(), DELTA);
             ControlSE2 n = r.next();
             assertEquals(0.065, n.velocity().x(), DELTA);
-            assertEquals(0.001, n.pose().getX(), DELTA);
+            assertEquals(0.009, n.pose().getX(), DELTA);
         }
         // way in the future, we're at the end.
         for (int i = 0; i < 500; ++i) {
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java
index 23aaf6bf..56c3f854 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java
@@ -64,7 +64,7 @@ void testSimple() throws IOException {
         c.execute();
         assertFalse(c.isDone());
         // the trajectory takes a little over 3s
-        for (double t = 0; t < 3.2; t += TimedRobot100.LOOP_PERIOD_S) {
+        for (double t = 0; t < 4; t += TimedRobot100.LOOP_PERIOD_S) {
             stepTime();
             c.execute();
             fixture.drive.periodic(); // for updateOdometry
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java
index 96d6fc2f..4de5c9fc 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java
@@ -90,21 +90,21 @@ void testTrajectoryStart() {
 
         stepTime();
         c.execute();
-        assertEquals(0.102, d.m_setpoint.x(), DELTA);
+        assertEquals(0.125, d.m_setpoint.x(), DELTA);
         assertEquals(0, d.m_setpoint.y(), DELTA);
         assertEquals(0, d.m_setpoint.theta(), DELTA);
 
         // more normal driving
         stepTime();
         c.execute();
-        assertEquals(0.139, d.m_setpoint.x(), DELTA);
+        assertEquals(0.171, d.m_setpoint.x(), DELTA);
         assertEquals(0, d.m_setpoint.y(), DELTA);
         assertEquals(0, d.m_setpoint.theta(), DELTA);
 
         // etc
         stepTime();
         c.execute();
-        assertEquals(0.179, d.m_setpoint.x(), DELTA);
+        assertEquals(0.217, d.m_setpoint.x(), DELTA);
         assertEquals(0, d.m_setpoint.y(), DELTA);
         assertEquals(0, d.m_setpoint.theta(), DELTA);
     }
@@ -219,7 +219,7 @@ void testRealDrive() throws IOException {
         // etc
         stepTime();
         command.execute();
-        assertEquals(0.064, collection.states().frontLeft().speedMetersPerSecond(), DELTA);
+        assertEquals(0.077, collection.states().frontLeft().speedMetersPerSecond(), DELTA);
         assertEquals(0, collection.states().frontLeft().angle().get().getRadians(), DELTA);
     }
 
diff --git a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
index 21ba9e62..d70e8201 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/Trajectory100Test.java
@@ -92,7 +92,7 @@ void testSample() {
         TimedState sample = trajectory.sample(0);
         assertEquals(0, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
         sample = trajectory.sample(1);
-        assertEquals(0.828, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
+        assertEquals(0.827, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
         sample = trajectory.sample(2);
         assertEquals(1, sample.point().waypoint().pose().getTranslation().getX(), DELTA);
     }
@@ -132,20 +132,20 @@ void testSampleThoroughly() {
 
         assertEquals(1.415, trajectory.duration(), DELTA);
         check(trajectory, 0.0, 0.000);
-        check(trajectory, 0.1, 0.010);
-        check(trajectory, 0.2, 0.040);
-        check(trajectory, 0.3, 0.090);
-        check(trajectory, 0.4, 0.160);
+        check(trajectory, 0.1, 0.025);
+        check(trajectory, 0.2, 0.050);
+        check(trajectory, 0.3, 0.093);
+        check(trajectory, 0.4, 0.161);
         check(trajectory, 0.5, 0.250);
         check(trajectory, 0.6, 0.360);
         check(trajectory, 0.7, 0.490);
         check(trajectory, 0.8, 0.622);
         check(trajectory, 0.9, 0.734);
-        check(trajectory, 1.0, 0.828);
-        check(trajectory, 1.1, 0.901);
-        check(trajectory, 1.2, 0.953);
-        check(trajectory, 1.3, 0.987);
-        check(trajectory, 1.4, 1.000);
+        check(trajectory, 1.0, 0.827);
+        check(trajectory, 1.1, 0.898);
+        check(trajectory, 1.2, 0.946);
+        check(trajectory, 1.3, 0.971);
+        check(trajectory, 1.4, 0.996);
         check(trajectory, 1.5, 1.000);
     }
 
diff --git a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
index eb997df8..d7d4e7e1 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/TrajectoryPlannerTest.java
@@ -247,7 +247,7 @@ void test2d() {
                 0);
         if (DEBUG)
             traj.dump();
-        assertEquals(2.741, traj.duration(), DELTA);
+        assertEquals(2.705, traj.duration(), DELTA);
     }
 
     /**
@@ -281,7 +281,7 @@ void test2d2() {
         Trajectory100 traj = planner.generateTrajectory(waypoints, 1, 1);
         if (DEBUG)
             traj.dump();
-        assertEquals(4.603, traj.duration(), DELTA);
+        assertEquals(4.562, traj.duration(), DELTA);
     }
 
     /**
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
index 75195f64..22d95d9c 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
@@ -64,14 +64,15 @@ void testBoth() {
         // note that the first accel (10) matches the max soft accel (10)
         List<TimingConstraint> constraints = List.of(
                 new ConstantConstraint(logger, 10, 10),
-                new SoftRegionConstraint(w0.pose().getTranslation(), 0.1, 0.5, 10),
-                new SoftRegionConstraint(w1.pose().getTranslation(), 0.1, 0.5, 10));
+                new SoftRegionConstraint(w0.pose().getTranslation(), 0.05, 0.5, 10),
+                new SoftRegionConstraint(w1.pose().getTranslation(), 0.05, 0.5, 10));
         TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
-        // note finer path than usual in order to see the detail
-        PathFactory pathFactory = new PathFactory(0.01, 0.01, 0.01, 0.1);
+        PathFactory pathFactory = new PathFactory(0.04, 0.1, 0.1, 0.1);
         TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
-        Trajectory100 trajectory = planner.restToRest(waypoints);
-        trajectory.tdump();
+        Trajectory100 original = planner.restToRest(waypoints);
+        original.dump();
+        Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.04);
+        recycled.dump();
     }
 
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
index aa9ed599..68fb54e7 100755
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TimedStateTest.java
@@ -40,6 +40,8 @@ void test() {
         assertEquals(start_state.acceleration(), intermediate_state.acceleration(), EPSILON);
         assertEquals(0.5, intermediate_state.velocityM_S(), EPSILON);
         // close to the start state by distance
-        assertEquals(0.125, intermediate_state.point().waypoint().pose().getTranslation().getX(), EPSILON);
+        // TODO: this is now wrong but I'm going to get rid of it anyway.
+        // assertEquals(0.125, intermediate_state.point().waypoint().pose().getTranslation().getX(), EPSILON);
+        assertEquals(0.25, intermediate_state.point().waypoint().pose().getTranslation().getX(), EPSILON);
     }
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
index d90e0b60..cff0a52d 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
@@ -1,7 +1,5 @@
 package org.team100.lib.trajectory.timing;
 
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
@@ -12,7 +10,6 @@
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.TrajectoryPlanner;
-import org.team100.lib.trajectory.examples.TrajectoryExamples;
 import org.team100.lib.trajectory.path.PathFactory;
 
 import com.ctre.phoenix6.Utils;
@@ -22,6 +19,7 @@
 
 public class TrajectoryRecyclerTest {
     LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger());
+    private static final boolean DEBUG = false;
 
     @Test
     void test0() {
@@ -39,9 +37,11 @@ void test0() {
                 new DirectionSE2(1, 0, 0), 1);
         List<WaypointSE2> waypoints = List.of(w0, w1);
         Trajectory100 original = planner.restToRest(waypoints);
-        original.dump();
+        if (DEBUG)
+            original.dump();
         Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.1);
-        recycled.dump();
+        if (DEBUG)
+            recycled.dump();
     }
 
     /**
@@ -52,24 +52,28 @@ void test0() {
     void test2d2() {
         // this forces the ctre output to the top
         Utils.isSimulation();
+        // tighter corner to make the constraint more difficult
         List<WaypointSE2> waypoints = List.of(
                 new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
-                new WaypointSE2(new Pose2d(1, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
-                new WaypointSE2(new Pose2d(2, 1, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3),
-                new WaypointSE2(new Pose2d(2, 2, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3));
+                new WaypointSE2(new Pose2d(2.5, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
+                new WaypointSE2(new Pose2d(3, 0.5, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3),
+                new WaypointSE2(new Pose2d(3, 3, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3));
 
+        // tippier to make it more obvious
         List<TimingConstraint> constraints = List.of(
-                new ConstantConstraint(log, 1, 1),
-                new CapsizeAccelerationConstraint(log, 0.5, 1));
+                new ConstantConstraint(log, 2, 1),
+                new CapsizeAccelerationConstraint(log, 0.4, 1));
         TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
         // coarser than before
-        PathFactory pathFactory = new PathFactory(0.5, 0.2, 0.2, 1);
+        PathFactory pathFactory = new PathFactory(0.4, 0.2, 0.2, 1);
         TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
 
         Trajectory100 original = planner.generateTrajectory(waypoints, 1, 1);
-        original.dump();
+        if (DEBUG)
+            original.dump();
         Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.1);
-        recycled.dump();
+        if (DEBUG)
+            recycled.dump();
 
     }
 

From 0719cfc25fb7f67f16695904bf8dec8ac80aacbd Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sun, 28 Dec 2025 19:13:18 -0800
Subject: [PATCH 4/5] remove jerk limiting from trajectories, it's too
 complicated

---
 .../frc2025/indicator/LEDIndicator.java       |  1 -
 .../org/team100/lib/geometry/PathPoint.java   |  3 +-
 .../swerve/commands/manual/DriveManually.java |  6 --
 .../team100/lib/trajectory/Trajectory100.java | 27 -------
 .../lib/trajectory/path/spline/SplineR1.java  |  2 +-
 .../timing/SoftRegionConstraint.java          | 66 ---------------
 .../lib/trajectory/timing/TimedState.java     | 29 -------
 .../trajectory/timing/TimingConstraint.java   |  9 ++-
 .../trajectory/timing/TrajectoryRecycler.java | 53 ------------
 .../se2/FeedforwardControllerSE2Test.java     |  1 -
 .../se2/FullStateControllerR3Test.java        |  2 -
 .../se2/ReferenceControllerSE2Test.java       |  3 -
 .../lib/geometry/GeometryUtilTest.java        |  5 --
 .../lib/profile/se2/HolonomicProfileTest.java |  2 -
 .../lib/subsystems/se2/MockSubsystemSE2.java  |  1 -
 .../commands/DriveToPoseWithProfileTest.java  |  1 -
 .../DriveWithTrajectoryFunctionTest.java      |  1 -
 .../se2/commands/PushbroomTest.java           |  1 -
 .../DriveWithTrajectoryListFunctionTest.java  |  1 -
 .../test/DriveWithTrajectoryTest.java         |  1 -
 .../kinodynamics/limiter/SwerveUtilTest.java  |  5 --
 .../trajectory/path/spline/SplineR1Test.java  |  4 -
 .../timing/SoftRegionConstraintTest.java      | 78 ------------------
 .../timing/TrajectoryRecyclerTest.java        | 80 -------------------
 24 files changed, 7 insertions(+), 375 deletions(-)
 delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
 delete mode 100644 lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
 delete mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java

diff --git a/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java b/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java
index 333df9c9..279a375e 100644
--- a/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java
+++ b/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java
@@ -3,7 +3,6 @@
 import org.team100.frc2025.Climber.ClimberIntake;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.coherence.Takt;
-import org.team100.lib.localization.AprilTagRobotLocalizer;
 import org.team100.lib.localization.NudgingVisionUpdater;
 
 import edu.wpi.first.wpilibj.AddressableLED;
diff --git a/lib/src/main/java/org/team100/lib/geometry/PathPoint.java b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
index 4cb30d51..3c379d1a 100644
--- a/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
+++ b/lib/src/main/java/org/team100/lib/geometry/PathPoint.java
@@ -117,8 +117,7 @@ public PathPoint interpolate(PathPoint other, double x) {
         if (spline != null)
             return spline.getPathPoint(s);
         // TODO: remove this way
-        if (DEBUG)
-            System.out.println("no spline");
+        System.out.println("WARNING: no spline, using linear interpolation ");
         return new PathPoint(
                 GeometryUtil.interpolate(m_waypoint, other.m_waypoint, x),
                 spline,
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java
index f8f172d3..c298ae29 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/DriveManually.java
@@ -101,12 +101,6 @@ public void execute() {
 
     }
 
-    @Override
-    public void end(boolean interrupted) {
-        // this can interfere with semi-auton commands, creating a "jerk" at engagement.
-        // m_drive.stop();
-    }
-
     /**
      * Override the driver mode.
      * 
diff --git a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
index cade34d6..7efab875 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/Trajectory100.java
@@ -65,34 +65,7 @@ public TimedState sample(double timeS) {
         throw new IllegalStateException("impossible trajectory: " + toString());
     }
 
-    /** Uses non-constant acceleration, for resampling */
-    public TimedState sample2(double timeS) {
-        // This scans the whole trajectory for every sample, but most of the time
-        // is the interpolation; I tried a TreeMap index and it only saved a few
-        // nanoseconds per call.
-        if (isEmpty())
-            throw new IllegalStateException("can't sample an empty trajectory");
-        if (timeS >= m_duration) {
-            return getLastPoint();
-        }
-        if (timeS <= 0) {
-            return getPoint(0);
-        }
 
-        for (int i = 1; i < length(); ++i) {
-            final TimedState ceil = getPoint(i);
-            if (ceil.getTimeS() >= timeS) {
-                final TimedState floor = getPoint(i - 1);
-                double span = ceil.getTimeS() - floor.getTimeS();
-                if (Math.abs(span) <= 1e-12) {
-                    return ceil;
-                }
-                double delta_t = timeS - floor.getTimeS();
-                return floor.interpolate2(ceil, delta_t);
-            }
-        }
-        throw new IllegalStateException("impossible trajectory: " + toString());
-    }
 
     /** Time is at or beyond the trajectory duration. */
     public boolean isDone(double timeS) {
diff --git a/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java b/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java
index 51640939..eeaf5d3f 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/path/spline/SplineR1.java
@@ -11,7 +11,7 @@
 /**
  * One-dimensional quintic spline, representing five derivatives of position.
  * 
- * The "t" parameter here is not time, its just a parameter.
+ * The "s" parameter here is not time, its just a parameter.
  */
 public class SplineR1 {
     /** crackle */
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
deleted file mode 100644
index 8568aad6..00000000
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/SoftRegionConstraint.java
+++ /dev/null
@@ -1,66 +0,0 @@
-package org.team100.lib.trajectory.timing;
-
-import org.team100.lib.geometry.PathPoint;
-import org.team100.lib.util.Math100;
-
-import edu.wpi.first.math.geometry.Translation2d;
-
-/**
- * Limits acceleration near a point.
- * 
- * There's a minimum at the point, and a maximum at the edge of a circle
- * surrounding it, interpolated using the cube root of the distance.  This
- * provides roughly constant jerk (i.e. constant growth rate of acceleration).
- * 
- * To increase resolution of the slow part, you should use the TrajectoryRecycler.
- * 
- * https://docs.google.com/spreadsheets/d/1sbB-zTBUjRRlWHaWXe-V1ZDhAZCFwItVVO1x3LmZ4B4/edit?gid=691127145#gid=691127145
- */
-public class SoftRegionConstraint implements TimingConstraint {
-    private final Translation2d m_center;
-    private final double m_radius;
-    private final double m_min;
-    private final double m_max;
-
-    /**
-     * @param center of the soft region
-     * @param radius of the soft region
-     * @param min    accel/decel at the center
-     * @param max    accel/decel at the edge
-     */
-    public SoftRegionConstraint(
-            Translation2d center, double radius, double min, double max) {
-        m_center = center;
-        m_radius = radius;
-        m_min = min;
-        m_max = max;
-    }
-
-    @Override
-    public double maxV(PathPoint state) {
-        return Double.POSITIVE_INFINITY;
-    }
-
-    @Override
-    public double maxAccel(PathPoint state, double velocityM_S) {
-        double s = near(state);
-        if (s < 1)
-            return Math100.interpolate(m_min, m_max, Math.cbrt(s));
-        return Double.POSITIVE_INFINITY;
-    }
-
-    @Override
-    public double maxDecel(PathPoint state, double velocityM_S) {
-        double s = near(state);
-        if (s < 1)
-            return -1.0 * Math100.interpolate(m_min, m_max, Math.cbrt(s));
-        return Double.NEGATIVE_INFINITY;
-    }
-
-    /** distance to the center compared with the radius */
-    private double near(PathPoint state) {
-        Translation2d translation = state.waypoint().pose().getTranslation();
-        return translation.getDistance(m_center) / m_radius;
-    }
-
-}
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
index bd5642fb..cc946ad9 100644
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimedState.java
@@ -98,35 +98,6 @@ public TimedState interpolate(TimedState other, double delta_t) {
                 m_accelM_S_S);
     }
 
-    /** Tries to take variation in acceleration into account. For resampling. */
-    public TimedState interpolate2(TimedState other, double delta_t) {
-        if (delta_t < 0)
-            throw new IllegalArgumentException("delta_t must be non-negative");
-        if (DEBUG)
-            System.out.println("lerp");
-        double tLerp = m_timeS + delta_t;
-        double vLerp = m_velocityM_S + m_accelM_S_S * delta_t;
-        double dt = other.m_timeS - m_timeS;
-        double j = (other.m_accelM_S_S - m_accelM_S_S) / dt;
-        double pathwiseDistance = m_velocityM_S * delta_t + 0.5 * m_accelM_S_S * delta_t * delta_t
-                + 0.1666 * j * delta_t * delta_t * delta_t;
-
-        double distanceBetween = m_velocityM_S * dt + 0.5 * m_accelM_S_S * dt * dt + 0.1666 * j * dt * dt * dt;
-        double interpolant = pathwiseDistance / distanceBetween;
-        if (Double.isNaN(interpolant)) {
-            interpolant = 1.0;
-        }
-
-        if (DEBUG)
-            System.out.printf("tlerp %f\n", tLerp);
-        return new TimedState(
-                // m_point.interpolate(other.m_point, interpolant),
-                m_point.interpolate(other.m_point, interpolant),
-                tLerp,
-                vLerp,
-                m_accelM_S_S);
-    }
-
     /** Translation only, ignores rotation */
     public double distanceCartesian(TimedState other) {
         return m_point.distanceCartesian(other.m_point);
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java
index 16007f26..9d78832a 100755
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java
+++ b/lib/src/main/java/org/team100/lib/trajectory/timing/TimingConstraint.java
@@ -8,10 +8,11 @@
  * tippiness, wheel slip, etc. Different maneuvers may want different
  * constraints, e.g. some should be slow and precise, others fast and risky.
  * 
- * Note that this interface doesn't support jerk limiting in a simple way.
- * If you want to limit jerk at the start and/or end of a trajectory,
- * you can implement that using the acceleration and deceleration limits,
- * with a constraint that is aware of the endpoint locations.
+ * Note that this interface doesn't support jerk limiting.
+ * 
+ * I've gone back and forth om supporting jerk limiting, and for now I took it
+ * out. It's complicated, we don't seem to need it, and mechanism slack creates
+ * jerk even if the motor tries not to.
  */
 public interface TimingConstraint {
     /**
diff --git a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java b/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
deleted file mode 100644
index 0a1bc525..00000000
--- a/lib/src/main/java/org/team100/lib/trajectory/timing/TrajectoryRecycler.java
+++ /dev/null
@@ -1,53 +0,0 @@
-package org.team100.lib.trajectory.timing;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import org.team100.lib.geometry.PathPoint;
-import org.team100.lib.trajectory.Trajectory100;
-
-/**
- * Resamples a trajectory with constant time between samples.
- * 
- * The idea is to prevent overworking the fast parts (where few samples are
- * required) and underworking the slow parts (where more samples are required),
- * as the spatial sampling tends to do. This allows the overall
- * trajectory-making process to be faster, but have higher resolution where it
- * matters.
- * 
- * The sampling goes all the way back to the source spline, to avoid
- * interpolation error, so the trajectory needs to include it.
- * 
- * The new samples are used to reschedule a new trajectory which may have a
- * different duration.
- * 
- * This does the right thing when acceleration is constant: samples are further
- * apart when moving fast.
- * 
- * But when acceleration is rising, this tends to oversample (because sampling
- * assumes the lower acceleration will continue for longer).
- */
-public class TrajectoryRecycler {
-    private static final boolean DEBUG = false;
-
-    public static Trajectory100 recycle(Trajectory100 original, double dt) {
-        double start_vel = original.sample(0).velocityM_S();
-        double end_vel = original.getLastPoint().velocityM_S();
-
-        List<PathPoint> points = new ArrayList<>();
-        // a little past the end to make sure we get the last point.
-        for (double t = 0; t < original.duration() + dt; t += dt) {
-            TimedState sample = original.sample2(t);
-            // these contain discretization errors
-            // TODO: go back to the original spline for these
-            PathPoint point = sample.point();
-            points.add(point);
-            if (DEBUG)
-                System.out.printf("recycle t %f %s\n", t, point);
-        }
-        return new TrajectoryFactory(original.m_constraints)
-                .fromSamples(points.toArray(new PathPoint[0]), start_vel, end_vel);
-
-    }
-
-}
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java b/lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java
index 18ccb0f8..48e7c208 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/FeedforwardControllerSE2Test.java
@@ -5,7 +5,6 @@
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.controller.se2.FeedforwardControllerSE2;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java b/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java
index 331c90ac..59dc1aaf 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/FullStateControllerR3Test.java
@@ -5,8 +5,6 @@
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.controller.se2.ControllerFactorySE2;
-import org.team100.lib.controller.se2.FullStateControllerSE2;
 import org.team100.lib.geometry.DeltaSE2;
 import org.team100.lib.geometry.PathPoint;
 import org.team100.lib.geometry.VelocitySE2;
diff --git a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
index 4702edf4..a21e8896 100644
--- a/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
+++ b/lib/src/test/java/org/team100/lib/controller/se2/ReferenceControllerSE2Test.java
@@ -9,9 +9,6 @@
 import java.util.List;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.controller.se2.ControllerFactorySE2;
-import org.team100.lib.controller.se2.ControllerSE2;
-import org.team100.lib.controller.se2.FullStateControllerSE2;
 import org.team100.lib.geometry.DirectionSE2;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.geometry.WaypointSE2;
diff --git a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java
index 1aa39ca1..d6af9470 100644
--- a/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java
+++ b/lib/src/test/java/org/team100/lib/geometry/GeometryUtilTest.java
@@ -5,23 +5,18 @@
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.trajectory.path.spline.HolonomicSpline;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Quaternion;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 
 class GeometryUtilTest {
-    private static final boolean DEBUG = false;
     private static final double DELTA = 0.001;
 
-   
-
     @Test
     void testProject() {
         {
diff --git a/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java b/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java
index ae7ee5af..23a33034 100644
--- a/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/profile/se2/HolonomicProfileTest.java
@@ -8,8 +8,6 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.se2.HolonomicProfile;
-import org.team100.lib.profile.se2.HolonomicProfileFactory;
 import org.team100.lib.state.ControlSE2;
 import org.team100.lib.state.ModelSE2;
 import org.team100.lib.testing.Timeless;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java b/lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java
index e1c2f4ee..5cbf00c3 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/MockSubsystemSE2.java
@@ -2,7 +2,6 @@
 
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.state.ModelSE2;
-import org.team100.lib.subsystems.se2.VelocitySubsystemSE2;
 
 public class MockSubsystemSE2 implements VelocitySubsystemSE2 {
     public VelocitySE2 m_setpoint;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java
index 84461f05..ebc01a6d 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveToPoseWithProfileTest.java
@@ -10,7 +10,6 @@
 import org.team100.lib.profile.se2.HolonomicProfileFactory;
 import org.team100.lib.state.ModelSE2;
 import org.team100.lib.subsystems.se2.MockSubsystemSE2;
-import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile;
 import org.team100.lib.testing.Timeless;
 
 import edu.wpi.first.math.geometry.Pose2d;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java
index 1146893f..e2281265 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/DriveWithTrajectoryFunctionTest.java
@@ -11,7 +11,6 @@
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
 import org.team100.lib.state.ModelSE2;
 import org.team100.lib.subsystems.se2.MockSubsystemSE2;
-import org.team100.lib.subsystems.se2.commands.DriveWithTrajectoryFunction;
 import org.team100.lib.testing.Timeless;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.trajectory.TrajectoryPlanner;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java
index 4e3684bd..fcb111b1 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/PushbroomTest.java
@@ -3,7 +3,6 @@
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import org.junit.jupiter.api.Test;
-import org.team100.lib.subsystems.se2.commands.Pushbroom;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java
index 56c3f854..a716e171 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryListFunctionTest.java
@@ -17,7 +17,6 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.subsystems.se2.commands.test.DriveWithTrajectoryListFunction;
 import org.team100.lib.subsystems.swerve.Fixture;
 import org.team100.lib.testing.Timeless;
 import org.team100.lib.trajectory.TrajectoryPlanner;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java
index 4de5c9fc..ab2c202c 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/se2/commands/test/DriveWithTrajectoryTest.java
@@ -24,7 +24,6 @@
 import org.team100.lib.sensor.gyro.SimulatedGyro;
 import org.team100.lib.state.ModelSE2;
 import org.team100.lib.subsystems.se2.MockSubsystemSE2;
-import org.team100.lib.subsystems.se2.commands.test.DriveWithTrajectory;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 import org.team100.lib.subsystems.swerve.SwerveLocal;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java
index 738acbce..e9a7c9bc 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveUtilTest.java
@@ -107,9 +107,4 @@ void simMinAccel() {
             v += dt * a;
         }
     }
-
-    @Test
-    void testJerkLimit() {
-
-    }
 }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java
index 02a9a365..7efd3452 100644
--- a/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java
+++ b/lib/src/test/java/org/team100/lib/trajectory/path/spline/SplineR1Test.java
@@ -28,10 +28,6 @@ void testEnds() {
     /** Look at an example */
     @Test
     void testSample() {
-        // an example from 0 to 1 with zero first and second derivatives at the ends.
-        // the jerk and snap of this spline is very high at the ends, so it
-        // is not useful without modifying the schedule.
-        // Spline1d spline = Spline1d.get(0, 1, 0, 0, 0, 0);
         SplineR1 spline = SplineR1.viaMatrix(0, 1, 0, 0, 0, 0);
         show(spline);
     }
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
deleted file mode 100644
index 22d95d9c..00000000
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/SoftRegionConstraintTest.java
+++ /dev/null
@@ -1,78 +0,0 @@
-package org.team100.lib.trajectory.timing;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-import org.team100.lib.geometry.DirectionSE2;
-import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.logging.TestLoggerFactory;
-import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.trajectory.Trajectory100;
-import org.team100.lib.trajectory.TrajectoryPlanner;
-import org.team100.lib.trajectory.path.PathFactory;
-
-import com.ctre.phoenix6.Utils;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-
-public class SoftRegionConstraintTest {
-    private static final double DELTA = 0.001;
-    private static final LoggerFactory logger = new TestLoggerFactory(new TestPrimitiveLogger());
-
-    @Test
-    void testStart() {
-        WaypointSE2 w0 = new WaypointSE2(
-                new Pose2d(
-                        new Translation2d(),
-                        new Rotation2d()),
-                new DirectionSE2(1, 0, 0), 1.2);
-        WaypointSE2 w1 = new WaypointSE2(
-                new Pose2d(
-                        new Translation2d(1, 0),
-                        new Rotation2d()),
-                new DirectionSE2(1, 0, 0), 1.2);
-        List<WaypointSE2> waypoints = List.of(w0, w1);
-        // note that the first accel (10) matches the max soft accel (10)
-        List<TimingConstraint> constraints = List.of(
-                new ConstantConstraint(logger, 10, 2),
-                new SoftRegionConstraint(w0.pose().getTranslation(), 0.1, 0.5, 10));
-        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
-        PathFactory pathFactory = new PathFactory();
-        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
-        Trajectory100 trajectory = planner.restToRest(waypoints);
-        trajectory.dump();
-    }
-
-    @Test
-    void testBoth() {
-        // this forces the ctre output to the top
-        Utils.isSimulation();
-        WaypointSE2 w0 = new WaypointSE2(
-                new Pose2d(
-                        new Translation2d(),
-                        new Rotation2d()),
-                new DirectionSE2(1, 0, 0), 1.2);
-        WaypointSE2 w1 = new WaypointSE2(
-                new Pose2d(
-                        new Translation2d(1, 0),
-                        new Rotation2d()),
-                new DirectionSE2(1, 0, 0), 1.2);
-        List<WaypointSE2> waypoints = List.of(w0, w1);
-        // note that the first accel (10) matches the max soft accel (10)
-        List<TimingConstraint> constraints = List.of(
-                new ConstantConstraint(logger, 10, 10),
-                new SoftRegionConstraint(w0.pose().getTranslation(), 0.05, 0.5, 10),
-                new SoftRegionConstraint(w1.pose().getTranslation(), 0.05, 0.5, 10));
-        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
-        PathFactory pathFactory = new PathFactory(0.04, 0.1, 0.1, 0.1);
-        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
-        Trajectory100 original = planner.restToRest(waypoints);
-        original.dump();
-        Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.04);
-        recycled.dump();
-    }
-
-}
diff --git a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java b/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
deleted file mode 100644
index cff0a52d..00000000
--- a/lib/src/test/java/org/team100/lib/trajectory/timing/TrajectoryRecyclerTest.java
+++ /dev/null
@@ -1,80 +0,0 @@
-package org.team100.lib.trajectory.timing;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-import org.team100.lib.geometry.DirectionSE2;
-import org.team100.lib.geometry.WaypointSE2;
-import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.logging.TestLoggerFactory;
-import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.trajectory.Trajectory100;
-import org.team100.lib.trajectory.TrajectoryPlanner;
-import org.team100.lib.trajectory.path.PathFactory;
-
-import com.ctre.phoenix6.Utils;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-
-public class TrajectoryRecyclerTest {
-    LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger());
-    private static final boolean DEBUG = false;
-
-    @Test
-    void test0() {
-        // this forces the ctre output to the top
-        Utils.isSimulation();
-        List<TimingConstraint> constraints = List.of(new ConstantConstraint(log, 1, 1));
-        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
-        PathFactory pathFactory = new PathFactory();
-        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
-        WaypointSE2 w0 = new WaypointSE2(
-                new Pose2d(0, 0, new Rotation2d()),
-                new DirectionSE2(1, 0, 0), 1);
-        WaypointSE2 w1 = new WaypointSE2(
-                new Pose2d(1, 0, new Rotation2d()),
-                new DirectionSE2(1, 0, 0), 1);
-        List<WaypointSE2> waypoints = List.of(w0, w1);
-        Trajectory100 original = planner.restToRest(waypoints);
-        if (DEBUG)
-            original.dump();
-        Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.1);
-        if (DEBUG)
-            recycled.dump();
-    }
-
-    /**
-     * see TrajectoryPlannerTest.test2d2()
-     * Straight, curve, straight.
-     */
-    @Test
-    void test2d2() {
-        // this forces the ctre output to the top
-        Utils.isSimulation();
-        // tighter corner to make the constraint more difficult
-        List<WaypointSE2> waypoints = List.of(
-                new WaypointSE2(new Pose2d(0, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
-                new WaypointSE2(new Pose2d(2.5, 0, new Rotation2d()), new DirectionSE2(1, 0, 0), 1.3),
-                new WaypointSE2(new Pose2d(3, 0.5, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3),
-                new WaypointSE2(new Pose2d(3, 3, new Rotation2d()), new DirectionSE2(0, 1, 0), 1.3));
-
-        // tippier to make it more obvious
-        List<TimingConstraint> constraints = List.of(
-                new ConstantConstraint(log, 2, 1),
-                new CapsizeAccelerationConstraint(log, 0.4, 1));
-        TrajectoryFactory trajectoryFactory = new TrajectoryFactory(constraints);
-        // coarser than before
-        PathFactory pathFactory = new PathFactory(0.4, 0.2, 0.2, 1);
-        TrajectoryPlanner planner = new TrajectoryPlanner(pathFactory, trajectoryFactory);
-
-        Trajectory100 original = planner.generateTrajectory(waypoints, 1, 1);
-        if (DEBUG)
-            original.dump();
-        Trajectory100 recycled = TrajectoryRecycler.recycle(original, 0.1);
-        if (DEBUG)
-            recycled.dump();
-
-    }
-
-}

From 17dafb1e2366c5fc2df7e14acc51fed01e2b9e8b Mon Sep 17 00:00:00 2001
From: Joel Truher <joel@truher.org>
Date: Sun, 28 Dec 2025 19:56:26 -0800
Subject: [PATCH 5/5] remove timed profiles; we never use them.

---
 .../frc2025/CalgamesArm/MechProfiles.java     |   4 +-
 .../org/team100/frc2025/Climber/Climber.java  |   4 +-
 .../frc2025/Swerve/ManualWithBargeAssist.java |   2 +-
 .../Swerve/ManualWithProfiledReefLock.java    |   2 +-
 .../frc2025/CalgamesArm/ProfiledTest.java     |   4 +-
 .../OutboardRotaryPositionSubsystem.java      |   4 +-
 .../motion/RotaryPositionSubsystem1d.java     |  12 +-
 .../java/org/team100/lib/profile/README.md    |  14 +-
 .../{incremental => r1}/CompleteProfile.java  |   2 +-
 .../CurrentLimitedExponentialProfile.java     |   2 +-
 .../ExponentialProfileWPI.java                |   2 +-
 .../IncrementalProfile.java                   |   2 +-
 .../{incremental => r1}/NullProfile.java      |   2 +-
 .../lib/profile/{incremental => r1}/README.md |   5 +-
 .../TrapezoidIncrementalProfile.java          |   2 +-
 .../TrapezoidProfileWPI.java                  |   2 +-
 .../roadrunner/AccelerationConstraint.java    |  12 -
 .../roadrunner/DynamicProfileGenerator.java   | 312 ------------------
 .../JerkLimitedProfileGenerator.java          | 221 -------------
 .../lib/profile/roadrunner/MotionProfile.java | 103 ------
 .../roadrunner/MotionProfileBuilder.java      |  78 -----
 .../lib/profile/roadrunner/MotionSegment.java |  46 ---
 .../lib/profile/roadrunner/MotionSpan.java    |  10 -
 .../lib/profile/roadrunner/MotionState.java   |  36 --
 .../team100/lib/profile/roadrunner/README.md  |   4 -
 .../roadrunner/TrapezoidProfileGenerator.java | 132 --------
 .../roadrunner/VelocityConstraint.java        |  13 -
 .../lib/profile/se2/FreeRotationProfile.java  |   2 +-
 .../lib/profile/se2/HolonomicProfile.java     |   2 +-
 .../profile/se2/HolonomicProfileFactory.java  |   6 +-
 .../timed/JerkLimitedTimedProfile.java        |  60 ----
 .../org/team100/lib/profile/timed/README.md   |   7 -
 .../lib/profile/timed/TaperedProfile.java     |  69 ----
 .../lib/profile/timed/TimedProfile.java       |  22 --
 .../r1/IncrementalProfileReferenceR1.java     |   2 +-
 .../reference/r1/TimedProfileReferenceR1.java |  69 ----
 .../lib/subsystems/five_bar/FiveBarServo.java |   4 +-
 .../subsystems/five_bar/commands/Move.java    |  18 +-
 .../lynxmotion_arm/MoveCommand.java           |  17 +-
 .../lynxmotion_arm/MoveCommandTwoDof.java     |  18 +-
 .../lib/subsystems/lynxmotion_arm/MoveXY.java |  18 +-
 .../lib/subsystems/lynxmotion_arm/MoveZ.java  |  17 +-
 .../prr/commands/FollowJointProfiles.java     |   2 +-
 .../subsystems/shooter/IndexerSubsystem.java  |   2 +-
 .../manual/ManualWithOptionalTargetLock.java  |   4 +-
 .../manual/ManualWithProfiledHeading.java     |   2 +-
 .../commands/manual/ManualWithTargetLock.java |   4 +-
 .../kinodynamics/SwerveKinodynamics.java      |   4 +-
 .../module/SimulatedSwerveModule100.java      |   2 +-
 .../swerve/module/WCPSwerveModule100.java     |   2 +-
 .../team100/lib/subsystems/turret/Turret.java |   4 +-
 .../optimization/PoseInterpolationTest.java   | 111 -------
 .../CompleteProfileTest.java                  |   2 +-
 .../CoordinatedProfileTest.java               |   2 +-
 .../CurrentLimitedExponentialProfileTest.java |   2 +-
 .../ExponentialProfileWPITest.java            |   2 +-
 .../IncrementalProfileTest.java               |   2 +-
 .../MockIncrementalProfile.java               |   2 +-
 .../TrapezoidIncrementalProfileTest.java      |   2 +-
 .../TrapezoidProfileWPITest.java              |   2 +-
 .../DynamicProfileGeneratorTest.java          | 129 --------
 .../JerkLimitedProfileGeneratorTest.java      | 131 --------
 .../roadrunner/MotionProfileBuilderTest.java  |  35 --
 .../MotionProfilePerformanceTest.java         |  32 --
 .../profile/roadrunner/MotionProfileTest.java |  45 ---
 .../profile/roadrunner/MotionSegmentTest.java |  23 --
 .../profile/roadrunner/MotionStateTest.java   |  39 ---
 .../TrapezoidProfileGeneratorTest.java        |  28 --
 .../timed/JerkLimitedTimedProfileTest.java    |  21 --
 .../lib/profile/timed/TaperedProfileTest.java |  61 ----
 .../r1/IncrementalProfileReferenceR1Test.java |   2 +-
 .../r1/TimedProfileReference1dTest.java       |  57 ----
 .../servo/AnglePositionServoProfileTest.java  |   4 +-
 .../lib/servo/AngularPositionProfileTest.java |   6 +-
 .../team100/lib/servo/GravityServoTest.java   |   4 +-
 .../OnboardAngularPositionServoTest.java      |   4 +-
 ...boardLinearDutyCyclePositionServoTest.java |   4 +-
 .../OutboardAngularPositionServoTest.java     |   4 +-
 .../ManualWithFullStateHeadingTest.java       |   4 +-
 .../manual/ManualWithProfiledHeadingTest.java |   2 +-
 .../limiter/SwerveLimiterTest.java            |   4 +-
 81 files changed, 139 insertions(+), 2016 deletions(-)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/CompleteProfile.java (99%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/CurrentLimitedExponentialProfile.java (98%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/ExponentialProfileWPI.java (97%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/IncrementalProfile.java (98%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/NullProfile.java (90%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/README.md (88%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/TrapezoidIncrementalProfile.java (99%)
 rename lib/src/main/java/org/team100/lib/profile/{incremental => r1}/TrapezoidProfileWPI.java (97%)
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/AccelerationConstraint.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/DynamicProfileGenerator.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGenerator.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfile.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfileBuilder.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSegment.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSpan.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/MotionState.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/README.md
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGenerator.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/roadrunner/VelocityConstraint.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/timed/JerkLimitedTimedProfile.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/timed/README.md
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/timed/TaperedProfile.java
 delete mode 100644 lib/src/main/java/org/team100/lib/profile/timed/TimedProfile.java
 delete mode 100644 lib/src/main/java/org/team100/lib/reference/r1/TimedProfileReferenceR1.java
 delete mode 100644 lib/src/test/java/org/team100/lib/optimization/PoseInterpolationTest.java
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/CompleteProfileTest.java (99%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/CoordinatedProfileTest.java (99%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/CurrentLimitedExponentialProfileTest.java (98%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/ExponentialProfileWPITest.java (97%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/IncrementalProfileTest.java (98%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/MockIncrementalProfile.java (93%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/TrapezoidIncrementalProfileTest.java (99%)
 rename lib/src/test/java/org/team100/lib/profile/{incremental => r1}/TrapezoidProfileWPITest.java (98%)
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/DynamicProfileGeneratorTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGeneratorTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileBuilderTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfilePerformanceTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/MotionSegmentTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/MotionStateTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGeneratorTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/timed/JerkLimitedTimedProfileTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/profile/timed/TaperedProfileTest.java
 delete mode 100644 lib/src/test/java/org/team100/lib/reference/r1/TimedProfileReference1dTest.java

diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechProfiles.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechProfiles.java
index de84c086..e34ea3f0 100644
--- a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechProfiles.java
+++ b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechProfiles.java
@@ -1,8 +1,8 @@
 package org.team100.frc2025.CalgamesArm;
 
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.incremental.CompleteProfile;
-import org.team100.lib.profile.incremental.CurrentLimitedExponentialProfile;
+import org.team100.lib.profile.r1.CompleteProfile;
+import org.team100.lib.profile.r1.CurrentLimitedExponentialProfile;
 import org.team100.lib.subsystems.prr.EAWConfig;
 import org.team100.lib.subsystems.prr.commands.FollowJointProfiles;
 
diff --git a/comp/src/main/java/org/team100/frc2025/Climber/Climber.java b/comp/src/main/java/org/team100/frc2025/Climber/Climber.java
index c56b1988..29b24276 100644
--- a/comp/src/main/java/org/team100/frc2025/Climber/Climber.java
+++ b/comp/src/main/java/org/team100/frc2025/Climber/Climber.java
@@ -12,8 +12,8 @@
 import org.team100.lib.motor.NeutralMode;
 import org.team100.lib.motor.ctre.Falcon6Motor;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.EncoderDrive;
diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java
index f061ff42..b5e61c11 100644
--- a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java
+++ b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java
@@ -11,7 +11,7 @@
 import org.team100.lib.logging.LoggerFactory.Control100Logger;
 import org.team100.lib.logging.LoggerFactory.DoubleLogger;
 import org.team100.lib.logging.LoggerFactory.StringLogger;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.state.ModelSE2;
diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java
index 5271345e..0bdfe3ba 100644
--- a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java
+++ b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java
@@ -12,7 +12,7 @@
 import org.team100.lib.logging.LoggerFactory.BooleanLogger;
 import org.team100.lib.logging.LoggerFactory.Control100Logger;
 import org.team100.lib.logging.LoggerFactory.DoubleLogger;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.state.ModelSE2;
diff --git a/comp/src/test/java/org/team100/frc2025/CalgamesArm/ProfiledTest.java b/comp/src/test/java/org/team100/frc2025/CalgamesArm/ProfiledTest.java
index d957032f..fa38a3d2 100644
--- a/comp/src/test/java/org/team100/frc2025/CalgamesArm/ProfiledTest.java
+++ b/comp/src/test/java/org/team100/frc2025/CalgamesArm/ProfiledTest.java
@@ -4,8 +4,8 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.subsystems.prr.EAWConfig;
diff --git a/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java b/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java
index 594bf023..59494dbd 100644
--- a/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java
+++ b/lib/src/main/java/org/team100/lib/examples/motion/OutboardRotaryPositionSubsystem.java
@@ -11,8 +11,8 @@
 import org.team100.lib.motor.rev.CANSparkMotor;
 import org.team100.lib.motor.rev.NeoCANSparkMotor;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
 import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder;
diff --git a/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java b/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java
index ff63b5d9..9e820a49 100644
--- a/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java
+++ b/lib/src/main/java/org/team100/lib/examples/motion/RotaryPositionSubsystem1d.java
@@ -11,9 +11,10 @@
 import org.team100.lib.motor.NeutralMode;
 import org.team100.lib.motor.ctre.Kraken6Motor;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
+import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
-import org.team100.lib.reference.r1.TimedProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.EncoderDrive;
 import org.team100.lib.sensor.position.absolute.RotaryPositionSensor;
 import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor;
@@ -71,9 +72,10 @@ public RotaryPositionSubsystem1d(LoggerFactory parent) {
 
         double maxVel = 40;
         double maxAccel = 40;
-        double maxJerk = 70;
-        JerkLimitedTimedProfile profile = new JerkLimitedTimedProfile(maxVel, maxAccel, maxJerk, true);
-        ProfileReferenceR1 ref = new TimedProfileReferenceR1(log, profile);
+        IncrementalProfile profile = new TrapezoidProfileWPI(maxVel, maxAccel);
+
+        ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, positionTolerance,
+                velocityTolerance);
 
         /*
          * Here we use the Team 100 "Identity" mechanism to allow different
diff --git a/lib/src/main/java/org/team100/lib/profile/README.md b/lib/src/main/java/org/team100/lib/profile/README.md
index 9fc22bdc..736a7633 100644
--- a/lib/src/main/java/org/team100/lib/profile/README.md
+++ b/lib/src/main/java/org/team100/lib/profile/README.md
@@ -4,20 +4,12 @@ This package supports "profiled" motion, which means movement from
 a specified starting state to an ending state, as fast as possible,
 within some constraints on velocity and acceleration.
 
-There are several subpackages:
+Our profiles have no state: you give one the current setpoint,
+and it produces the next one.
 
-* `timed` profiles are like a trajectories: you precalculate the schedule
-and then sample it.  The main interface is `TimedProfile`.
-
-* `incremental` profiles have no state: you give one the current setpoint,
-and it produces the next one.  The main interface is `IncrementalProfile`.
+* `r1` provides one-dimensional profiles.  The main interface is `IncrementalProfile`.
 
 * `se2` provides multi-dimensional profiled motion in the SE(2) manifold
 (x, y, theta), useful for navigation or planar motion.  The main
 interface is `ProfileSE2`.  
 
-* `roadrunner` is a direct translation of the RoadRunner Kotlin classes,
-which are used in the `timed` package.
-
-
-
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/CompleteProfile.java b/lib/src/main/java/org/team100/lib/profile/r1/CompleteProfile.java
similarity index 99%
rename from lib/src/main/java/org/team100/lib/profile/incremental/CompleteProfile.java
rename to lib/src/main/java/org/team100/lib/profile/r1/CompleteProfile.java
index 6dd15c82..22f283b6 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/CompleteProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/CompleteProfile.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.state.Control100;
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfile.java b/lib/src/main/java/org/team100/lib/profile/r1/CurrentLimitedExponentialProfile.java
similarity index 98%
rename from lib/src/main/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfile.java
rename to lib/src/main/java/org/team100/lib/profile/r1/CurrentLimitedExponentialProfile.java
index 0f334ec6..5d4f55b9 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/CurrentLimitedExponentialProfile.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/ExponentialProfileWPI.java b/lib/src/main/java/org/team100/lib/profile/r1/ExponentialProfileWPI.java
similarity index 97%
rename from lib/src/main/java/org/team100/lib/profile/incremental/ExponentialProfileWPI.java
rename to lib/src/main/java/org/team100/lib/profile/r1/ExponentialProfileWPI.java
index 383ae1f4..16a8ba17 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/ExponentialProfileWPI.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/ExponentialProfileWPI.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/IncrementalProfile.java b/lib/src/main/java/org/team100/lib/profile/r1/IncrementalProfile.java
similarity index 98%
rename from lib/src/main/java/org/team100/lib/profile/incremental/IncrementalProfile.java
rename to lib/src/main/java/org/team100/lib/profile/r1/IncrementalProfile.java
index 0beaeef3..e344315b 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/IncrementalProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/IncrementalProfile.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.optimization.Bisection1d;
 import org.team100.lib.state.Control100;
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/NullProfile.java b/lib/src/main/java/org/team100/lib/profile/r1/NullProfile.java
similarity index 90%
rename from lib/src/main/java/org/team100/lib/profile/incremental/NullProfile.java
rename to lib/src/main/java/org/team100/lib/profile/r1/NullProfile.java
index 170d7093..c365420a 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/NullProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/NullProfile.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/README.md b/lib/src/main/java/org/team100/lib/profile/r1/README.md
similarity index 88%
rename from lib/src/main/java/org/team100/lib/profile/incremental/README.md
rename to lib/src/main/java/org/team100/lib/profile/r1/README.md
index 21637712..e0ca348b 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/README.md
+++ b/lib/src/main/java/org/team100/lib/profile/r1/README.md
@@ -1,7 +1,6 @@
-# lib.profile.incremental
+# lib.profile.r1
 
-This package contains profiles which have
-no state: they just impose constraints at each time step.
+This package contains one-dimensional profiles.
 
 There are methods for scaling these profiles so they can be
 coordinated to complete in the same duration.
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfile.java b/lib/src/main/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfile.java
similarity index 99%
rename from lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfile.java
rename to lib/src/main/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfile.java
index 18ba956d..608757fa 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfile.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.state.Control100;
diff --git a/lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidProfileWPI.java b/lib/src/main/java/org/team100/lib/profile/r1/TrapezoidProfileWPI.java
similarity index 97%
rename from lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidProfileWPI.java
rename to lib/src/main/java/org/team100/lib/profile/r1/TrapezoidProfileWPI.java
index 9ee7c466..3e5abf73 100644
--- a/lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidProfileWPI.java
+++ b/lib/src/main/java/org/team100/lib/profile/r1/TrapezoidProfileWPI.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/AccelerationConstraint.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/AccelerationConstraint.java
deleted file mode 100644
index b7aae06d..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/AccelerationConstraint.java
+++ /dev/null
@@ -1,12 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-/**
- * Motion profile acceleration constraint.
- */
-@FunctionalInterface
-public interface AccelerationConstraint {
-    /**
-     * Returns the maximum profile acceleration at displacement [s].
-     */
-    double get(double s);
-}
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/DynamicProfileGenerator.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/DynamicProfileGenerator.java
deleted file mode 100644
index 6bea0579..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/DynamicProfileGenerator.java
+++ /dev/null
@@ -1,312 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import java.util.ArrayList;
-import java.util.LinkedList;
-import java.util.List;
-
-import edu.wpi.first.math.MathUtil;
-
-/**
- * Motion profile generator with arbitrary start and end motion states and
- * dynamic constraints.
- * 
- * The main issue with this approach is the spatial sampling, which means that
- * the resulting temporal sampling depends on the profile speed. At low speed,
- * it can be quite coarse.
- */
-public class DynamicProfileGenerator {
-
-    /**
-     * Generates a motion profile with dynamic maximum velocity and acceleration.
-     * Uses the algorithm described in section 3.2 of
-     * [Sprunk2008.pdf](http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf).
-     *
-     * Warning:
-     * Profiles may be generated incorrectly if the endpoint velocity/acceleration
-     * values preclude the obedience of the motion constraints. To protect against
-     * this, verify the continuity of the generated profile or keep the start and
-     * goal velocities at 0.
-     *
-     * @param start                  start motion state
-     * @param goal                   goal motion state
-     * @param velocityConstraint     velocity constraint
-     * @param accelerationConstraint acceleration constraint
-     * @param resolution             separation between constraint samples
-     */
-    public static MotionProfile generateMotionProfile(
-            MotionState start,
-            MotionState goal,
-            VelocityConstraint velocityConstraint,
-            AccelerationConstraint accelerationConstraint,
-            double resolution) {
-        if (goal.x() < start.x()) {
-            return generateMotionProfile(
-                    start.flipped(),
-                    goal.flipped(),
-                    (s) -> velocityConstraint.get(-s),
-                    (s) -> accelerationConstraint.get(-s),
-                    resolution).flipped();
-        }
-
-        double length = goal.x() - start.x();
-        // dx is an adjusted resolution that fits nicely within length
-        // at least two samples are required to have a valid profile
-        int samples = Math.max(2, (int) Math.ceil(length / resolution));
-
-        int size = samples - 1;
-        double step = length / size;
-
-        List<MotionSpan> forwardStates = computeForward(
-                start, velocityConstraint, accelerationConstraint, step, size);
-
-        List<MotionSpan> backwardStates = computeBackward(
-                goal, velocityConstraint, accelerationConstraint, step, size);
-
-        List<MotionSpan> finalStates = merge(forwardStates, backwardStates);
-
-        return getMotionSegments(finalStates);
-    }
-
-    /**
-     * Computes the forward states.
-     * 
-     * Applies maximum acceleration starting at min(last velocity, max vel) on a
-     * segment-by-segment basis.
-     */
-    static List<MotionSpan> computeForward(
-            MotionState start,
-            VelocityConstraint velocityConstraint,
-            AccelerationConstraint accelerationConstraint,
-            double step,
-            int size) {
-        List<MotionSpan> forwardStates = new ArrayList<>();
-        MotionState lastState = start;
-        for (int i = 0; i < size; ++i) {
-            double displacement = start.x() + step * i;
-            double maxVel = velocityConstraint.get(displacement);
-            double maxAccel = accelerationConstraint.get(displacement);
-            if (lastState.v() >= maxVel) {
-                // the last velocity exceeds max vel so we just coast
-                MotionState state = new MotionState(displacement, maxVel, 0.0, 0);
-                forwardStates.add(new MotionSpan(state, step));
-                lastState = evolve(state, step);
-            } else {
-                // compute the final velocity assuming max accel
-                double finalVel = Math.sqrt(lastState.v() * lastState.v() + 2 * maxAccel * step);
-                if (finalVel <= maxVel) {
-                    // we're still under max vel so we're good
-                    MotionState state = new MotionState(displacement, lastState.v(), maxAccel, 0);
-                    forwardStates.add(new MotionSpan(state, step));
-                    lastState = evolve(state, step);
-                } else {
-                    // we went over max vel so now we split the segment
-                    double accelDx = (maxVel * maxVel - lastState.v() * lastState.v()) / (2 * maxAccel);
-                    MotionState accelState = new MotionState(displacement, lastState.v(), maxAccel, 0);
-                    MotionState coastState = new MotionState(displacement + accelDx, maxVel, 0.0, 0);
-                    forwardStates.add(new MotionSpan(accelState, accelDx));
-                    forwardStates.add(new MotionSpan(coastState, step - accelDx));
-                    lastState = evolve(coastState, step - accelDx);
-                }
-            }
-        }
-        return forwardStates;
-    }
-
-    /**
-     * Computes the backward states.
-     * 
-     * Walks backwards from the goal, computing states with maximum decel.
-     */
-    static List<MotionSpan> computeBackward(
-            MotionState goal,
-            VelocityConstraint velocityConstraint,
-            AccelerationConstraint accelerationConstraint,
-            double step,
-            int size) {
-        // linkedlist allows efficient addFirst
-        LinkedList<MotionSpan> backwardStates = new LinkedList<>();
-        // going back in time, so this is the "next" state not the "last" state.
-        MotionState nextState = goal;
-        for (int i = 0; i < size; ++i) {
-            // walk backwards from the goal
-            double displacement = goal.x() - step * i;
-            // compute the segment constraints
-            double maxVel = velocityConstraint.get(displacement);
-            double maxAccel = accelerationConstraint.get(displacement);
-            if (nextState.v() >= maxVel) {
-                // the last velocity exceeds max vel so we just coast
-                MotionState state = new MotionState(displacement, maxVel, 0.0, 0);
-                // step backwards
-                nextState = devolve(state, step);
-                backwardStates.addFirst(new MotionSpan(nextState, step));
-            } else {
-                // compute the final velocity assuming max accel
-                double finalVel = Math.sqrt(nextState.v() * nextState.v() + 2 * maxAccel * step);
-                if (finalVel <= maxVel) {
-                    // we're still under max vel so we're good
-                    // note negative accel
-                    MotionState state = new MotionState(displacement, nextState.v(), -maxAccel, 0);
-                    nextState = devolve(state, step);
-                    backwardStates.addFirst(new MotionSpan(nextState, step));
-                } else {
-                    // we went over max vel so now we split the segment
-                    double accelDx = (maxVel * maxVel - nextState.v() * nextState.v()) / (2 * maxAccel);
-                    // note negative accel
-                    MotionState accelState = new MotionState(displacement, nextState.v(), -maxAccel, 0);
-                    MotionState coastState = new MotionState(displacement - accelDx, maxVel, 0.0, 0);
-                    backwardStates.addFirst(new MotionSpan(devolve(accelState, accelDx), accelDx));
-                    nextState = devolve(coastState, step - accelDx);
-                    backwardStates.addFirst(new MotionSpan(nextState, step - accelDx));
-                }
-            }
-        }
-        return backwardStates;
-    }
-
-    /** merge the forward and backward states */
-    private static List<MotionSpan> merge(
-            List<MotionSpan> forwardStates,
-            List<MotionSpan> backwardStates) {
-        List<MotionSpan> finalStates = new ArrayList<>();
-
-        int i = 0;
-        while (i < forwardStates.size() && i < backwardStates.size()) {
-            // retrieve the start states and displacement deltas
-            MotionState forwardStartState = forwardStates.get(i).start();
-            double forwardDx = forwardStates.get(i).dx();
-            MotionState backwardStartState = backwardStates.get(i).start();
-            double backwardDx = backwardStates.get(i).dx();
-
-            // if there's a discrepancy in the displacements, split the the longer chunk in
-            // two and add the second
-            // to the corresponding list; this guarantees that segments are always aligned
-            if (!(MathUtil.isNear(forwardDx, backwardDx, 1e-6))) {
-                if (forwardDx > backwardDx) {
-                    // forward longer
-                    forwardStates.add(
-                            i + 1,
-                            new MotionSpan(evolve(forwardStartState, backwardDx), forwardDx - backwardDx));
-                    forwardDx = backwardDx;
-                } else {
-                    // backward longer
-                    backwardStates.add(
-                            i + 1,
-                            new MotionSpan(evolve(backwardStartState, forwardDx), backwardDx - forwardDx));
-                    backwardDx = forwardDx;
-                }
-            }
-
-            // compute the end states (after alignment)
-            MotionState forwardEndState = evolve(forwardStartState, forwardDx);
-            MotionState backwardEndState = evolve(backwardStartState, backwardDx);
-
-            if (forwardStartState.v() <= backwardStartState.v()) {
-                // forward start lower
-                if (forwardEndState.v() <= backwardEndState.v()) {
-                    // forward end lower
-                    finalStates.add(new MotionSpan(forwardStartState, forwardDx));
-                } else {
-                    // backward end lower
-                    double intersection = intersection(
-                            forwardStartState,
-                            backwardStartState);
-                    finalStates.add(new MotionSpan(forwardStartState, intersection));
-                    finalStates.add(
-                            new MotionSpan(
-                                    evolve(backwardStartState, intersection),
-                                    backwardDx - intersection));
-                }
-            } else {
-                // backward start lower
-                if (forwardEndState.v() >= backwardEndState.v()) {
-                    // backward end lower
-                    finalStates.add(new MotionSpan(backwardStartState, backwardDx));
-                } else {
-                    // forward end lower
-                    double intersection = intersection(
-                            forwardStartState,
-                            backwardStartState);
-                    finalStates.add(new MotionSpan(backwardStartState, intersection));
-                    finalStates.add(
-                            new MotionSpan(
-                                    evolve(forwardStartState, intersection),
-                                    forwardDx - intersection));
-                }
-            }
-            i++;
-        }
-        return finalStates;
-    }
-
-    /** Turn the final states into actual time-parameterized motion segments */
-    private static MotionProfile getMotionSegments(List<MotionSpan> finalStates) {
-        List<MotionSegment> motionSegments = new ArrayList<MotionSegment>();
-        for (MotionSpan finalState : finalStates) {
-            MotionState state = finalState.start();
-            double stateDx = finalState.dx();
-            double dt;
-            if (Math.abs(state.a()) < 1e-6) {
-                dt = stateDx / state.v();
-            } else {
-                double discriminant = state.v() * state.v() + 2 * state.a() * stateDx;
-                if (Math.abs(discriminant) < 1e-6) {
-                    dt = -state.v() / state.a();
-                } else {
-                    dt = (Math.sqrt(discriminant) - state.v()) / state.a();
-                }
-            }
-            motionSegments.add(new MotionSegment(state, dt));
-        }
-
-        return new MotionProfile(motionSegments);
-    }
-
-    ////////////////////////////////////////////////////////
-
-    /** Integrates the starting state over distance (not time) dx. */
-    static MotionState evolve(MotionState state, double dx) {
-        double discriminant = state.v() * state.v() + 2 * state.a() * dx;
-        if (discriminant < -1e-6) {
-            throw new IllegalArgumentException("state " + state + " does not extend to " + dx);
-        }
-        if (MathUtil.isNear(discriminant, 0.0, 1e-6)) {
-            return new MotionState(state.x() + dx, 0.0, state.a(), 0);
-        }
-        return new MotionState(state.x() + dx, Math.sqrt(discriminant), state.a(), 0);
-    }
-
-    /** Backwards in time */
-    static MotionState devolve(MotionState state, double dx) {
-        double discriminant = state.v() * state.v() - 2 * state.a() * dx;
-        if (discriminant < -1e-6) {
-            throw new IllegalArgumentException("state " + state + " does not extend to " + dx);
-        }
-        if (MathUtil.isNear(discriminant, 0.0, 1e-6)) {
-            return new MotionState(state.x() - dx, 0.0, state.a(), 0);
-        }
-        return new MotionState(state.x() - dx, Math.sqrt(discriminant), state.a(), 0);
-    }
-
-    /**
-     * In phase space, a constant acceleration trajectory is given by
-     * 
-     * v = sqrt(v0^2 + 2ax)
-     * 
-     * So to solve for the intersection (x,y) between two trajectories, set the two
-     * velocities equal and solve for x:
-     * 
-     * sqrt(v1^2 + 2a1x) = sqrt(v2^2 + 2a2x)
-     * v1^2-v2^2 = 2a2x - 2a1x
-     * x = (v1^2-v2^2)/(2a2-2a1)
-     * 
-     * note that this intersection doesn't occur at the same time on each
-     * trajectory.
-     * 
-     * see https://www.desmos.com/calculator/jnc7u3jg11
-     */
-    static double intersection(MotionState state1, MotionState state2) {
-        return (state1.v() * state1.v() - state2.v() * state2.v())
-                / (2 * state2.a() - 2 * state1.a());
-    }
-
-}
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGenerator.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGenerator.java
deleted file mode 100644
index ba87c18a..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGenerator.java
+++ /dev/null
@@ -1,221 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import java.util.List;
-
-import org.team100.lib.util.Math100;
-
-/**
- * Motion profile generator with arbitrary start and end motion states and
- * jerk limiting.
- */
-public class JerkLimitedProfileGenerator {
-    public static MotionProfile generateMotionProfile(
-            MotionState start,
-            MotionState goal,
-            double maxVel,
-            double maxAccel,
-            double maxJerk,
-            boolean overshoot) {
-        if (goal.x() < start.x()) {
-            // ensure the goal is always after the start; plan the flipped profile otherwise
-            return generateMotionProfile(start.flipped(), goal.flipped(), maxVel, maxAccel, maxJerk, overshoot)
-                    .flipped();
-        }
-
-        return sCurve(start, goal, maxVel, maxAccel, maxJerk, overshoot);
-    }
-
-    //////////////////////////////////////////////////
-
-    /** Jerk-limited profile (S-curve) with finite jerk limit. */
-    private static MotionProfile sCurve(
-            MotionState start,
-            MotionState goal,
-            double maxVel,
-            double maxAccel,
-            double maxJerk,
-            boolean overshoot) {
-        MotionProfile accelerationProfile = generateAccelProfile(start, maxVel, maxAccel, maxJerk);
-        // decel is reversed accel with the goal accel flipped
-        MotionProfile decelerationProfile = generateAccelProfile(
-                new MotionState(
-                        goal.x(),
-                        goal.v(),
-                        -goal.a(),
-                        goal.j()),
-                maxVel,
-                maxAccel,
-                maxJerk)
-                .reversed();
-
-        MotionProfile noCoastProfile = accelerationProfile.append(decelerationProfile);
-        double remainingDistance = goal.x() - noCoastProfile.end().x();
-
-        if (remainingDistance >= 0.0) {
-            // we just need to add a constant-velocity segment of appropriate duration
-            double deltaT4 = remainingDistance / maxVel;
-
-            return new MotionProfileBuilder(start)
-                    .appendProfile(accelerationProfile)
-                    .appendJerkSegment(0.0, deltaT4)
-                    .appendProfile(decelerationProfile)
-                    .build();
-        }
-
-        // the profile never reaches maxV
-        // thus, we need to compute the peak velocity (0 < peak vel < max vel)
-        // we *could* construct a large polynomial expression (i.e., a nasty cubic) and
-        // solve it using Cardano's
-        // method, some kind of inclusion method like modified Anderson-Bjorck-King, or
-        // a host of other methods
-        // (see https://link.springer.com/content/pdf/bbm%3A978-3-642-05175-3%2F1.pdf
-        // for modified ABK)
-        // instead, however, we conduct a binary search as it's sufficiently performant
-        // for this use case,
-        // requires less code, and is overall significantly more comprehensible
-        double upperBound = maxVel;
-        double lowerBound = 0.0;
-        int iterations = 0;
-        while (iterations < 1000) {
-            double peakVel = (upperBound + lowerBound) / 2;
-
-            MotionProfile searchAccelProfile = generateAccelProfile(start, peakVel, maxAccel, maxJerk);
-            MotionProfile searchDecelProfile = generateAccelProfile(goal, peakVel, maxAccel, maxJerk)
-                    .reversed();
-
-            MotionProfile searchProfile = searchAccelProfile.append(searchDecelProfile);
-
-            double error = goal.x() - searchProfile.end().x();
-
-            if (Math.abs(error) < 1e-6) {
-                return searchProfile;
-            }
-
-            if (error > 0.0) {
-                // we undershot so shift the lower bound up
-                lowerBound = peakVel;
-            } else {
-                // we overshot so shift the upper bound down
-                upperBound = peakVel;
-            }
-
-            iterations++;
-        }
-
-        // constraints are not satisfiable
-        if (overshoot) {
-            return noCoastProfile.append(
-                    generateMotionProfile(
-                            noCoastProfile.end(),
-                            goal,
-                            maxVel,
-                            maxAccel,
-                            maxJerk,
-                            true));
-        }
-        // violate max jerk first
-        return TrapezoidProfileGenerator.generateSimpleMotionProfile(start, goal, maxVel, maxAccel, false);
-    }
-
-    /**
-     * Returns a profile with two (jerk-limited) or three (jerk-limited,
-     * acceleration-limited, jerk-limited) segments to full velocity. The end state
-     * has zero acceleration.
-     */
-    static MotionProfile generateAccelProfile(
-            MotionState start,
-            double maxVel,
-            double maxAccel,
-            double maxJerk) {
-
-        // compute the duration and velocity of the first segment
-        double deltaT1;
-        double deltaV1;
-        if (start.a() > maxAccel) {
-            // slow down and see where we are
-            deltaT1 = (start.a() - maxAccel) / maxJerk;
-            deltaV1 = start.a() * deltaT1 - 0.5 * maxJerk * deltaT1 * deltaT1;
-        } else {
-            // otherwise accelerate
-            deltaT1 = (maxAccel - start.a()) / maxJerk;
-            deltaV1 = start.a() * deltaT1 + 0.5 * maxJerk * deltaT1 * deltaT1;
-        }
-
-        // compute the duration and velocity of the third segment
-        double deltaT3 = maxAccel / maxJerk;
-        double deltaV3 = maxAccel * deltaT3 - 0.5 * maxJerk * deltaT3 * deltaT3;
-
-        // compute the velocity change required in the second segment
-        double deltaV2 = maxVel - start.v() - deltaV1 - deltaV3;
-
-        if (deltaV2 < 0.0) {
-            // there is no constant acceleration phase
-            // the second case checks if we're going to exceed max vel
-            if (start.a() > maxAccel
-                    || (start.v() - maxVel) > (start.a() * start.a()) / (2 * maxJerk)) {
-                // problem: we need to cut down on our acceleration but we can't cut our initial
-                // decel
-                // solution: we'll lengthen our initial decel to -max accel and similarly with
-                // our final accel
-                // if this results in an over correction, decel instead to a good accel
-                double newDeltaT1 = (start.a() + maxAccel) / maxJerk;
-                double newDeltaV1 = start.a() * newDeltaT1 - 0.5 * maxJerk * newDeltaT1 * newDeltaT1;
-
-                double newDeltaV2 = maxVel - start.v() - newDeltaV1 + deltaV3;
-
-                if (newDeltaV2 > 0.0) {
-                    // we decelerated too much
-                    List<Double> roots = Math100.solveQuadratic(
-                            -maxJerk,
-                            2 * start.a(),
-                            start.v() - maxVel - start.a() * start.a() / (2 * maxJerk));
-                    double finalDeltaT1 = roots.stream().filter((it) -> it >= 0.0).min(Double::compare)
-                            .orElseThrow();
-                    double finalDeltaT3 = finalDeltaT1 - start.a() / maxJerk;
-
-                    return new MotionProfileBuilder(start)
-                            .appendJerkSegment(-maxJerk, finalDeltaT1)
-                            .appendJerkSegment(maxJerk, finalDeltaT3)
-                            .build();
-                } else {
-                    // we're almost good
-                    double newDeltaT2 = newDeltaV2 / -maxAccel;
-
-                    return new MotionProfileBuilder(start)
-                            .appendJerkSegment(-maxJerk, newDeltaT1)
-                            .appendJerkSegment(0.0, newDeltaT2)
-                            .appendJerkSegment(maxJerk, deltaT3)
-                            .build();
-                }
-            } else {
-                // cut out the constant accel phase and find a shorter delta t1 and delta t3
-                List<Double> roots = Math100.solveQuadratic(
-                        maxJerk,
-                        2 * start.a(),
-                        start.v() - maxVel + start.a() * start.a() / (2 * maxJerk));
-                double newDeltaT1 = roots.stream().filter((it) -> it >= 0.0).min(Double::compare).orElseThrow();
-                double newDeltaT3 = newDeltaT1 + start.a() / maxJerk;
-
-                return new MotionProfileBuilder(start)
-                        .appendJerkSegment(maxJerk, newDeltaT1)
-                        .appendJerkSegment(-maxJerk, newDeltaT3)
-                        .build();
-            }
-        } else {
-            // there is a constant acceleration phase
-            double deltaT2 = deltaV2 / maxAccel;
-
-            MotionProfileBuilder builder = new MotionProfileBuilder(start);
-            if (start.a() > maxAccel) {
-                builder.appendJerkSegment(-maxJerk, deltaT1);
-            } else {
-                builder.appendJerkSegment(maxJerk, deltaT1);
-            }
-            return builder.appendJerkSegment(0.0, deltaT2)
-                    .appendJerkSegment(-maxJerk, deltaT3)
-                    .build();
-        }
-
-    }
-
-}
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfile.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfile.java
deleted file mode 100644
index b6d5f4d0..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfile.java
+++ /dev/null
@@ -1,103 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import java.util.Collections;
-import java.util.List;
-import java.util.stream.Collectors;
-
-/** Time-parameterized motion profile. */
-public class MotionProfile {
-    private final List<MotionSegment> segments;
-
-    /**
-     * Trapezoidal motion profile composed of motion segments.
-     *
-     * @param segments profile motion segments
-     */
-    public MotionProfile(List<MotionSegment> segments) {
-        if (segments.isEmpty())
-            throw new IllegalArgumentException();
-        this.segments = segments;
-    }
-
-    /**
-     * Returns the [MotionState] at time [t].
-     */
-    public MotionState get(double t) {
-        if (t < 0.0)
-            return segments.get(0).start().stationary();
-
-        var remainingTime = t;
-        for (MotionSegment segment : segments) {
-            if (remainingTime <= segment.dt()) {
-                return segment.get(remainingTime);
-            }
-            remainingTime -= segment.dt();
-        }
-
-        return segments.get(segments.size() - 1).end().stationary();
-    }
-
-    /**
-     * Returns the duration of the motion profile.
-     */
-    public double duration() {
-        return segments.stream().map((it) -> it.dt()).reduce(0.0, Double::sum);
-    }
-
-    /**
-     * Returns a reversed version of the motion profile.
-     */
-    public MotionProfile reversed() {
-        List<MotionSegment> l = segments.stream().map((it) -> it.reversed()).collect(Collectors.toList());
-        Collections.reverse(l);
-        return new MotionProfile(l);
-    }
-
-    /**
-     * Returns a flipped version of the motion profile.
-     */
-    public MotionProfile flipped() {
-        return new MotionProfile(segments.stream().map((it) -> it.flipped()).collect(Collectors.toList()));
-    }
-
-    /**
-     * Returns the start [MotionState].
-     */
-    public MotionState start() {
-        return segments.get(0).start();
-    }
-
-    /**
-     * Returns the end [MotionState].
-     */
-    public MotionState end() {
-        return segments.get(segments.size() - 1).end();
-    }
-
-    /**
-     * Returns a new motion profile with [other] concatenated.
-     */
-    MotionProfile append(MotionProfile other) {
-        MotionProfileBuilder builder = new MotionProfileBuilder(start());
-        builder.appendProfile(this);
-        builder.appendProfile(other);
-        return builder.build();
-    }
-
-    public List<MotionSegment> getSegments() {
-        return segments;
-    }
-
-    @Override
-    public String toString() {
-        StringBuilder sb = new StringBuilder();
-        sb.append("MotionProfile [\n");
-        for (MotionSegment s : segments) {
-            sb.append(s);
-            sb.append("\n");
-            sb.append("]");
-        }
-        return sb.toString();
-    }
-
-}
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfileBuilder.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfileBuilder.java
deleted file mode 100644
index 11069874..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionProfileBuilder.java
+++ /dev/null
@@ -1,78 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import java.util.ArrayList;
-import java.util.List;
-
-public class MotionProfileBuilder {
-    private final List<MotionSegment> segments;
-    private MotionState currentState;
-
-    /**
-     * Easy-to-use builder for creating motion profiles.
-     *
-     * @param start start motion state
-     */
-    public MotionProfileBuilder(MotionState start) {
-        currentState = start;
-        segments = new ArrayList<MotionSegment>();
-    }
-
-    /**
-     * Appends a constant-jerk control for the provided duration.
-     */
-    MotionProfileBuilder appendJerkSegment(double jerk, double dt) {
-        MotionSegment segment = new MotionSegment(
-                new MotionState(currentState.x(), currentState.v(), currentState.a(), jerk),
-                dt);
-        segments.add(segment);
-        currentState = segment.end();
-        return this;
-    }
-
-    /**
-     * Appends a constant-acceleration control for the provided duration.
-     */
-    MotionProfileBuilder appendAccelerationSegment(double accel, double dt) {
-        MotionSegment segment = new MotionSegment(
-                new MotionState(currentState.x(), currentState.v(), accel, 0),
-                dt);
-        segments.add(segment);
-        currentState = segment.end();
-        return this;
-    }
-
-    /**
-     * Appends a constant-velocity control for the provided duration.
-     */
-    MotionProfileBuilder appendVelocitySegment(double dt) {
-        MotionSegment segment = new MotionSegment(
-                new MotionState(currentState.x(), currentState.v(), 0, 0),
-                dt);
-        segments.add(segment);
-        currentState = segment.end();
-        return this;
-    }
-
-    /**
-     * Appends a [MotionProfile] to the current queue of control actions.
-     */
-    MotionProfileBuilder appendProfile(MotionProfile profile) {
-        for (MotionSegment segment : profile.getSegments()) {
-            if (Math.abs(segment.start().j()) < 1e-6) {
-                // constant acceleration
-                appendAccelerationSegment(segment.start().a(), segment.dt());
-            } else {
-                // constant jerk
-                appendJerkSegment(segment.start().j(), segment.dt());
-            }
-        }
-        return this;
-    }
-
-    /**
-     * Constructs and returns the [MotionProfile] instance.
-     */
-    MotionProfile build() {
-        return new MotionProfile(segments);
-    }
-}
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSegment.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSegment.java
deleted file mode 100644
index ce7cb035..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSegment.java
+++ /dev/null
@@ -1,46 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-/**
- * Time-parameterized segment of a profile with constant acceleration
- * 
- * @param start state at t = 0
- * @param dt    duration of this segment
- */
-public record MotionSegment(MotionState start, double dt) {
-
-    /**
-     * @param t in range [0, duration]
-     */
-    MotionState get(double t) {
-        return start.get(t);
-    }
-
-    /**
-     * State at the end of the segment.
-     */
-    MotionState end() {
-        return start.get(dt);
-    }
-
-    /**
-     * Returns a reversed version of the segment. Note: it isn't possible to reverse
-     * a segment completely so this
-     * method only guarantees that the start and end velocities will be swapped.
-     */
-    MotionSegment reversed() {
-        MotionState end = end();
-        MotionState state = new MotionState(end.x(), end.v(), -end.a(), end.j());
-        return new MotionSegment(state, dt);
-    }
-
-    /**
-     * Returns a flipped (negated) version of the segment.
-     */
-    MotionSegment flipped() {
-        return new MotionSegment(start.flipped(), dt);
-    }
-
-    public String toString() {
-        return String.format("(%s, %f)", start, dt);
-    }
-}
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSpan.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSpan.java
deleted file mode 100644
index f939a0ee..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionSpan.java
+++ /dev/null
@@ -1,10 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-/**
- * Part of a motion profile, parameterized by distance.
- * 
- * @param start state at x = 0
- * @param dx    length of this span
- */
-public record MotionSpan(MotionState start, double dx) {
-}
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionState.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionState.java
deleted file mode 100644
index 8c8b97c5..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/MotionState.java
+++ /dev/null
@@ -1,36 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-/**
- * Kinematic state of a motion profile at any given time.
- */
-public record MotionState(double x, double v, double a, double j) {
-
-    /**
-     * Returns the [MotionState] at time [t].
-     */
-    public MotionState get(double t) {
-        return new MotionState(
-                x + v * t + a / 2 * t * t + j / 6 * t * t * t,
-                v + a * t + j / 2 * t * t,
-                a + j * t,
-                j);
-    }
-
-    /**
-     * Returns a flipped (negated) version of the state.
-     */
-    public MotionState flipped() {
-        return new MotionState(-x, -v, -a, -j);
-    }
-
-    /**
-     * Returns the state with velocity, acceleration, and jerk zeroed.
-     */
-    public MotionState stationary() {
-        return new MotionState(x, 0.0, 0.0, 0.0);
-    }
-
-    public String toString() {
-        return String.format("(x=%.3f, v=%.3f, a=%.3f, j=%.3f)", x, v, a, j);
-    }
-}
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/README.md b/lib/src/main/java/org/team100/lib/profile/roadrunner/README.md
deleted file mode 100644
index 87fe4e7a..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/README.md
+++ /dev/null
@@ -1,4 +0,0 @@
-# lib.profile.roadrunner
-
-The package inclues classes directly from Roadrunner Kotlin code,
-as of in 2023, in order to use the `JerkLimitedProfileGenerator`.
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGenerator.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGenerator.java
deleted file mode 100644
index 22f9d3a2..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGenerator.java
+++ /dev/null
@@ -1,132 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import java.util.List;
-
-import org.team100.lib.util.Math100;
-
-/**
- * Motion profile generator with arbitrary start and end motion states and
- * infinite jerk.
- */
-public class TrapezoidProfileGenerator {
-    public static MotionProfile generateSimpleMotionProfile(
-            MotionState start,
-            MotionState goal,
-            double maxVel,
-            double maxAccel,
-            boolean overshoot) {
-        if (goal.x() < start.x()) {
-            // ensure the goal is always after the start; plan the flipped profile otherwise
-            return generateSimpleMotionProfile(start.flipped(), goal.flipped(), maxVel, maxAccel, overshoot)
-                    .flipped();
-        }
-        return trapezoid(start, goal, maxVel, maxAccel, overshoot);
-    }
-    
-    /////////////////////////////////////////////////
-
-    /** Acceleration-limited profile (trapezoidal) */
-    private static MotionProfile trapezoid(
-            MotionState start,
-            MotionState goal,
-            double maxVel,
-            double maxAccel,
-            boolean overshoot) {
-        double requiredAccel = (goal.v() * goal.v() - start.v() * start.v())
-                / (2 * (goal.x() - start.x()));
-
-        MotionProfile accelProfile = unlimitedJerk(start, maxVel, maxAccel);
-        MotionProfile decelProfile = unlimitedJerk(
-                new MotionState(
-                        goal.x(),
-                        goal.v(),
-                        -goal.a(),
-                        goal.j()),
-                maxVel,
-                maxAccel)
-                .reversed();
-
-        MotionProfile noCoastProfile = accelProfile.append(decelProfile);
-        double remainingDistance = goal.x() - noCoastProfile.end().x();
-
-        if (remainingDistance >= 0.0) {
-            return threeSegment(start, maxVel, accelProfile, decelProfile, remainingDistance);
-
-        } else if (Math.abs(requiredAccel) > maxAccel) {
-            // not possible within constraints
-            if (overshoot) {
-                return noCoastProfile.append(generateSimpleMotionProfile(
-                        noCoastProfile.end(), goal, maxVel, maxAccel, true));
-            } else {
-                // violate the constraints
-                // single segment profile
-                double dt = (goal.v() - start.v()) / requiredAccel;
-                return new MotionProfileBuilder(start)
-                        .appendAccelerationSegment(requiredAccel, dt)
-                        .build();
-            }
-        } else if (start.v() > maxVel && goal.v() > maxVel) {
-            // this should not happen.
-            // decel, accel
-            List<Double> roots = Math100.solveQuadratic(
-                    -maxAccel,
-                    2 * start.v(),
-                    (goal.v() * goal.v() - start.v() * start.v()) / (2 * maxAccel) - goal.x()
-                            + start.x());
-            double deltaT1 = roots.stream().filter((it) -> it >= 0.0).min(Double::compare).orElseThrow();
-            double deltaT3 = Math.abs(start.v() - goal.v()) / maxAccel + deltaT1;
-
-            return new MotionProfileBuilder(start)
-                    .appendAccelerationSegment(-maxAccel, deltaT1)
-                    .appendAccelerationSegment(maxAccel, deltaT3)
-                    .build();
-        } else {
-            // accel, decel
-            List<Double> roots = Math100.solveQuadratic(
-                    maxAccel,
-                    2 * start.v(),
-                    (start.v() * start.v() - goal.v() * goal.v()) / (2 * maxAccel) - goal.x()
-                            + start.x());
-            double deltaT1 = roots.stream().filter((it) -> it >= 0.0).min(Double::compare).orElseThrow();
-            double deltaT3 = Math.abs(start.v() - goal.v()) / maxAccel + deltaT1;
-
-            return new MotionProfileBuilder(start)
-                    .appendAccelerationSegment(maxAccel, deltaT1)
-                    .appendAccelerationSegment(-maxAccel, deltaT3)
-                    .build();
-        }
-    }
-
-    /**
-     * Normal 3-segment profile: accel, cruise, decel, with unlimited jerk in
-     * between.
-     */
-    private static MotionProfile threeSegment(
-            MotionState start,
-            double maxVel,
-            MotionProfile accelProfile,
-            MotionProfile decelProfile,
-            double remainingDistance) {
-        double deltaT2 = remainingDistance / maxVel;
-        return new MotionProfileBuilder(start)
-                .appendProfile(accelProfile)
-                .appendVelocitySegment(deltaT2)
-                .appendProfile(decelProfile)
-                .build();
-    }
-
-    /**
-     * Returns a profile with one max-acceleration segment to full velocity.
-     */
-    static MotionProfile unlimitedJerk(MotionState start, double maxVel, double maxAccel) {
-        double deltaT1 = Math.abs(start.v() - maxVel) / maxAccel;
-        MotionProfileBuilder builder = new MotionProfileBuilder(start);
-        if (start.v() > maxVel) {
-            // we need to decelerate
-            builder.appendAccelerationSegment(-maxAccel, deltaT1);
-        } else {
-            builder.appendAccelerationSegment(maxAccel, deltaT1);
-        }
-        return builder.build();
-    }
-}
diff --git a/lib/src/main/java/org/team100/lib/profile/roadrunner/VelocityConstraint.java b/lib/src/main/java/org/team100/lib/profile/roadrunner/VelocityConstraint.java
deleted file mode 100644
index 7fcfb775..00000000
--- a/lib/src/main/java/org/team100/lib/profile/roadrunner/VelocityConstraint.java
+++ /dev/null
@@ -1,13 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-
-/**
- * Motion profile velocity constraint.
- */
-@FunctionalInterface
-public interface VelocityConstraint {
-    /**
-     * Returns the maximum profile velocity at displacement [s].
-     */
-    double get(double s);
-}
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java b/lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java
index 23d858b9..0cf4d7bb 100644
--- a/lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/se2/FreeRotationProfile.java
@@ -1,7 +1,7 @@
 package org.team100.lib.profile.se2;
 
 import org.team100.lib.framework.TimedRobot100;
-import org.team100.lib.profile.incremental.IncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.ControlSE2;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java
index ea656e87..620e2eb6 100644
--- a/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java
+++ b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfile.java
@@ -1,7 +1,7 @@
 package org.team100.lib.profile.se2;
 
 import org.team100.lib.framework.TimedRobot100;
-import org.team100.lib.profile.incremental.IncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.ControlSE2;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java
index 6d0a2646..e1476a2a 100644
--- a/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java
+++ b/lib/src/main/java/org/team100/lib/profile/se2/HolonomicProfileFactory.java
@@ -2,9 +2,9 @@
 
 import org.team100.lib.logging.Level;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.incremental.CurrentLimitedExponentialProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidProfileWPI;
+import org.team100.lib.profile.r1.CurrentLimitedExponentialProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 
 public class HolonomicProfileFactory {
diff --git a/lib/src/main/java/org/team100/lib/profile/timed/JerkLimitedTimedProfile.java b/lib/src/main/java/org/team100/lib/profile/timed/JerkLimitedTimedProfile.java
deleted file mode 100644
index 03639abb..00000000
--- a/lib/src/main/java/org/team100/lib/profile/timed/JerkLimitedTimedProfile.java
+++ /dev/null
@@ -1,60 +0,0 @@
-package org.team100.lib.profile.timed;
-
-import org.team100.lib.profile.roadrunner.JerkLimitedProfileGenerator;
-import org.team100.lib.profile.roadrunner.MotionProfile;
-import org.team100.lib.profile.roadrunner.MotionState;
-import org.team100.lib.state.Control100;
-import org.team100.lib.state.Model100;
-
-/** Adapter for Roadrunner jerk-limited profiles. */
-public class JerkLimitedTimedProfile implements TimedProfile {
-    private static final boolean DEBUG = false;
-
-    private final double vel;
-    private final double acc;
-    private final double jerk;
-    private final boolean overshoot;
-    private MotionProfile m_profile;
-
-    /**
-     * You can specify independent limits for velocity, acceleration, and jerk.
-     * 
-     * @param overshoot if a single profile is impossible, true means to use one
-     *                  that overshoots the goal, and a second one that returns to
-     *                  it from the other side. false means to violate the
-     *                  constraints. you should probably say "true" here.
-     */
-    public JerkLimitedTimedProfile(double vel, double acc, double jerk, boolean overshoot) {
-        this.vel = vel;
-        this.acc = acc;
-        this.jerk = jerk;
-        this.overshoot = overshoot;
-    }
-
-    @Override
-    public void init(Control100 initial, Model100 goal) {
-        MotionState start = new MotionState(initial.x(), initial.v(), initial.a(), 0);
-        MotionState end = new MotionState(goal.x(), goal.v(), 0, 0);
-        // "true" below means "overshoot rather than violating constraints"
-        m_profile = JerkLimitedProfileGenerator.generateMotionProfile(
-                start, end, vel, acc, jerk, overshoot);
-        if (DEBUG) {
-            System.out.printf("init %s goal %s profile %s\n", initial, goal, m_profile);
-        }
-    }
-
-    @Override
-    public Control100 sample(double timeS) {
-        MotionState s = m_profile.get(timeS);
-        if (DEBUG) {
-            System.out.printf("time %f x %f v %f a %f\n", timeS, s.x(), s.v(), s.a());
-        }
-        return new Control100(s.x(), s.v(), s.a());
-    }
-
-    @Override
-    public double duration() {
-        return m_profile.duration();
-    }
-
-}
diff --git a/lib/src/main/java/org/team100/lib/profile/timed/README.md b/lib/src/main/java/org/team100/lib/profile/timed/README.md
deleted file mode 100644
index 00b50220..00000000
--- a/lib/src/main/java/org/team100/lib/profile/timed/README.md
+++ /dev/null
@@ -1,7 +0,0 @@
-# lib.profile.timed
-
-This package includes profiles that require
-significant precomputation, kinda like a trajectory.
-
-At the moment, the only implementations are wrappers of `roadrunner`
-profiles.
\ No newline at end of file
diff --git a/lib/src/main/java/org/team100/lib/profile/timed/TaperedProfile.java b/lib/src/main/java/org/team100/lib/profile/timed/TaperedProfile.java
deleted file mode 100644
index bf5fc01f..00000000
--- a/lib/src/main/java/org/team100/lib/profile/timed/TaperedProfile.java
+++ /dev/null
@@ -1,69 +0,0 @@
-package org.team100.lib.profile.timed;
-
-import org.team100.lib.profile.roadrunner.DynamicProfileGenerator;
-import org.team100.lib.profile.roadrunner.MotionProfile;
-import org.team100.lib.profile.roadrunner.MotionState;
-import org.team100.lib.state.Control100;
-import org.team100.lib.state.Model100;
-
-/**
- * Adapter for Roadrunner dynamic profiles.
- * 
- * The use-case is soft-landing, so the acceleration constraint tapers to zero
- * at the goal.
- * 
- * If the taper is linear, then the goal is never reached; we use the cube root
- * to obtain roughly linear tapering with time, i.e. constant jerk
- */
-public class TaperedProfile implements TimedProfile {
-    private final double vel;
-    private final double acc;
-    private final double taper;
-    private final double resolution;
-    private MotionProfile m_profile;
-
-    /**
-     * Taper the acceleration near the goal kinda coarsely, using
-     * constant-acceleration segments.
-     * 
-     * @param vel
-     * @param acc
-     * @param jerk       I kinda guessed what to do with this parameter, so it's
-     *                   only like very roughly the jerk. The actual jerk is pretty
-     *                   high between segments.
-     * @param resolution how many constant-accel segments to use; note these are
-     *                   segmented by *distance* so the last few are larger in time.
-     *                   Higher resolution takes a little longer to calculate.
-     */
-    public TaperedProfile(double vel, double acc, double jerk, double resolution) {
-        this.vel = vel;
-        this.acc = acc;
-        // i have no idea if this is right; it does seem to fit the data.
-        this.taper = 1.8 * Math.pow(jerk, 2.0 / 3);
-        this.resolution = resolution;
-    }
-
-    @Override
-    public void init(Control100 initial, Model100 goal) {
-        MotionState start = new MotionState(initial.x(), initial.v(), initial.a(), 0);
-        MotionState end = new MotionState(goal.x(), goal.v(), 0, 0);
-        m_profile = DynamicProfileGenerator.generateMotionProfile(
-                start, end, (s) -> vel, (s) -> {
-                    double togo = goal.x() - s;
-                    double taperedAcc = Math.pow(Math.abs(togo), 1.0 / 3) * taper;
-                    return Math.min(acc, taperedAcc);
-                }, resolution);
-    }
-
-    @Override
-    public Control100 sample(double timeS) {
-        MotionState s = m_profile.get(timeS);
-        return new Control100(s.x(), s.v(), s.a());
-    }
-
-    @Override
-    public double duration() {
-        return m_profile.duration();
-    }
-
-}
diff --git a/lib/src/main/java/org/team100/lib/profile/timed/TimedProfile.java b/lib/src/main/java/org/team100/lib/profile/timed/TimedProfile.java
deleted file mode 100644
index 56273abf..00000000
--- a/lib/src/main/java/org/team100/lib/profile/timed/TimedProfile.java
+++ /dev/null
@@ -1,22 +0,0 @@
-package org.team100.lib.profile.timed;
-
-import org.team100.lib.state.Control100;
-import org.team100.lib.state.Model100;
-
-/**
- * A timed profile is analogous to a trajectory: you precalculate something,
- * and then you sample it by time.
- */
-public interface TimedProfile {
-
-    /**
-     * Initial is Control100 so we can make acceleration smooth between multiple
-     * profiles.
-     */
-    void init(Control100 initial, Model100 goal);
-
-    Control100 sample(double timeS);
-
-    double duration();
-
-}
diff --git a/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java b/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java
index 190012da..4327d9a0 100644
--- a/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java
+++ b/lib/src/main/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1.java
@@ -8,7 +8,7 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.LoggerFactory.BooleanLogger;
 import org.team100.lib.logging.LoggerFactory.SetpointsR1Logger;
-import org.team100.lib.profile.incremental.IncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 
diff --git a/lib/src/main/java/org/team100/lib/reference/r1/TimedProfileReferenceR1.java b/lib/src/main/java/org/team100/lib/reference/r1/TimedProfileReferenceR1.java
deleted file mode 100644
index 3116d83a..00000000
--- a/lib/src/main/java/org/team100/lib/reference/r1/TimedProfileReferenceR1.java
+++ /dev/null
@@ -1,69 +0,0 @@
-package org.team100.lib.reference.r1;
-
-import org.team100.lib.coherence.Takt;
-import org.team100.lib.framework.TimedRobot100;
-import org.team100.lib.logging.Level;
-import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.logging.LoggerFactory.BooleanLogger;
-import org.team100.lib.logging.LoggerFactory.SetpointsR1Logger;
-import org.team100.lib.profile.timed.TimedProfile;
-import org.team100.lib.state.Model100;
-
-/**
- * Extracts current and next references from a timed profile.
- */
-public class TimedProfileReferenceR1 implements ProfileReferenceR1 {
-    private final SetpointsR1Logger m_log_setpoints;
-    private final BooleanLogger m_log_done;
-    private final TimedProfile m_profile;
-    private Model100 m_goal;
-    private double m_startTimeS;
-
-    public TimedProfileReferenceR1(LoggerFactory parent, TimedProfile profile) {
-        LoggerFactory log = parent.type(this);
-        m_log_setpoints = log.setpointsR1Logger(Level.TRACE, "setpoints");
-        m_log_done = log.booleanLogger(Level.TRACE, "done");
-        m_profile = profile;
-    }
-
-    @Override
-    public void setGoal(Model100 goal) {
-        m_goal = goal;
-    }
-
-    @Override
-    public void init(Model100 measurement) {
-        if (m_goal == null)
-            throw new IllegalStateException("goal must be set");
-        m_startTimeS = Takt.get();
-        m_profile.init(measurement.control(), m_goal);
-    }
-
-    @Override
-    public SetpointsR1 get() {
-        double progress = progress();
-        SetpointsR1 setpoints = new SetpointsR1(
-                m_profile.sample(progress),
-                m_profile.sample(progress + TimedRobot100.LOOP_PERIOD_S));
-        m_log_setpoints.log(() -> setpoints);
-        return setpoints;
-    }
-
-    @Override
-    public boolean profileDone() {
-        boolean done = progress() >= duration();
-        m_log_done.log(() -> done);
-        return done;
-    }
-
-    //////////////////////////////////////////////////////////////////////////////////////
-
-    private double duration() {
-        return m_profile.duration();
-    }
-
-    private double progress() {
-        return Takt.get() - m_startTimeS;
-    }
-
-}
diff --git a/lib/src/main/java/org/team100/lib/subsystems/five_bar/FiveBarServo.java b/lib/src/main/java/org/team100/lib/subsystems/five_bar/FiveBarServo.java
index e673ce09..4129ba08 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/five_bar/FiveBarServo.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/five_bar/FiveBarServo.java
@@ -9,8 +9,8 @@
 import org.team100.lib.motor.MotorPhase;
 import org.team100.lib.motor.NeutralMode;
 import org.team100.lib.motor.ctre.Falcon6Motor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.ProxyRotaryPositionSensor;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/five_bar/commands/Move.java b/lib/src/main/java/org/team100/lib/subsystems/five_bar/commands/Move.java
index 27689e99..b602d9f4 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/five_bar/commands/Move.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/five_bar/commands/Move.java
@@ -1,6 +1,8 @@
 package org.team100.lib.subsystems.five_bar.commands;
 
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
+import org.team100.lib.framework.TimedRobot100;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.subsystems.five_bar.FiveBarCartesian;
@@ -20,9 +22,12 @@ public class Move extends Command {
 
     private final FiveBarCartesian m_fiveBar;
     private final Translation2d m_goal;
-    private final JerkLimitedTimedProfile m_profile;
+    private final IncrementalProfile m_profile;
     private final Timer m_timer;
 
+    private Control100 m_setpoint;
+    private Model100 m_profileGoal;
+
     private Translation2d m_start;
     private double m_distance;
     private boolean m_done;
@@ -30,7 +35,8 @@ public class Move extends Command {
     public Move(FiveBarCartesian fiveBar, Translation2d goal, double velocity) {
         m_fiveBar = fiveBar;
         m_goal = goal;
-        m_profile = new JerkLimitedTimedProfile(velocity, 1, 10, true);
+        m_profile = new TrapezoidProfileWPI(velocity, 1);
+
         m_timer = new Timer();
         addRequirements(fiveBar);
     }
@@ -39,14 +45,16 @@ public Move(FiveBarCartesian fiveBar, Translation2d goal, double velocity) {
     public void initialize() {
         m_start = m_fiveBar.getPosition();
         m_distance = m_start.getDistance(m_goal);
-        m_profile.init(new Control100(), new Model100(m_distance, 0));
+        m_setpoint = new Control100();
+        m_profileGoal = new Model100(m_distance, 0);
         m_timer.restart();
         m_done = false;
     }
 
     @Override
     public void execute() {
-        Control100 c = m_profile.sample(m_timer.get());
+        m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_profileGoal);
+        Control100 c = m_setpoint;
         double s = c.x() / m_distance;
         Translation2d setpoint = m_start.interpolate(m_goal, s);
         double togo = setpoint.getDistance(m_goal);
diff --git a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommand.java b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommand.java
index 468ff8cc..a6844736 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommand.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommand.java
@@ -1,7 +1,9 @@
 package org.team100.lib.subsystems.lynxmotion_arm;
 
+import org.team100.lib.framework.TimedRobot100;
 import org.team100.lib.geometry.GeometryUtil;
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.util.StrUtil;
@@ -19,9 +21,12 @@ public class MoveCommand extends Command {
     private static final boolean DEBUG = false;
     private final LynxArm m_arm;
     private final Pose3d m_goal;
-    private final JerkLimitedTimedProfile m_profile;
+    private final IncrementalProfile m_profile;
     private final Timer m_timer;
 
+    private Control100 m_setpoint;
+    private Model100 m_profileGoal;
+
     private Pose3d m_start;
     private double m_grip;
     private double m_distance;
@@ -30,7 +35,7 @@ public class MoveCommand extends Command {
     public MoveCommand(LynxArm arm, Pose3d goal, double velocity) {
         m_arm = arm;
         m_goal = goal;
-        m_profile = new JerkLimitedTimedProfile(velocity, 1, 10, true);
+        m_profile = new TrapezoidProfileWPI(velocity, 1);
         m_timer = new Timer();
         addRequirements(arm);
     }
@@ -44,7 +49,8 @@ public void initialize() {
 
         // this doesn't work for twist-only moves without the minimum
         m_distance = Math.max(0.01, m_start.getTranslation().getDistance(m_goal.getTranslation()));
-        m_profile.init(new Control100(), new Model100(m_distance, 0));
+        m_setpoint = new Control100();
+        m_profileGoal = new Model100(m_distance, 0);
         m_timer.restart();
         m_done = false;
         if (DEBUG) {
@@ -60,7 +66,8 @@ public void execute() {
         // the servo doesn't need to be commanded to maintain its position but it's a
         // good habit since some motor types do need this.
         m_arm.setGrip(m_grip);
-        Control100 c = m_profile.sample(m_timer.get());
+        m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_profileGoal);
+        Control100 c = m_setpoint;
         double s = c.x() / m_distance;
         Pose3d setpoint = GeometryUtil.interpolate(m_start, m_goal, s);
         Pose3d measurement = m_arm.getPosition().p6();
diff --git a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommandTwoDof.java b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommandTwoDof.java
index 729271cd..3894542a 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommandTwoDof.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveCommandTwoDof.java
@@ -1,6 +1,8 @@
 package org.team100.lib.subsystems.lynxmotion_arm;
 
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
+import org.team100.lib.framework.TimedRobot100;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 
@@ -11,9 +13,12 @@
 public class MoveCommandTwoDof extends Command {
     private final LynxArmTwoDof m_arm;
     private final Translation2d m_goal;
-    private final JerkLimitedTimedProfile m_profile;
+    private final IncrementalProfile m_profile;
     private final Timer m_timer;
 
+    private Control100 m_setpoint;
+    private Model100 m_profileGoal;
+
     private Translation2d m_start;
     private double m_distance;
     private boolean m_done;
@@ -21,7 +26,7 @@ public class MoveCommandTwoDof extends Command {
     public MoveCommandTwoDof(LynxArmTwoDof arm, Translation2d goal) {
         m_arm = arm;
         m_goal = goal;
-        m_profile = new JerkLimitedTimedProfile(0.1, 1, 10, true);
+        m_profile = new TrapezoidProfileWPI(0.1, 1);
         m_timer = new Timer();
         addRequirements(arm);
     }
@@ -30,14 +35,17 @@ public MoveCommandTwoDof(LynxArmTwoDof arm, Translation2d goal) {
     public void initialize() {
         m_start = m_arm.getPosition().p2();
         m_distance = m_start.getDistance(m_goal);
-        m_profile.init(new Control100(), new Model100(m_distance, 0));
+
+        m_setpoint = new Control100();
+        m_profileGoal = new Model100(m_distance, 0);
         m_timer.restart();
         m_done = false;
     }
 
     @Override
     public void execute() {
-        Control100 c = m_profile.sample(m_timer.get());
+        m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_profileGoal);
+        Control100 c = m_setpoint;
         double s = c.x() / m_distance;
         Translation2d setpoint = m_start.interpolate(m_goal, s);
         double togo = setpoint.getDistance(m_goal);
diff --git a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveXY.java b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveXY.java
index 1e773260..99b7e580 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveXY.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveXY.java
@@ -1,6 +1,8 @@
 package org.team100.lib.subsystems.lynxmotion_arm;
 
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
+import org.team100.lib.framework.TimedRobot100;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 
@@ -12,9 +14,12 @@
 public class MoveXY extends Command {
     private final LynxArm m_arm;
     private final Translation2d m_goal;
-    private final JerkLimitedTimedProfile m_profile;
+    private final IncrementalProfile m_profile;
     private final Timer m_timer;
 
+    private Control100 m_setpoint;
+    private Model100 m_profileGoal;
+
     private Translation2d m_start;
     private double m_grip;
     private double m_distance;
@@ -25,7 +30,7 @@ public MoveXY(
             Translation2d goal) {
         m_arm = arm;
         m_goal = goal;
-        m_profile = new JerkLimitedTimedProfile(0.1, 1, 10, true);
+        m_profile = new TrapezoidProfileWPI(0.1, 1);
         m_timer = new Timer();
         addRequirements(arm);
     }
@@ -34,9 +39,9 @@ public MoveXY(
     public void initialize() {
         m_start = m_arm.getPosition().p6().toPose2d().getTranslation();
         m_grip = m_arm.getGrip();
-
         m_distance = m_start.getDistance(m_goal);
-        m_profile.init(new Control100(), new Model100(m_distance, 0));
+        m_setpoint = new Control100();
+        m_profileGoal = new Model100(m_distance, 0);
         m_timer.restart();
         m_done = false;
     }
@@ -44,7 +49,8 @@ public void initialize() {
     @Override
     public void execute() {
         m_arm.setGrip(m_grip);
-        Control100 c = m_profile.sample(m_timer.get());
+        m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_profileGoal);
+        Control100 c = m_setpoint;
         double s = c.x() / m_distance;
         Translation2d setpoint = m_start.interpolate(m_goal, s);
         double distance = m_goal.getDistance(setpoint);
diff --git a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveZ.java b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveZ.java
index a67d8254..caa7a6b5 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveZ.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/lynxmotion_arm/MoveZ.java
@@ -1,6 +1,8 @@
 package org.team100.lib.subsystems.lynxmotion_arm;
 
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
+import org.team100.lib.framework.TimedRobot100;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 
@@ -11,9 +13,12 @@
 public class MoveZ extends Command {
     private final LynxArm m_arm;
     private final double m_goal;
-    private final JerkLimitedTimedProfile m_profile;
+    private final IncrementalProfile m_profile;
     private final Timer m_timer;
 
+    private Control100 m_setpoint;
+    private Model100 m_profileGoal;
+
     private double m_start;
     private double m_grip;
     private double m_distance;
@@ -22,7 +27,7 @@ public class MoveZ extends Command {
     public MoveZ(LynxArm arm, double goal) {
         m_arm = arm;
         m_goal = goal;
-        m_profile = new JerkLimitedTimedProfile(1, 1, 10, true);
+        m_profile = new TrapezoidProfileWPI(1, 1);
         m_timer = new Timer();
         addRequirements(arm);
     }
@@ -32,7 +37,8 @@ public void initialize() {
         m_start = m_arm.getPosition().p6().getZ();
         m_grip = m_arm.getGrip();
         m_distance = Math.abs(m_start - m_goal);
-        m_profile.init(new Control100(), new Model100(m_distance, 0));
+        m_setpoint = new Control100();
+        m_profileGoal = new Model100(m_distance, 0);
         m_timer.restart();
         m_done = false;
     }
@@ -40,7 +46,8 @@ public void initialize() {
     @Override
     public void execute() {
         m_arm.setGrip(m_grip);
-        Control100 c = m_profile.sample(m_timer.get());
+        m_setpoint = m_profile.calculate(TimedRobot100.LOOP_PERIOD_S, m_setpoint, m_profileGoal);
+        Control100 c = m_setpoint;
         double s = c.x() / m_distance;
         double setpoint = MathUtil.interpolate(m_start, m_goal, s);
 
diff --git a/lib/src/main/java/org/team100/lib/subsystems/prr/commands/FollowJointProfiles.java b/lib/src/main/java/org/team100/lib/subsystems/prr/commands/FollowJointProfiles.java
index bdcb01fc..0347b66a 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/prr/commands/FollowJointProfiles.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/prr/commands/FollowJointProfiles.java
@@ -2,7 +2,7 @@
 
 import org.team100.lib.commands.MoveAndHold;
 import org.team100.lib.framework.TimedRobot100;
-import org.team100.lib.profile.incremental.IncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.subsystems.prr.EAWConfig;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/shooter/IndexerSubsystem.java b/lib/src/main/java/org/team100/lib/subsystems/shooter/IndexerSubsystem.java
index ab767d2c..48a5b245 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/shooter/IndexerSubsystem.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/shooter/IndexerSubsystem.java
@@ -2,7 +2,7 @@
 
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.mechanism.LinearMechanism;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.servo.OutboardLinearPositionServo;
 
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java
index 0da0e721..b88100f3 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithOptionalTargetLock.java
@@ -11,8 +11,8 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger;
 import org.team100.lib.logging.LoggerFactory.DoubleLogger;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.state.ModelSE2;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java
index 6d18804e..1c49eeac 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeading.java
@@ -11,7 +11,7 @@
 import org.team100.lib.logging.LoggerFactory.BooleanLogger;
 import org.team100.lib.logging.LoggerFactory.Control100Logger;
 import org.team100.lib.logging.LoggerFactory.DoubleLogger;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.state.ModelSE2;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java
index 14a5a394..217b752d 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithTargetLock.java
@@ -10,8 +10,8 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger;
 import org.team100.lib.logging.LoggerFactory.DoubleLogger;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.state.ModelSE2;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java
index 29c96c00..c6976295 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/kinodynamics/SwerveKinodynamics.java
@@ -6,8 +6,8 @@
 import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.geometry.VelocitySE2;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.subsystems.swerve.VeeringCorrection;
 import org.team100.lib.subsystems.swerve.module.state.SwerveModuleStates;
 import org.team100.lib.tuning.Mutable;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SimulatedSwerveModule100.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SimulatedSwerveModule100.java
index 687fc25c..d4186933 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SimulatedSwerveModule100.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/SimulatedSwerveModule100.java
@@ -7,7 +7,7 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.CombinedRotaryPositionSensor;
 import org.team100.lib.sensor.position.absolute.ProxyRotaryPositionSensor;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java
index f21c17fa..b278dec6 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/swerve/module/WCPSwerveModule100.java
@@ -11,7 +11,7 @@
 import org.team100.lib.motor.NeutralMode;
 import org.team100.lib.motor.ctre.Falcon6Motor;
 import org.team100.lib.motor.ctre.Kraken6Motor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.CombinedRotaryPositionSensor;
diff --git a/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java b/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java
index 2967d473..85dd8cd5 100644
--- a/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java
+++ b/lib/src/main/java/org/team100/lib/subsystems/turret/Turret.java
@@ -12,8 +12,8 @@
 import org.team100.lib.logging.LoggerFactory.DoubleArrayLogger;
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor;
diff --git a/lib/src/test/java/org/team100/lib/optimization/PoseInterpolationTest.java b/lib/src/test/java/org/team100/lib/optimization/PoseInterpolationTest.java
deleted file mode 100644
index 4896c3c6..00000000
--- a/lib/src/test/java/org/team100/lib/optimization/PoseInterpolationTest.java
+++ /dev/null
@@ -1,111 +0,0 @@
-package org.team100.lib.optimization;
-
-import org.junit.jupiter.api.Test;
-import org.team100.lib.geometry.GeometryUtil;
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
-import org.team100.lib.state.Control100;
-import org.team100.lib.state.Model100;
-import org.team100.lib.util.StrUtil;
-
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.geometry.Translation3d;
-
-public class PoseInterpolationTest {
-    private static final boolean DEBUG = false;
-
-    @Test
-    void test1() {
-        // does WPI pose interpolation make straight lines? no.
-        Pose3d start = new Pose3d();
-        Pose3d end = new Pose3d(1, 1, 0, new Rotation3d(0, 0, -3));
-        double distance = start.getTranslation().getDistance(end.getTranslation());
-        JerkLimitedTimedProfile profile = new JerkLimitedTimedProfile(1, 5, 20, true);
-        profile.init(new Control100(), new Model100(distance, 0));
-        int i = 0;
-        for (double t = 0; t < 2; t += 0.02) {
-            Control100 c = profile.sample(t);
-            double s = c.x() / distance;
-            Pose3d setpoint = start.interpolate(end, s);
-            if (DEBUG)
-                System.out.printf("%d, %.8e, %.8e, %s\n", i, t, s, StrUtil.poseStr(setpoint));
-            ++i;
-        }
-    }
-
-    @Test
-    void test1b() {
-        // does my pose interpolation make straight lines? yes. :-)
-        Pose3d start = new Pose3d();
-        Pose3d end = new Pose3d(1, 1, 0, new Rotation3d(0, 0, -3));
-        double distance = start.getTranslation().getDistance(end.getTranslation());
-        JerkLimitedTimedProfile profile = new JerkLimitedTimedProfile(1, 5, 20, true);
-        profile.init(new Control100(), new Model100(distance, 0));
-        int i = 0;
-        for (double t = 0; t < 2; t += 0.02) {
-            Control100 c = profile.sample(t);
-            double s = c.x() / distance;
-            Pose3d setpoint = GeometryUtil.interpolate(start, end, s);
-            if (DEBUG)
-                System.out.printf("%d, %.8e, %.8e, %s\n", i, t, s, StrUtil.poseStr(setpoint));
-            ++i;
-        }
-    }
-
-    @Test
-    void test2() {
-        // does WPI rotation interpolation make straight lines? no.
-        Rotation3d start = new Rotation3d();
-        Rotation3d end = new Rotation3d(0, 1, -3);
-        double distance = 1.5;
-        JerkLimitedTimedProfile profile = new JerkLimitedTimedProfile(1, 5, 20, true);
-        profile.init(new Control100(), new Model100(distance, 0));
-        int i = 0;
-        for (double t = 0; t < 2; t += 0.02) {
-            Control100 c = profile.sample(t);
-            double s = c.x() / distance;
-            Rotation3d setpoint = start.interpolate(end, s);
-            if (DEBUG)
-                System.out.printf("%d, %.8e, %.8e, %s\n", i, t, s, StrUtil.rotStr(setpoint));
-            ++i;
-        }
-    }
-
-    @Test
-    void test2b() {
-        // does my rotation interpolation make straight lines? yes. :-)
-        Rotation3d start = new Rotation3d();
-        Rotation3d end = new Rotation3d(0, 1, -3);
-        double distance = 1.5;
-        JerkLimitedTimedProfile profile = new JerkLimitedTimedProfile(1, 5, 20, true);
-        profile.init(new Control100(), new Model100(distance, 0));
-        int i = 0;
-        for (double t = 0; t < 2; t += 0.02) {
-            Control100 c = profile.sample(t);
-            double s = c.x() / distance;
-            Rotation3d setpoint = GeometryUtil.interpolate(start, end, s);
-            if (DEBUG)
-                System.out.printf("%d, %.8e, %.8e, %s\n", i, t, s, StrUtil.rotStr(setpoint));
-            ++i;
-        }
-    }
-
-    @Test
-    void test3() {
-        // does translation interpolation make straight lines? yes.
-        Translation3d start = new Translation3d();
-        Translation3d end = new Translation3d(0, 1, -3);
-        double distance = 1.5;
-        JerkLimitedTimedProfile profile = new JerkLimitedTimedProfile(1, 5, 20, true);
-        profile.init(new Control100(), new Model100(distance, 0));
-        int i = 0;
-        for (double t = 0; t < 2; t += 0.02) {
-            Control100 c = profile.sample(t);
-            double s = c.x() / distance;
-            Translation3d setpoint = start.interpolate(end, s);
-            if (DEBUG)
-                System.out.printf("%d, %.8e, %.8e, %s\n", i, t, s, StrUtil.transStr(setpoint));
-            ++i;
-        }
-    }
-}
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/CompleteProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/CompleteProfileTest.java
similarity index 99%
rename from lib/src/test/java/org/team100/lib/profile/incremental/CompleteProfileTest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/CompleteProfileTest.java
index 7441934e..70c05240 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/CompleteProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/CompleteProfileTest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertThrows;
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/CoordinatedProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/CoordinatedProfileTest.java
similarity index 99%
rename from lib/src/test/java/org/team100/lib/profile/incremental/CoordinatedProfileTest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/CoordinatedProfileTest.java
index 8b8c5054..45de8ffd 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/CoordinatedProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/CoordinatedProfileTest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/CurrentLimitedExponentialProfileTest.java
similarity index 98%
rename from lib/src/test/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfileTest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/CurrentLimitedExponentialProfileTest.java
index d86ac228..623cd81e 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/CurrentLimitedExponentialProfileTest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/ExponentialProfileWPITest.java b/lib/src/test/java/org/team100/lib/profile/r1/ExponentialProfileWPITest.java
similarity index 97%
rename from lib/src/test/java/org/team100/lib/profile/incremental/ExponentialProfileWPITest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/ExponentialProfileWPITest.java
index 379dc3e4..2443fd92 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/ExponentialProfileWPITest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/ExponentialProfileWPITest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/IncrementalProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java
similarity index 98%
rename from lib/src/test/java/org/team100/lib/profile/incremental/IncrementalProfileTest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java
index dc24ddf7..21f1fb5f 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/IncrementalProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/IncrementalProfileTest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/MockIncrementalProfile.java b/lib/src/test/java/org/team100/lib/profile/r1/MockIncrementalProfile.java
similarity index 93%
rename from lib/src/test/java/org/team100/lib/profile/incremental/MockIncrementalProfile.java
rename to lib/src/test/java/org/team100/lib/profile/r1/MockIncrementalProfile.java
index 61fd6c3b..9661cf42 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/MockIncrementalProfile.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/MockIncrementalProfile.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfileTest.java b/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java
similarity index 99%
rename from lib/src/test/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfileTest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java
index e713ecd7..d237b75c 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidIncrementalProfileTest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotEquals;
diff --git a/lib/src/test/java/org/team100/lib/profile/incremental/TrapezoidProfileWPITest.java b/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java
similarity index 98%
rename from lib/src/test/java/org/team100/lib/profile/incremental/TrapezoidProfileWPITest.java
rename to lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java
index 468ba030..ad54d15b 100644
--- a/lib/src/test/java/org/team100/lib/profile/incremental/TrapezoidProfileWPITest.java
+++ b/lib/src/test/java/org/team100/lib/profile/r1/TrapezoidProfileWPITest.java
@@ -1,4 +1,4 @@
-package org.team100.lib.profile.incremental;
+package org.team100.lib.profile.r1;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/DynamicProfileGeneratorTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/DynamicProfileGeneratorTest.java
deleted file mode 100644
index 936f5cf4..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/DynamicProfileGeneratorTest.java
+++ /dev/null
@@ -1,129 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-public class DynamicProfileGeneratorTest {
-    private static final double DELTA = 0.001;
-
-    @Test
-    void testGenerateMotionProfile() {
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionState goal = new MotionState(5, 0, 0, 0);
-
-        double resolution = 1;
-        MotionProfile p = DynamicProfileGenerator.generateMotionProfile(
-                start,
-                goal,
-                (s) -> 1,
-                (s) -> 1,
-                resolution);
-
-        assertEquals(7, p.getSegments().size());
-        assertEquals(0, p.get(0).x(), DELTA);
-        assertEquals(0.5, p.get(1).x(), DELTA);
-        assertEquals(1.5, p.get(2).x(), DELTA);
-        assertEquals(2.5, p.get(3).x(), DELTA);
-        assertEquals(3.5, p.get(4).x(), DELTA);
-        assertEquals(4.5, p.get(5).x(), DELTA);
-        assertEquals(5.0, p.get(6).x(), DELTA);
-
-        assertEquals(6.0, p.duration(), DELTA);
-
-        MotionState s0 = p.get(0);
-        assertEquals(0, s0.v(), DELTA);
-
-        MotionProfile p1 = p.append(p);
-        assertEquals(12, p1.duration(), DELTA);
-
-        MotionState s1 = p.get(1);
-        assertEquals(1, s1.v(), DELTA);
-    }
-
-    @Test
-    void testEvolve() {
-        {
-            // this is a bit nonsensical, this state would never move.
-            MotionState s0 = new MotionState(0, 0, 0, 0);
-            MotionState s1 = DynamicProfileGenerator.evolve(s0, 1);
-            assertEquals(1, s1.x(), DELTA);
-            assertEquals(0, s1.v(), DELTA);
-            assertEquals(0, s1.a(), DELTA);
-            assertEquals(0, s1.j(), DELTA);
-        }
-        {
-            // nonzero v, zero a, end v is the same
-            MotionState s0 = new MotionState(0, 1, 0, 0);
-            MotionState s1 = DynamicProfileGenerator.evolve(s0, 1);
-            assertEquals(1, s1.x(), DELTA);
-            assertEquals(1, s1.v(), DELTA);
-            assertEquals(0, s1.a(), DELTA);
-            assertEquals(0, s1.j(), DELTA);
-        }
-        {
-            // nonzero a, end v is more
-            MotionState s0 = new MotionState(0, 0, 1, 0);
-            MotionState s1 = DynamicProfileGenerator.evolve(s0, 1);
-            assertEquals(1, s1.x(), DELTA);
-            assertEquals(1.414, s1.v(), DELTA);
-            assertEquals(1, s1.a(), DELTA);
-            assertEquals(0, s1.j(), DELTA);
-        }
-        {
-            // v is low, a is negative
-            MotionState s0 = new MotionState(0, 0.1, -10, 0);
-            assertThrows(IllegalArgumentException.class, () -> DynamicProfileGenerator.evolve(s0, 1));
-        }
-    }
-
-    @Test
-    void testComputeForward() {
-        MotionState start = new MotionState(1, 0, 0, 0);
-        int size = 3;
-        double step = 0.3;
-        List<MotionSpan> forwardStates = DynamicProfileGenerator.computeForward(
-                start, (s) -> 2.0, (s) -> 6.0, step, size);
-        assertEquals(4, forwardStates.size());
-        // position shifted to start
-        verify(forwardStates.get(0), 1, 0, 6, 0, 0.3); // full-length segment at max A
-        verify(forwardStates.get(1), 1.3, 1.897, 6, 0, 0.033); // short segment to max V
-        verify(forwardStates.get(2), 1.333, 2, 0, 0, 0.267);
-        verify(forwardStates.get(3), 1.6, 2, 0, 0, 0.3);
-    }
-
-    @Test
-    void testComputeBackwards() {
-        MotionState goal = new MotionState(1.9, 0, 0, 0);
-        int size = 3;
-        double step = 0.3;
-        List<MotionSpan> backwardStates = DynamicProfileGenerator.computeBackward(
-                goal, (s) -> 2.0, (s) -> 6.0, step, size);
-        assertEquals(4, backwardStates.size());
-        verify(backwardStates.get(0), 1, 2, 0, 0, 0.3);
-        verify(backwardStates.get(1), 1.3, 2, 0, 0, 0.267);
-        verify(backwardStates.get(2), 1.567, 2, -6, 0, 0.033);
-        verify(backwardStates.get(3), 1.6, 1.897, -6, 0, 0.3);
-    }
-
-    @Test
-    void testIntersection() {
-        MotionState s0 = new MotionState(0, 0, 1, 0);
-        MotionState s1 = new MotionState(0, 0.5, 0.5, 0);
-        double c = DynamicProfileGenerator.intersection(s0, s1);
-        assertEquals(0.25, c, DELTA);
-    }
-
-    void verify(MotionSpan p, double x, double v, double a, double j, double dx) {
-        assertEquals(x, p.start().x(), DELTA);
-        assertEquals(v, p.start().v(), DELTA);
-        assertEquals(a, p.start().a(), DELTA);
-        assertEquals(j, p.start().j(), DELTA);
-        assertEquals(dx, p.dx(), DELTA);
-
-    }
-
-}
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGeneratorTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGeneratorTest.java
deleted file mode 100644
index 280cd448..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/JerkLimitedProfileGeneratorTest.java
+++ /dev/null
@@ -1,131 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-// passes uncommented
-// import com.acmerobotics.roadrunner.profile.MotionProfile;
-// import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
-// import com.acmerobotics.roadrunner.profile.MotionState;
-// import com.acmerobotics.roadrunner.profile.AccelerationConstraint;
-// import com.acmerobotics.roadrunner.profile.VelocityConstraint;
-
-import org.junit.jupiter.api.Test;
-
-public class JerkLimitedProfileGeneratorTest {
-    private static final boolean DEBUG = false;
-    private static final double DELTA = 0.001;
-
-    /**
-     * see
-     * https://docs.google.com/spreadsheets/d/19WbkNaxcRGHwYwLH1pu9ER3qxZrsYqDlZTdV-cmOM0I
-     * 
-     */
-    @Test
-    void testSample() {
-        // an example from 0 to 1, with constraints on derivatives at the ends and along
-        // the path.
-        // see Spline1dTest.testSample()
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionState goal = new MotionState(1, 0, 0, 0);
-        double maxVel = 2;
-        double maxAccel = 6;
-        double maxJerk = 20;
-        boolean overshoot = false;
-        MotionProfile p = JerkLimitedProfileGenerator.generateMotionProfile(start, goal, maxVel, maxAccel,
-                maxJerk,
-                overshoot);
-        for (double t = 0; t < p.duration(); t += 0.01) {
-            MotionState state = p.get(t);
-            if (DEBUG) {
-                double x = state.x();
-                double v = state.v();
-                double a = state.a();
-                double j = state.j();
-                System.out.printf("%8.3f %8.3f %8.3f %8.3f %8.3f\n",
-                        t, x, v, a, j);
-            }
-        }
-    }
-
-    @Test
-    void testMovingEntry() {
-        // an example from 0 to 1, with constraints on derivatives at the ends and along
-        // the path.
-        // see Spline1dTest.testSample()
-        MotionState start = new MotionState(0, 1, 0, 0);
-        MotionState goal = new MotionState(1, 0, 0, 0);
-        double maxVel = 1;
-        double maxAccel = 0.1;
-        double maxJerk = 100;
-        boolean overshoot = false;
-        MotionProfile p = JerkLimitedProfileGenerator.generateMotionProfile(start, goal, maxVel, maxAccel,
-                maxJerk,
-                overshoot);
-        for (double t = 0; t < p.duration(); t += 0.01) {
-            MotionState state = p.get(t);
-            if (DEBUG) {
-                double x = state.x();
-                double v = state.v();
-                double a = state.a();
-                double j = state.j();
-                System.out.printf("%8.3f %8.3f %8.3f %8.3f %8.3f\n",
-                        t, x, v, a, j);
-            }
-        }
-    }
-
-    @Test
-    void testGenerateSimpleMotionProfile() {
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionState goal = new MotionState(10, 0, 0, 0);
-        double maxVel = 1;
-        double maxAccel = 1;
-        double maxJerk = 1;
-        boolean overshoot = false;
-        MotionProfile p = JerkLimitedProfileGenerator.generateMotionProfile(start, goal, maxVel, maxAccel,
-                maxJerk,
-                overshoot);
-
-        assertEquals(12, p.duration(), DELTA);
-
-        MotionState s0 = p.get(0);
-        assertEquals(0, s0.v(), DELTA);
-
-        MotionProfile p1 = p.append(p);
-        assertEquals(24, p1.duration(), DELTA);
-
-        MotionState s1 = p.get(1);
-        assertEquals(0.5, s1.v(), DELTA);
-    }
-
-    @Test
-    public void testSampleCount() {
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionState goal = new MotionState(5, 0, 0, 0);
-        double resolution = 1;
-        double length = goal.x() - start.x();
-        int samples = Math.max(2, (int) Math.ceil(length / resolution));
-        assertEquals(5, samples);
-
-    }
-
-
-    @Test
-    void testGenerateAccelProfile() {
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionProfile p = JerkLimitedProfileGenerator.generateAccelProfile(start, 2, 6, 20);
-        // end-state accel is zero
-        assertEquals(0.000, p.get(p.duration()).a(), 0.001);
-        for (double t = 0; t < p.duration(); t += 0.01) {
-            MotionState state = p.get(t);
-            if (DEBUG) {
-                double x = state.x();
-                double v = state.v();
-                double a = state.a();
-                double j = state.j();
-                System.out.printf("%8.3f %8.3f %8.3f %8.3f %8.3f\n",
-                        t, x, v, a, j);
-            }
-        }
-    }
-}
\ No newline at end of file
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileBuilderTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileBuilderTest.java
deleted file mode 100644
index fa7bfcee..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileBuilderTest.java
+++ /dev/null
@@ -1,35 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-// passes uncommented
-// import com.acmerobotics.roadrunner.profile.MotionProfile;
-// import com.acmerobotics.roadrunner.profile.MotionProfileBuilder;
-// import com.acmerobotics.roadrunner.profile.MotionState;
-
-import org.junit.jupiter.api.Test;
-
-public class MotionProfileBuilderTest {
-    private static final double DELTA = 0.001;
-
-    @Test
-    void testBasic() {
-        MotionState s = new MotionState(0, 1, 0, 0);
-        MotionProfileBuilder b = new MotionProfileBuilder(s);
-        b.appendJerkSegment(1, 1);
-        
-        MotionProfile p = b.build();
-        assertEquals(1, p.duration(), DELTA);
-
-        MotionState s0 = p.get(0);
-        assertEquals(1, s0.v(), DELTA);
-
-        MotionProfile p1 = p.append(p);
-        assertEquals(2, p1.duration(), DELTA);
-
-        MotionState s1 = p.get(1);
-        assertEquals(1.5, s1.v(), DELTA);
-
-    }
-    
-}
\ No newline at end of file
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfilePerformanceTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfilePerformanceTest.java
deleted file mode 100644
index 7ab55cff..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfilePerformanceTest.java
+++ /dev/null
@@ -1,32 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import static org.junit.jupiter.api.Assertions.assertNotNull;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.Timer;
-
-public class MotionProfilePerformanceTest {
-    @Test
-    void testLotsOfThem() {
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionState goal = new MotionState(1, 0, 0, 0);
-
-        Timer timer = new Timer();
-        timer.start();
-        int count = 1000;
-        for (int i = 0; i < count; ++i) {
-            MotionProfile profile = JerkLimitedProfileGenerator.generateMotionProfile(
-                    start, goal, 1, 1, 1, false);
-            assertNotNull(profile);
-        }
-        timer.stop();
-        double timeS = timer.get();
-        double timePerProfileMs = 1000 * timeS / count;
-        // for slow machines this might be too low; it's ok to raise it a little
-        // but if profiles are very slow that would be bad.
-        assertTrue(timePerProfileMs < 0.2);
-    }
-
-}
\ No newline at end of file
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileTest.java
deleted file mode 100644
index 665262cb..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionProfileTest.java
+++ /dev/null
@@ -1,45 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import java.util.ArrayList;
-import java.util.List;
-
-// passes uncommented
-// import com.acmerobotics.roadrunner.profile.MotionProfile;
-// import com.acmerobotics.roadrunner.profile.MotionState;
-// import com.acmerobotics.roadrunner.profile.MotionSegment;
-
-import org.junit.jupiter.api.Test;
-
-public class MotionProfileTest {
-    private static final double DELTA = 0.001;
-
-    @Test
-    void testBasic() {
-        List<MotionSegment> segments = new ArrayList<MotionSegment>();
-        segments.add(new MotionSegment(new MotionState(0, 1, 0, 0), 1));
-        MotionProfile p = new MotionProfile(segments);
-        assertEquals(1, p.duration(), DELTA);
-
-        MotionState s0 = p.get(0);
-        assertEquals(1, s0.v(), DELTA);
-
-        MotionProfile p1 = p.append(p);
-        assertEquals(2, p1.duration(), DELTA);
-
-        MotionState s1 = p.get(1);
-        assertEquals(1, s1.v(), DELTA);
-
-    }
-
-    @Test
-    void testReverse() {
-        List<MotionSegment> segments = new ArrayList<MotionSegment>();
-        segments.add(new MotionSegment(new MotionState(0, 1, 0, 0), 1));
-        MotionProfile p = new MotionProfile(segments);
-        MotionProfile b = p.reversed();
-        assertEquals(1, b.duration(), DELTA);
-
-    }
-}
\ No newline at end of file
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionSegmentTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionSegmentTest.java
deleted file mode 100644
index 13b7ffda..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionSegmentTest.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import org.junit.jupiter.api.Test;
-
-// passes uncommented
-// import com.acmerobotics.roadrunner.profile.MotionSegment;
-// import com.acmerobotics.roadrunner.profile.MotionState;
-
-
-public class MotionSegmentTest {
-    private static final double DELTA = 0.001;
-
-    @Test
-    void testBasic() {
-        MotionSegment s = new MotionSegment(new MotionState(0, 1, 0, 0), 1);
-        MotionState s1 = s.get(0.5);
-        assertEquals(0.5, s1.x(), DELTA);
-        assertEquals(1, s1.v(), DELTA);
-    }
-}
\ No newline at end of file
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionStateTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionStateTest.java
deleted file mode 100644
index 877edde4..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/MotionStateTest.java
+++ /dev/null
@@ -1,39 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-// passes uncommented
-// import com.acmerobotics.roadrunner.profile.MotionState;
-
-import org.junit.jupiter.api.Test;
-
-public class MotionStateTest {
-    private static final double DELTA = 0.001;
-
-    @Test
-    void testBasic() {
-        MotionState s = new MotionState(1, 0, 0, 0);
-        MotionState s1 = s.get(1);
-        assertEquals(1, s1.x(), DELTA);
-        assertEquals(0, s1.v(), DELTA);
-    }
-
-    @Test
-    void testV() {
-        MotionState s = new MotionState(0, 1, 0, 0);
-        MotionState s1 = s.get(1);
-        assertEquals(1, s1.x(), DELTA);
-        assertEquals(1, s1.v(), DELTA);
-    }
-
-    @Test
-    void testA() {
-        MotionState s = new MotionState(0, 0, 1, 0);
-        MotionState s1 = s.get(1);
-        assertEquals(0.5, s1.x(), DELTA);
-        assertEquals(1, s1.v(), DELTA);
-        assertEquals(1, s1.a(), DELTA);
-
-    }
-}
\ No newline at end of file
diff --git a/lib/src/test/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGeneratorTest.java b/lib/src/test/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGeneratorTest.java
deleted file mode 100644
index a3078a19..00000000
--- a/lib/src/test/java/org/team100/lib/profile/roadrunner/TrapezoidProfileGeneratorTest.java
+++ /dev/null
@@ -1,28 +0,0 @@
-package org.team100.lib.profile.roadrunner;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import org.junit.jupiter.api.Test;
-
-public class TrapezoidProfileGeneratorTest {
-    private static final boolean DEBUG = false;
-    private static final double DELTA = 0.001;
-
-    @Test
-    void testUnlimitedJerk() {
-        MotionState start = new MotionState(0, 0, 0, 0);
-        MotionProfile p = TrapezoidProfileGenerator.unlimitedJerk(start, 2, 6);
-        // end-state accel is max (because the transition to cruise can be sharp in the
-        // non-jerk-limited case.
-        assertEquals(6.000, p.get(p.duration()).a(), DELTA);
-        for (double t = 0; t < p.duration(); t += 0.01) {
-            MotionState state = p.get(t);
-            double x = state.x();
-            double v = state.v();
-            double a = state.a();
-            double j = state.j();
-            if (DEBUG)
-                System.out.printf("%8.3f %8.3f %8.3f %8.3f %8.3f\n", t, x, v, a, j);
-        }
-    }
-}
diff --git a/lib/src/test/java/org/team100/lib/profile/timed/JerkLimitedTimedProfileTest.java b/lib/src/test/java/org/team100/lib/profile/timed/JerkLimitedTimedProfileTest.java
deleted file mode 100644
index ed93dabb..00000000
--- a/lib/src/test/java/org/team100/lib/profile/timed/JerkLimitedTimedProfileTest.java
+++ /dev/null
@@ -1,21 +0,0 @@
-package org.team100.lib.profile.timed;
-
-import org.junit.jupiter.api.Test;
-import org.team100.lib.state.Control100;
-import org.team100.lib.state.Model100;
-
-public class JerkLimitedTimedProfileTest {
-    private static final boolean DEBUG = false;
-
-    @Test
-    void testSimple() {
-        JerkLimitedTimedProfile p = new JerkLimitedTimedProfile(2, 6, 25, false);
-        p.init(new Control100(), new Model100(1, 0));
-        for (double t = 0; t < 2; t += 0.01) {
-            Control100 c = p.sample(t);
-            if (DEBUG)
-                System.out.printf("%12.3f %12.3f %12.3f %12.3f\n", t, c.x(), c.v(), c.a());
-        }
-
-    }
-}
diff --git a/lib/src/test/java/org/team100/lib/profile/timed/TaperedProfileTest.java b/lib/src/test/java/org/team100/lib/profile/timed/TaperedProfileTest.java
deleted file mode 100644
index 8a142389..00000000
--- a/lib/src/test/java/org/team100/lib/profile/timed/TaperedProfileTest.java
+++ /dev/null
@@ -1,61 +0,0 @@
-package org.team100.lib.profile.timed;
-
-import org.junit.jupiter.api.Test;
-import org.team100.lib.state.Control100;
-import org.team100.lib.state.Model100;
-
-public class TaperedProfileTest {
-    private static final boolean DEBUG = false;
-
-    /**
-     * see
-     * https://docs.google.com/spreadsheets/d/1VKc6t_AHfW9Ovo8R1cov4UfGLI1Ab3QKrIuhZbfsCR8/edit?gid=297712153#gid=297712153
-     */
-    @Test
-    void testSimple() {
-        // use a higher resolution to make a nice looking graph.
-        TaperedProfile p = new TaperedProfile(2, 6, 20, 0.01);
-        p.init(new Control100(), new Model100(1, 0));
-        for (double t = 0; t < 2; t += 0.01) {
-            Control100 c = p.sample(t);
-            if (DEBUG)
-                System.out.printf("%12.3f %12.3f %12.3f %12.3f\n", t, c.x(), c.v(), c.a());
-        }
-    }
-
-    /** make sure it also works in the opposite direction :-) */
-    @Test
-    void testNegative() {
-        TaperedProfile p = new TaperedProfile(2, 6, 20, 0.01);
-        p.init(new Control100(1, 0), new Model100());
-        for (double t = 0; t < 2; t += 0.01) {
-            Control100 c = p.sample(t);
-            if (DEBUG)
-                System.out.printf("%12.3f %12.3f %12.3f %12.3f\n", t, c.x(), c.v(), c.a());
-        }
-    }
-
-    /** This case works, but other cases don't work. */
-    @Test
-    void testMovingEntry() {
-        TaperedProfile p = new TaperedProfile(2, 6, 20, 0.01);
-        p.init(new Control100(1, 1), new Model100());
-        for (double t = 0; t < 2; t += 0.01) {
-            Control100 c = p.sample(t);
-            if (DEBUG)
-                System.out.printf("%12.3f %12.3f %12.3f %12.3f\n", t, c.x(), c.v(), c.a());
-        }
-    }
-
-    /** This doesn't work: velocity is immediately clamped to the goal path. */
-    @Test
-    void testOvershoot() {
-        TaperedProfile p = new TaperedProfile(5, 1, 20, 0.01);
-        p.init(new Control100(-1, 5), new Model100());
-        for (double t = 0; t < 2; t += 0.01) {
-            Control100 c = p.sample(t);
-            if (DEBUG)
-                System.out.printf("%12.3f %12.3f %12.3f %12.3f\n", t, c.x(), c.v(), c.a());
-        }
-    }
-}
diff --git a/lib/src/test/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1Test.java b/lib/src/test/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1Test.java
index 1cdc91fe..2bbc7b34 100644
--- a/lib/src/test/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1Test.java
+++ b/lib/src/test/java/org/team100/lib/reference/r1/IncrementalProfileReferenceR1Test.java
@@ -6,7 +6,7 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.incremental.TrapezoidProfileWPI;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.state.Model100;
 import org.team100.lib.testing.Timeless;
 
diff --git a/lib/src/test/java/org/team100/lib/reference/r1/TimedProfileReference1dTest.java b/lib/src/test/java/org/team100/lib/reference/r1/TimedProfileReference1dTest.java
deleted file mode 100644
index 13b67814..00000000
--- a/lib/src/test/java/org/team100/lib/reference/r1/TimedProfileReference1dTest.java
+++ /dev/null
@@ -1,57 +0,0 @@
-package org.team100.lib.reference.r1;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-import org.junit.jupiter.api.Test;
-import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.logging.TestLoggerFactory;
-import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.timed.JerkLimitedTimedProfile;
-import org.team100.lib.state.Model100;
-import org.team100.lib.testing.Timeless;
-
-public class TimedProfileReference1dTest implements Timeless {
-    private static final double DELTA = 0.001;
-    private static final LoggerFactory log = new TestLoggerFactory(new TestPrimitiveLogger());
-
-    @Test
-    void testSimple() {
-        JerkLimitedTimedProfile p = new JerkLimitedTimedProfile(2, 6, 25, false);
-        Model100 goal = new Model100(1, 0);
-        ProfileReferenceR1 ref = new TimedProfileReferenceR1(log, p);
-        ref.setGoal(goal);
-        Model100 measurement = new Model100();
-        ref.init(measurement);
-
-        // initial current setpoint is the measurement.
-        SetpointsR1 s = ref.get();
-        assertEquals(0, s.current().x(), DELTA);
-        assertEquals(0, s.current().v(), DELTA);
-        assertEquals(0, s.current().a(), DELTA);
-        assertEquals(0.00003, s.next().x(), DELTA);
-        assertEquals(0.005, s.next().v(), DELTA);
-        assertEquals(0.500, s.next().a(), DELTA);
-
-        // if time does not pass, nothing changes.
-        s = ref.get();
-        assertEquals(0, s.current().x(), DELTA);
-        assertEquals(0, s.current().v(), DELTA);
-        assertEquals(0, s.current().a(), DELTA);
-        assertEquals(0.00003, s.next().x(), DELTA);
-        assertEquals(0.005, s.next().v(), DELTA);
-        assertEquals(0.500, s.next().a(), DELTA);
-
-        stepTime();
-
-        // now the setpoint has advanced
-        s = ref.get();
-        assertEquals(0, s.current().x(), DELTA);
-        assertEquals(0.005, s.current().v(), DELTA);
-        assertEquals(0.500, s.current().a(), DELTA);
-        assertEquals(0.00026, s.next().x(), DELTA);
-        assertEquals(0.020, s.next().v(), DELTA);
-        assertEquals(1.000, s.next().a(), DELTA);
-
-    }
-
-}
diff --git a/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java b/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java
index 907d1ef3..a602f023 100644
--- a/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/servo/AnglePositionServoProfileTest.java
@@ -11,8 +11,8 @@
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.MockBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.MockRotaryPositionSensor;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java b/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java
index 5b934c3d..967f3a08 100644
--- a/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java
+++ b/lib/src/test/java/org/team100/lib/servo/AngularPositionProfileTest.java
@@ -12,9 +12,9 @@
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.MockBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidProfileWPI;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidProfileWPI;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.MockRotaryPositionSensor;
 import org.team100.lib.testing.Timeless;
diff --git a/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java b/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java
index bf55197d..38dca045 100644
--- a/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java
+++ b/lib/src/test/java/org/team100/lib/servo/GravityServoTest.java
@@ -15,8 +15,8 @@
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.MockBareMotor;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.MockProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
diff --git a/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java b/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java
index e16260b2..a5b483cc 100644
--- a/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java
+++ b/lib/src/test/java/org/team100/lib/servo/OnboardAngularPositionServoTest.java
@@ -12,8 +12,8 @@
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.MockBareMotor;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.sensor.position.absolute.MockRotaryPositionSensor;
 import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor;
diff --git a/lib/src/test/java/org/team100/lib/servo/OnboardLinearDutyCyclePositionServoTest.java b/lib/src/test/java/org/team100/lib/servo/OnboardLinearDutyCyclePositionServoTest.java
index 89e7506b..557589c3 100644
--- a/lib/src/test/java/org/team100/lib/servo/OnboardLinearDutyCyclePositionServoTest.java
+++ b/lib/src/test/java/org/team100/lib/servo/OnboardLinearDutyCyclePositionServoTest.java
@@ -8,8 +8,8 @@
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
 import org.team100.lib.mechanism.LinearMechanism;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder;
 import org.team100.lib.testing.Timeless;
diff --git a/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java b/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java
index b956d2a3..08183d9c 100644
--- a/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java
+++ b/lib/src/test/java/org/team100/lib/servo/OutboardAngularPositionServoTest.java
@@ -11,8 +11,8 @@
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.MockBareMotor;
 import org.team100.lib.motor.sim.SimulatedBareMotor;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
 import org.team100.lib.reference.r1.MockProfileReferenceR1;
 import org.team100.lib.reference.r1.ProfileReferenceR1;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java
index a33d9901..db17837b 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithFullStateHeadingTest.java
@@ -14,8 +14,8 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.sensor.gyro.MockGyro;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java
index a1129680..d75eb38a 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/commands/manual/ManualWithProfiledHeadingTest.java
@@ -16,7 +16,7 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.sensor.gyro.MockGyro;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
diff --git a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java
index 509f7ef0..ecc5740a 100644
--- a/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java
+++ b/lib/src/test/java/org/team100/lib/subsystems/swerve/kinodynamics/limiter/SwerveLimiterTest.java
@@ -9,8 +9,8 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.logging.TestLoggerFactory;
 import org.team100.lib.logging.primitive.TestPrimitiveLogger;
-import org.team100.lib.profile.incremental.IncrementalProfile;
-import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;