feat: assemble complete EM trajectories

2026-08-04 11:35:50 +08:00
parent 591143f181
commit 1d58864908
5 changed files with 391 additions and 1 deletions
@@ -0,0 +1,74 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class TrajectorySampleSchedule
+{
+    private const double ZeroTolerance = 1e-12d;
+
+    public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
+    {
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
+        if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
+            throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
+        if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance)
+            throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));
+
+        var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < candidate.KnotTimes.Count; index++)
+        {
+            if (candidate.S[index] < previousPathS)
+                throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate));
+            if (candidate.U[index] < -ZeroTolerance)
+                throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate));
+
+            samples.Add(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]),
+                candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, false));
+            previousPathS = candidate.S[index];
+        }
+
+        TrajectorySample terminal = samples[samples.Count - 1];
+        double holdElapsed = 0d;
+        while (holdElapsed < holdDurationSeconds - ZeroTolerance)
+        {
+            holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
+            samples.Add(new TrajectorySample(terminal.TimeFromStart + holdElapsed, terminal.PathS, 0d, 0d, 0d, true));
+        }
+
+        Samples = new ReadOnlyCollection<TrajectorySample>(samples);
+    }
+
+    public IReadOnlyList<TrajectorySample> Samples { get; }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class TrajectorySample
+{
+    public TrajectorySample(double timeFromStart, double pathS, double progressSpeed, double acceleration, double jerk,
+        bool isHoldSample)
+    {
+        TimeFromStart = timeFromStart;
+        PathS = pathS;
+        ProgressSpeed = progressSpeed;
+        Acceleration = acceleration;
+        Jerk = jerk;
+        IsHoldSample = isHoldSample;
+    }
+
+    public double TimeFromStart { get; }
+    public double PathS { get; }
+    public double ProgressSpeed { get; }
+    public double Acceleration { get; }
+    public double Jerk { get; }
+    public bool IsHoldSample { get; }
+}