feat: assemble longitudinal ST quadratic programs

2026-08-04 09:21:07 +08:00
parent 62ea9db8cd
commit c01d0d5b47
6 changed files with 642 additions and 0 deletions
@@ -0,0 +1,124 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable time-knot s/u/a/j iterate used for ST linearization and strict validation.</summary>
+public sealed class LongitudinalCandidate
+{
+    public LongitudinalCandidate(IReadOnlyList<double> knotTimes, IReadOnlyList<double> s, IReadOnlyList<double> u,
+        IReadOnlyList<double> a, IReadOnlyList<double> j)
+    {
+        KnotTimes = CopyTimes(knotTimes);
+        S = CopyValues(s, KnotTimes.Count, nameof(s));
+        U = CopyValues(u, KnotTimes.Count, nameof(u));
+        A = CopyValues(a, KnotTimes.Count, nameof(a));
+        J = CopyValues(j, KnotTimes.Count - 1, nameof(j));
+    }
+
+    public IReadOnlyList<double> KnotTimes { get; }
+
+    public IReadOnlyList<double> S { get; }
+
+    public IReadOnlyList<double> U { get; }
+
+    public IReadOnlyList<double> A { get; }
+
+    public IReadOnlyList<double> J { get; }
+
+    public static LongitudinalCandidate Integrate(IReadOnlyList<double> knotTimes, double initialS, double initialU,
+        double initialA, IReadOnlyList<double> jerk)
+    {
+        IReadOnlyList<double> times = CopyTimes(knotTimes);
+        if (!IsFinite(initialS) || !IsFinite(initialU) || !IsFinite(initialA))
+            throw new ArgumentOutOfRangeException(nameof(initialS));
+        IReadOnlyList<double> copiedJerk = CopyValues(jerk, times.Count - 1, nameof(jerk));
+        var s = new double[times.Count];
+        var u = new double[times.Count];
+        var a = new double[times.Count];
+        s[0] = initialS;
+        u[0] = initialU;
+        a[0] = initialA;
+        for (int index = 0; index < copiedJerk.Count; index++)
+        {
+            double dt = times[index + 1] - times[index];
+            double currentJerk = copiedJerk[index];
+            a[index + 1] = a[index] + dt * currentJerk;
+            u[index + 1] = u[index] + dt * a[index] + 0.5d * dt * dt * currentJerk;
+            s[index + 1] = s[index] + dt * u[index] + 0.5d * dt * dt * a[index] +
+                dt * dt * dt * currentJerk / 6d;
+        }
+        return new LongitudinalCandidate(times, s, u, a, copiedJerk);
+    }
+
+    public bool SatisfiesExactDiscreteDynamics(double tolerance)
+    {
+        if (!IsFinite(tolerance) || tolerance < 0d)
+            throw new ArgumentOutOfRangeException(nameof(tolerance));
+        for (int index = 0; index < J.Count; index++)
+        {
+            double dt = KnotTimes[index + 1] - KnotTimes[index];
+            if (Math.Abs(A[index + 1] - (A[index] + dt * J[index])) > tolerance ||
+                Math.Abs(U[index + 1] - (U[index] + dt * A[index] + 0.5d * dt * dt * J[index])) > tolerance ||
+                Math.Abs(S[index + 1] - (S[index] + dt * U[index] + 0.5d * dt * dt * A[index] +
+                    dt * dt * dt * J[index] / 6d)) > tolerance)
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    public static IReadOnlyList<double> CreateKnotTimes(double timeHorizonSeconds, double outputTimeStepSeconds)
+    {
+        if (!IsFinite(timeHorizonSeconds) || !IsFinite(outputTimeStepSeconds) || timeHorizonSeconds <= 0d ||
+            outputTimeStepSeconds <= 0d)
+        {
+            throw new ArgumentOutOfRangeException(nameof(timeHorizonSeconds));
+        }
+        int intervalCount = checked((int)Math.Ceiling(timeHorizonSeconds / outputTimeStepSeconds));
+        var times = new double[intervalCount + 1];
+        for (int index = 0; index < intervalCount; index++)
+            times[index] = index * outputTimeStepSeconds;
+        times[intervalCount] = timeHorizonSeconds;
+        return new ReadOnlyCollection<double>(times);
+    }
+
+    private static IReadOnlyList<double> CopyTimes(IReadOnlyList<double> source)
+    {
+        if (source == null || source.Count < 2)
+            throw new ArgumentException("At least two strictly increasing time knots are required.", nameof(source));
+        var copy = new List<double>(source.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]) || source[index] <= previous)
+                throw new ArgumentException("Time knots must be finite and strictly increasing.", nameof(source));
+            if (index == 0 && Math.Abs(source[index]) > 1e-12d)
+                throw new ArgumentException("The first time knot must be exact zero.", nameof(source));
+            copy.Add(source[index]);
+            previous = source[index];
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyValues(IReadOnlyList<double> source, int expectedCount, string parameterName)
+    {
+        if (source == null || source.Count != expectedCount)
+            throw new ArgumentException("Longitudinal values do not match the time-knot layout.", parameterName);
+        var copy = new List<double>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]))
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}