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/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())