First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: accelerate EM trajectories from rest

Browse Source
This commit is contained in:
梁薄云
2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
+20
View File
@@ -19,6 +19,7 @@ internal static class EmPlanningServiceChecks
VerifiesFullScopePublishesItsRequestScope();
VerifiesRequestAndStateFailuresPublishNoTrajectory();
VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
VerifiesNoProgressPublishesNoTrajectory();
VerifiesTimeoutFallbackAndCancellationSemantics();
VerifiesPublicationFailureAndDebugIsolation();
}
@@ -396,6 +397,22 @@ internal static class EmPlanningServiceChecks
}
}
private static void VerifiesNoProgressPublishesNoTrajectory()
{
EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false,
CreateReferencePath(TravelDirection.Forward, false, 0.10d), null,
EmPlanningScope.FullDirectionSegment);
request.Configuration.Scheduling.MaximumOptimizationSpatialStepMeters = 1d;
request.Configuration.Scheduling.MaximumOptimizationTimeStepSeconds = 1d;
EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.NoProgress)).Plan(
request, CancellationToken.None);
VerifyFailure(result, EmPlanningStatus.NoProgress, "full nonterminal no progress");
Verification.True(result.FailureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0,
"no-progress failure preserves its diagnostic");
}
private static void VerifiesPublicationFailureAndDebugIsolation()
{
EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
@@ -610,6 +627,7 @@ internal static class EmPlanningServiceChecks
SolverUnavailable,
TimeoutWithoutFallback,
TimeoutWithFallback,
NoProgress,
PublicationValidationFailure,
}
@@ -647,6 +665,8 @@ internal static class EmPlanningServiceChecks
LastLongitudinalProblem = problem;
if (mode == PipelineSolverMode.LongitudinalInfeasible)
return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
if (mode == PipelineSolverMode.NoProgress)
return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount)
{
longitudinalCallCount++;
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+51
View File
@@ -14,6 +14,7 @@ internal static class LongitudinalIntegrationChecks
{
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
VerifiesFullDirectionScheduleIsIndependentFromPublicationCadence();
VerifiesFullDirectionStaticStartMakesProgress();
VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold();
VerifiesExactStopIncludesAStabilizationTail();
VerifiesLastStrictCandidateSurvivesLaterTimeout();
@@ -109,6 +110,46 @@ internal static class LongitudinalIntegrationChecks
"halving publication cadence doubles emitted trajectory intervals without changing optimization knots");
}
private static void VerifiesFullDirectionStaticStartMakesProgress()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.SolverTimeoutSeconds = 1d;
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 1d;
LateralPath path = new LateralPath(new[]
{
Point(0d, 0d, 0d),
Point(2.5d, 2.5d, 0d),
Point(5d, 5d, 0d),
}, true);
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0d,
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "static-start envelope: " + failureReason);
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0d, 0d,
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
out LongitudinalKnotSchedule schedule, out failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "static-start schedule: " + failureReason);
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 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,
"static start succeeds: " + result.FailureReason);
LongitudinalCandidate candidate = result.Candidate ??
throw new InvalidOperationException("static start must expose a successful candidate.");
Verification.True(HasValueGreaterThan(candidate.S, 0.05d), "static start makes measurable progress");
Verification.True(HasValueGreaterThan(candidate.U, 0.05d), "static start accelerates");
Verification.NearlyEqual(0d, candidate.U[candidate.U.Count - 1],
"full segment stops at the terminal boundary");
}
private static void VerifiesFullDirectionPublicationDoesNotDuplicateItsTerminalHold()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -191,6 +232,16 @@ internal static class LongitudinalIntegrationChecks
return count;
}
private static bool HasValueGreaterThan(IReadOnlyList<double> values, double threshold)
{
for (int index = 0; index < values.Count; index++)
{
if (values[index] > threshold)
return true;
}
return false;
}
public static void RunRealOsqp()
{
foreach (LongitudinalScenario scenario in CreateRealOsqpScenarios())
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+58 -8
View File
@@ -24,6 +24,7 @@ internal static class LongitudinalModelChecks
VerifiesFullDirectionInitialFeasibilityProjectionAndFallbackSemantics();
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
VerifiesModeSpecificSolutionValidation();
VerifiesFullDirectionNoProgressValidation();
VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
}
@@ -196,9 +197,10 @@ internal static class LongitudinalModelChecks
out string failureReason);
Verification.Equal(EmPlanningStatus.Success, status, "jerk-limited stopping-tail status: " + failureReason);
double nearBoundaryPathS = input.StopBoundaryPathS - 0.005d;
Verification.NearlyEqual(MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS),
envelope.StoppingLimitAt(nearBoundaryPathS),
"near-boundary speed cap uses jerk-limited distance inversion");
Verification.True(envelope.StoppingLimitAt(nearBoundaryPathS) > 0d &&
envelope.StoppingLimitAt(nearBoundaryPathS) <=
MaximumJerkLimitedStopSpeed(input, nearBoundaryPathS) + 1e-12d,
"near-boundary speed cap conservatively interpolates jerk-limited distance inversion");
Verification.NearlyEqual(0d, envelope.StoppingLimitAt(input.StopBoundaryPathS),
"real stop boundary keeps an exact zero stopping cap");
}
@@ -608,10 +610,11 @@ internal static class LongitudinalModelChecks
(envelope.PathS[envelopeSegment + 1] - envelope.PathS[envelopeSegment]);
double envelopeIntercept = envelope.MaximumSpeedMetersPerSecond[envelopeSegment] -
envelopeSlope * envelope.PathS[envelopeSegment];
Verification.Equal(1, CountBoundedRow(problem, new Dictionary<int, double>
{
{ layout.U(1), 1d }, { layout.S(1), -envelopeSlope },
}, -QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
var envelopeRow = new Dictionary<int, double> { { layout.U(1), 1d } };
if (Math.Abs(envelopeSlope) > 1e-12d)
envelopeRow.Add(layout.S(1), -envelopeSlope);
Verification.Equal(1, CountBoundedRow(problem, envelopeRow,
-QuadraticProgram.MaximumFiniteBound, envelopeIntercept),
"U upper bound linearly re-evaluates the actual PathS envelope");
FindSingleVariableBounds(problem, layout.A(1), out double aLower, out double aUpper);
Verification.NearlyEqual(-1d, aLower, "deceleration lower bound");
@@ -724,6 +727,53 @@ internal static class LongitudinalModelChecks
"approach failure identifies the jerk-limited stoppable set");
}
private static void VerifiesFullDirectionNoProgressValidation()
{
EmPlannerConfiguration configuration = CreateUnitScaleConfiguration();
double[] times = { 0d, 0.10d, 0.20d, 0.30d, 0.50d };
var fullSchedule = new LongitudinalKnotSchedule(times,
new[] { 0d, 0.04d, 0.08d, 0.10d, 0.10d }, new[] { 0d, 0.20d, 0.10d, 0d, 0d }, true, 3);
LateralPath path = CreateStraightPath(0.10d);
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
EmPlanningScope.FullDirectionSegment, fullSchedule, Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit speedLimit,
out string speedFailure);
Verification.Equal(EmPlanningStatus.Success, speedStatus, "no-progress envelope: " + speedFailure);
LongitudinalCandidate stationary = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d,
new[] { 0d, 0d, 0d, 0d });
var validator = new LongitudinalSolutionValidator();
Verification.True(!validator.TryValidate(input, speedLimit, stationary, out _, out EmPlanningStatus failureStatus,
out string failureReason), "nonterminal stationary full segment is rejected");
Verification.Equal(EmPlanningStatus.NoProgress, failureStatus, "stationary full segment has exact status");
Verification.True(failureReason.IndexOf("NoProgress", StringComparison.Ordinal) >= 0,
"stationary full segment has exact diagnostic");
Verification.True(new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()).TryBuild(input,
speedLimit, stationary, out QuadraticProgram fullProblem, out string buildFailure),
"full no-progress objective builds: " + buildFailure);
var fullLayout = new LongitudinalVariableLayout(times.Length);
double expectedReferenceLinearCost = -2d * configuration.Longitudinal.Weights.ReferenceSpeed *
fullSchedule.ReferenceSpeedMetersPerSecond[1] /
(input.DirectionMaximumSpeedMetersPerSecond * input.DirectionMaximumSpeedMetersPerSecond);
Verification.NearlyEqual(expectedReferenceLinearCost, fullProblem.LinearCost[fullLayout.U(1)],
"full direction objective tracks adaptive speed reference");
LateralPath nearTerminalPath = CreateStraightPath(0.02d);
var nearTerminalSchedule = new LongitudinalKnotSchedule(times,
new[] { 0d, 0.01d, 0.02d, 0.02d, 0.02d }, new[] { 0d, 0d, 0d, 0d, 0d }, true, 3);
var nearTerminalInput = new LongitudinalPlanningInput(nearTerminalPath, TravelDirection.Forward, 0d, 0d,
EmTerminalType.GearSwitch, EmLongitudinalMode.ExactStopAtBoundary, configuration,
EmPlanningScope.FullDirectionSegment, nearTerminalSchedule, Array.Empty<double>(), Array.Empty<double>());
speedStatus = new PathSpeedLimitBuilder().Build(nearTerminalInput, out PathSpeedLimit nearTerminalLimit,
out speedFailure);
Verification.Equal(EmPlanningStatus.Success, speedStatus, "near-terminal no-progress envelope: " + speedFailure);
Verification.True(!validator.TryValidate(nearTerminalInput, nearTerminalLimit, stationary, out _,
out failureStatus, out _), "near-terminal fixture remains subject to its stop-tail contract");
Verification.True(failureStatus != EmPlanningStatus.NoProgress,
"near-terminal gear-switch stop is not classified as no progress");
}
private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
{
LateralPath path = CreateStraightPath(1d);
@@ -848,7 +898,7 @@ internal static class LongitudinalModelChecks
LongitudinalConfiguration limits = input.Configuration.Longitudinal;
return JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
Math.Max(0d, input.StopBoundaryPathS - pathS),
limits.MaximumAccelerationMetersPerSecondSquared,
Math.Max(0d, input.InitialAccelerationMetersPerSecondSquared),
limits.MaximumDecelerationMetersPerSecondSquared,
limits.MaximumJerkMetersPerSecondCubed,
input.DirectionMaximumSpeedMetersPerSecond);