[choreolib] Remove jerk from trajectory sample interpolation

Fixes #1295.

Author: calcmogul

choreolib/py/choreo/trajectory/__init__.py

@@ -168,13 +168,8 @@ def f(state, input):
             α = (ar - al) / width
             return [v * cos(θ), v * sin(θ), ω, al, ar, α]
 
-        τ = t - self.timestamp
         sample = solve_ivp(f, (self.timestamp, t), initial_state).y
 
-        dt = end_value.timestamp - self.timestamp
-        jl = (end_value.al - self.al) / dt
-        jr = (end_value.ar - self.ar) / dt
-
         return DifferentialSample(
             t,
             sample[0, 0],
@@ -183,8 +178,8 @@ def f(state, input):
             sample[3, 0],
             sample[4, 0],
             sample[5, 0],
-            self.al + jl * τ,
-            self.ar + jr * τ,
+            self.al,
+            self.ar,
             lerp(self.fl, end_value.fl, scale),
             lerp(self.fr, end_value.fr, scale),
         )
@@ -463,30 +458,22 @@ def interpolate(self, end_value: SwerveSample, t: float) -> SwerveSample:
         #   Δt = tₖ₊₁ − tₖ
         #   τ = timestamp − tₖ
         #
-        #   x(τ) = xₖ + vₖτ + 1/2 aₖτ² + 1/6 jₖτ³
-        #   v(τ) = vₖ + aₖτ + 1/2 jₖτ²
-        #   a(τ) = aₖ + jₖτ
-        #
-        # where jₖ = (aₖ₊₁ − aₖ)/Δt
-        dt = end_value.timestamp - t
+        #   x(τ) = xₖ + vₖτ + 1/2 aₖτ²
+        #   v(τ) = vₖ + aₖτ
         τ = t - self.timestamp
         τ2 = τ * τ
-        τ3 = τ * τ * τ
-        jx = (end_value.ax - self.ax) / dt
-        jy = (end_value.ay - self.ay) / dt
-        η = (end_value.alpha - self.alpha) / dt
 
         return SwerveSample(
             t,
-            self.x + self.vx * τ + 0.5 * self.ax * τ2 + 1.0 / 6.0 * jx * τ3,
-            self.y + self.vy * τ + 0.5 * self.ay * τ2 + 1.0 / 6.0 * jy * τ3,
-            self.heading + self.omega * τ + 0.5 * self.alpha * τ2 + 1.0 / 6.0 * η * τ3,
-            self.vx + self.ax * τ + 0.5 * jx * τ2,
-            self.vy + self.ay * τ + 0.5 * jy * τ2,
-            self.omega + self.alpha * τ + 0.5 * η * τ2,
-            self.ax + jx * τ,
-            self.ay + jy * τ,
-            self.alpha + η * τ,
+            self.x + self.vx * τ + 0.5 * self.ax * τ2,
+            self.y + self.vy * τ + 0.5 * self.ay * τ2,
+            self.heading + self.omega * τ + 0.5 * self.alpha * τ2,
+            self.vx + self.ax * τ,
+            self.vy + self.ay * τ,
+            self.omega + self.alpha * τ,
+            self.ax,
+            self.ay,
+            self.alpha,
             [lerp(self.fx[i], end_value.fx[i], scale) for i in range(len(self.fx))],
             [lerp(self.fy[i], end_value.fy[i], scale) for i in range(len(self.fy))],
         )

choreolib/src/main/java/choreo/trajectory/DifferentialSample.java

@@ -152,10 +152,6 @@ public DifferentialSample interpolate(DifferentialSample endValue, double timest
     double τ = timestamp - this.t;
     var sample = NumericalIntegration.rkdp(f, initialState, VecBuilder.fill(al, ar), τ);
 
-    double dt = endValue.t - this.t;
-    double jl = (endValue.al - this.al) / dt;
-    double jr = (endValue.ar - this.ar) / dt;
-
     return new DifferentialSample(
         MathUtil.interpolate(this.t, endValue.t, scale),
         sample.get(0, 0),
@@ -164,8 +160,8 @@ public DifferentialSample interpolate(DifferentialSample endValue, double timest
         sample.get(3, 0),
         sample.get(4, 0),
         sample.get(5, 0),
-        this.al + jl * τ,
-        this.ar + jr * τ,
+        this.al,
+        this.ar,
         MathUtil.interpolate(this.fl, endValue.fl, scale),
         MathUtil.interpolate(this.fr, endValue.fr, scale));
   }

choreolib/src/main/java/choreo/trajectory/SwerveSample.java

@@ -162,29 +162,21 @@ public SwerveSample interpolate(SwerveSample endValue, double timestamp) {
     //   Δt = tₖ₊₁ − tₖ
     //   τ = timestamp − tₖ
     //
-    //   x(τ) = xₖ + vₖτ + 1/2 aₖτ² + 1/6 jₖτ³
-    //   v(τ) = vₖ + aₖτ + 1/2 jₖτ²
-    //   a(τ) = aₖ + jₖτ
-    //
-    // where jₖ = (aₖ₊₁ − aₖ)/Δt
-    double dt = endValue.t - this.t;
+    //   x(τ) = xₖ + vₖτ + 1/2 aₖτ²
+    //   v(τ) = vₖ + aₖτ
     double τ = timestamp - this.t;
     double τ2 = τ * τ;
-    double τ3 = τ * τ * τ;
-    double jx = (endValue.ax - this.ax) / dt;
-    double jy = (endValue.ay - this.ay) / dt;
-    double η = (endValue.alpha - this.alpha) / dt;
     return new SwerveSample(
         timestamp,
-        this.x + this.vx * τ + 0.5 * this.ax * τ2 + 1.0 / 6.0 * jx * τ3,
-        this.y + this.vy * τ + 0.5 * this.ay * τ2 + 1.0 / 6.0 * jy * τ3,
-        this.heading + this.omega * τ + 0.5 * this.alpha * τ2 + 1.0 / 6.0 * η * τ3,
-        this.vx + this.ax * τ + 0.5 * jx * τ2,
-        this.vy + this.ay * τ + 0.5 * jy * τ2,
-        this.omega + this.alpha * τ + 0.5 * η * τ2,
-        this.ax + jx * τ,
-        this.ay + jy * τ,
-        this.alpha + η * τ,
+        this.x + this.vx * τ + 0.5 * this.ax * τ2,
+        this.y + this.vy * τ + 0.5 * this.ay * τ2,
+        this.heading + this.omega * τ + 0.5 * this.alpha * τ2,
+        this.vx + this.ax * τ,
+        this.vy + this.ay * τ,
+        this.omega + this.alpha * τ,
+        this.ax,
+        this.ay,
+        this.alpha,
         interp_fx,
         interp_fy);
   }

choreolib/src/main/native/include/choreo/trajectory/DifferentialSample.h

@@ -169,10 +169,6 @@ class DifferentialSample {
     auto sample =
         frc::RKDP(f, initialState, Eigen::Vector<double, 2>(al, ar), τ);
 
-    auto dt = endValue.timestamp - timestamp;
-    auto jl = (endValue.al - al) / dt;
-    auto jr = (endValue.ar - ar) / dt;
-
     return DifferentialSample{
         wpi::Lerp(timestamp, endValue.timestamp, scale),
         units::meter_t{sample(0, 0)},
@@ -181,8 +177,8 @@ class DifferentialSample {
         units::meters_per_second_t{sample(3, 0)},
         units::meters_per_second_t{sample(4, 0)},
         units::radians_per_second_t{sample(5, 0)},
-        al + jl * τ,
-        ar + jr * τ,
+        al,
+        ar,
         wpi::Lerp(fl, endValue.fl, scale),
         wpi::Lerp(fr, endValue.fr, scale),
     };

choreolib/src/main/native/include/choreo/trajectory/SwerveSample.h

@@ -195,31 +195,22 @@ class SwerveSample {
     //   Δt = tₖ₊₁ − tₖ
     //   τ = timestamp − tₖ
     //
-    //   x(τ) = xₖ + vₖτ + 1/2 aₖτ² + 1/6 jₖτ³
-    //   v(τ) = vₖ + aₖτ + 1/2 jₖτ²
-    //   a(τ) = aₖ + jₖτ
-    //
-    // where jₖ = (aₖ₊₁ − aₖ)/Δt
-    auto dt = endValue.timestamp - timestamp;
+    //   x(τ) = xₖ + vₖτ + 1/2 aₖτ²
+    //   v(τ) = vₖ + aₖτ
     auto τ = t - timestamp;
     auto τ2 = τ * τ;
-    auto τ3 = τ * τ * τ;
-    auto jx = (endValue.ax - ax) / dt;
-    auto jy = (endValue.ay - ay) / dt;
-    auto η = (endValue.alpha - alpha) / dt;
-    return SwerveSample{
-        wpi::Lerp(timestamp, endValue.timestamp, scale),
-        x + vx * τ + 0.5 * ax * τ2 + 1.0 / 6.0 * jx * τ3,
-        y + vy * τ + 0.5 * ay * τ2 + 1.0 / 6.0 * jy * τ3,
-        heading + omega * τ + 0.5 * alpha * τ2 + 1.0 / 6.0 * η * τ3,
-        vx + ax * τ + 0.5 * jx * τ2,
-        vy + ay * τ + 0.5 * jy * τ2,
-        omega + alpha * τ + 0.5 * η * τ2,
-        ax + jx * τ,
-        ay + jy * τ,
-        alpha + η * τ,
-        interpolatedForcesX,
-        interpolatedForcesY};
+    return SwerveSample{wpi::Lerp(timestamp, endValue.timestamp, scale),
+                        x + vx * τ + 0.5 * ax * τ2,
+                        y + vy * τ + 0.5 * ay * τ2,
+                        heading + omega * τ + 0.5 * alpha * τ2,
+                        vx + ax * τ,
+                        vy + ay * τ,
+                        omega + alpha * τ,
+                        ax,
+                        ay,
+                        alpha,
+                        interpolatedForcesX,
+                        interpolatedForcesY};
   }
 
   /**