fiddling with trajectories, removed timed profiles

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;
 

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;

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;

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;

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;

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;

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;

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

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);

lib/src/main/java/org/team100/lib/geometry/Pose2dWithMotion.java → lib/src/main/java/org/team100/lib/geometry/PathPoint.java

@@ -1,14 +1,23 @@
 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;
 
-/** 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;
+    /** 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. */
@@ -19,16 +28,31 @@ 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,
+            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 WaypointSE2 getPose() {
+    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;
+    }
+
+    public WaypointSE2 waypoint() {
         return m_waypoint;
     }
 
@@ -46,14 +70,58 @@ 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 Pose2dWithMotion interpolate(Pose2dWithMotion other, double x) {
-        return new Pose2dWithMotion(
+    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 {
+            // 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
+                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(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
+        System.out.println("WARNING: no spline, using linear interpolation ");
+        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));
     }
@@ -65,18 +133,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");

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;
@@ -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);
@@ -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 {

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

lib/src/main/java/org/team100/lib/profile/incremental/CompleteProfile.java → 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;

lib/src/main/java/org/team100/lib/profile/incremental/CurrentLimitedExponentialProfile.java → 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;

lib/src/main/java/org/team100/lib/profile/incremental/ExponentialProfileWPI.java → 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;

lib/src/main/java/org/team100/lib/profile/incremental/IncrementalProfile.java → 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;

lib/src/main/java/org/team100/lib/profile/incremental/NullProfile.java → 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;

lib/src/main/java/org/team100/lib/profile/incremental/README.md → 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.

lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidIncrementalProfile.java → 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;

lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidProfileWPI.java → 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;