fix: generate deterministic S-curve static-start ST seeds

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -537,7 +537,86 @@ public sealed class SequentialLongitudinalOptimizer
                 return true;
             }
         }
-        failureReason = "exactJerkSeed=failed; sampledSeeds=failed";
+        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, out candidate))
+        {
+            failureReason = string.Empty;
+            return true;
+        }
+        failureReason = "exactJerkSeed=failed; sampledSeeds=failed; scurveSeeds=failed";
+        return false;
+    }
+
+    private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
+        IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
+        out LongitudinalCandidate candidate)
+    {
+        candidate = null;
+        int intervalCount = stabilizationStart;
+        int maximumRamp = Math.Max(1, intervalCount / 6);
+        var motionTimes = new double[intervalCount + 1];
+        for (int index = 0; index <= intervalCount; index++)
+            motionTimes[index] = times[index];
+
+        for (int ramp = 1; ramp <= maximumRamp; ramp++)
+        {
+            for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
+            {
+                int cruise = intervalCount - 4 * ramp - 2 * plateau;
+                var basisAccel = new double[intervalCount];
+                var basisBrake = new double[intervalCount];
+                var basisOffset = new double[intervalCount];
+                int cursor = 0;
+                for (int index = 0; index < ramp; index++)
+                    basisAccel[cursor++] = 1d;
+                for (int index = 0; index < plateau; index++)
+                    cursor++;
+                for (int index = 0; index < ramp; index++)
+                    basisAccel[cursor++] = -1d;
+                for (int index = 0; index < cruise; index++)
+                    cursor++;
+                for (int index = 0; index < ramp; index++)
+                    basisBrake[cursor++] = -1d;
+                for (int index = 0; index < plateau; index++)
+                    cursor++;
+                for (int index = 0; index < ramp; index++)
+                    basisBrake[cursor++] = 1d;
+                if (cursor != intervalCount)
+                    continue;
+                for (int index = 0; index < intervalCount; index++)
+                    basisOffset[index] = 1d;
+
+                LongitudinalCandidate accelResponse = LongitudinalCandidate.Integrate(
+                    motionTimes, 0d, 0d, 0d, basisAccel);
+                LongitudinalCandidate brakeResponse = LongitudinalCandidate.Integrate(
+                    motionTimes, 0d, 0d, 0d, basisBrake);
+                LongitudinalCandidate offsetResponse = LongitudinalCandidate.Integrate(
+                    motionTimes, 0d, 0d, 0d, basisOffset);
+                var influence = new double[3, 3];
+                influence[0, 0] = accelResponse.A[intervalCount];
+                influence[1, 0] = accelResponse.U[intervalCount];
+                influence[2, 0] = accelResponse.S[intervalCount];
+                influence[0, 1] = brakeResponse.A[intervalCount];
+                influence[1, 1] = brakeResponse.U[intervalCount];
+                influence[2, 1] = brakeResponse.S[intervalCount];
+                influence[0, 2] = offsetResponse.A[intervalCount];
+                influence[1, 2] = offsetResponse.U[intervalCount];
+                influence[2, 2] = offsetResponse.S[intervalCount];
+
+                double[] target = { 0d, 0d, input.StopBoundaryPathS };
+                if (!TrySolveThreeByThree(influence, target, out double[] multipliers))
+                    continue;
+
+                var jerk = new double[intervalCount];
+                for (int index = 0; index < intervalCount; index++)
+                {
+                    jerk[index] = multipliers[0] * basisAccel[index] +
+                        multipliers[1] * basisBrake[index] +
+                        multipliers[2] * basisOffset[index];
+                }
+                if (TryValidateExactSeed(input, times, stabilizationStart, speedLimit, jerk, out candidate))
+                    return true;
+            }
+        }
         return false;
     }
 

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs

@@ -16,6 +16,7 @@ internal static class LongitudinalIntegrationChecks
         VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
         VerifiesFullDirectionStaticStartMakesProgress();
         VerifiesFullDirectionNearZeroSpeedUsesStaticStartSeed();
+        VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed();
         VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
         VerifiesExactStopIncludesAStabilizationTail();
         VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -187,6 +188,42 @@ internal static class LongitudinalIntegrationChecks
             "near-zero start stops at the terminal boundary");
     }
 
+    private static void VerifiesFullDirectionCurvedNearZeroStartUsesStaticStartSeed()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Scheduling.SolverTimeoutSeconds = 5d;
+        configuration.Solver.MaximumOsqpIterations = 100000;
+        LateralPath path = new LateralPath(new[]
+        {
+            Point(0d, 0d, 0d),
+            Point(1d, 2.25d, 0.1d),
+            Point(2d, 4.5d, 0d),
+        }, true);
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.0001d,
+            EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero envelope: " + failureReason);
+        status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.0001d, 0d,
+            configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
+            out LongitudinalKnotSchedule schedule, out failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "curved near-zero schedule: " + failureReason);
+
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.0001d, 0d,
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
+        LongitudinalPlanningResult result = new LongitudinalPlanner(new OsqpNativeSolver()).Plan(input,
+            CancellationToken.None);
+
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            "curved near-zero full-direction start succeeds: " + result.FailureReason);
+        LongitudinalCandidate candidate = result.Candidate ??
+            throw new InvalidOperationException("Curved near-zero full-direction candidate was missing.");
+        Verification.NearlyEqual(path.Points[path.Points.Count - 1].PathS, candidate.S[candidate.S.Count - 1],
+            "curved near-zero start reaches the terminal boundary");
+        Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
+            "curved near-zero start stops at the terminal boundary");
+    }
+
     private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
     {
         EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();