949 lines
52 KiB
C#
949 lines
52 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Reflection;
|
|
using System.Threading;
|
|
using EMPlannerVerificationHost;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class EmPlanningServiceChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
|
|
VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
|
|
VerifiesServicePublishesRollingApproachAndExactStopModes();
|
|
VerifiesFullScopePublishesItsRequestScope();
|
|
VerifiesRequestAndStateFailuresPublishNoTrajectory();
|
|
VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
|
|
VerifiesTimeoutFallbackAndCancellationSemantics();
|
|
VerifiesPublicationFailureAndDebugIsolation();
|
|
}
|
|
|
|
private static void VerifiesForwardReverseAndBoundarySuccessesAreDeterministic()
|
|
{
|
|
EmPlanningRequest forwardRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
var forwardService = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success));
|
|
EmPlanningResult firstForward = forwardService.Plan(forwardRequest, CancellationToken.None);
|
|
EmPlanningResult secondForward = forwardService.Plan(forwardRequest, CancellationToken.None);
|
|
VerifySuccess(firstForward, forwardRequest, EmTerminalType.Goal, "forward");
|
|
VerifySuccess(secondForward, forwardRequest, EmTerminalType.Goal, "forward repeat");
|
|
VerifySameTrajectory(firstForward, secondForward, "forward deterministic result");
|
|
Verification.Equal(2, forwardRequest.ReferencePath.Path.Count, "request-owned reference list remains unchanged");
|
|
|
|
EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, -0.01d, false, false);
|
|
EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest,
|
|
CancellationToken.None);
|
|
VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse");
|
|
Verification.True(reverse.Trajectory.Points[1].SignedLongitudinalVelocity < 0d,
|
|
"reverse service publishes negative signed velocity");
|
|
|
|
EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0d, true, false);
|
|
EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(gearRequest,
|
|
CancellationToken.None);
|
|
VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear switch");
|
|
|
|
EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0d, false, true);
|
|
EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(rollingRequest,
|
|
CancellationToken.None);
|
|
VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop");
|
|
}
|
|
|
|
private static void VerifiesServicePublishesRollingApproachAndExactStopModes()
|
|
{
|
|
EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
|
|
CreateReferencePath(TravelDirection.Forward, false, 10d), CreateMap(false, 12d));
|
|
ConfigureFiveMeterWindowAndTwoSecondHorizon(rollingRequest.Configuration);
|
|
EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
|
rollingRequest, CancellationToken.None);
|
|
VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "cycle 1 rolling");
|
|
Verification.Equal(EmLongitudinalMode.RollingContinuation,
|
|
rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls");
|
|
Verification.Equal(21, rolling.Trajectory.Points.Count, "rolling publishes the two-second ST knot count");
|
|
EmTrajectoryPoint rollingTerminal = rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1];
|
|
Verification.True(rollingTerminal.PathS < 5d, "two-second ST output remains inside the five-metre LS window");
|
|
Verification.True(rollingTerminal.SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed");
|
|
|
|
EmPlanningRequest approachRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
|
|
CreateReferencePath(TravelDirection.Forward, false, 4d), CreateMap(false, 5d));
|
|
EmPlanningResult approach = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
|
approachRequest, CancellationToken.None);
|
|
VerifySuccess(approach, approachRequest, EmTerminalType.Goal, "cycle 2 approach");
|
|
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
|
|
approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches");
|
|
Verification.True(approach.Trajectory.Points[approach.Trajectory.Points.Count - 1].SignedLongitudinalVelocity != 0d,
|
|
"approach has no synthetic stop tail");
|
|
|
|
EmPlanningRequest exactRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
|
|
CreateReferencePath(TravelDirection.Forward, false, 0.0075d));
|
|
ConfigureExactStopServiceScenario(exactRequest.Configuration);
|
|
EmPlanningResult exact = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
|
exactRequest, CancellationToken.None);
|
|
VerifySuccess(exact, exactRequest, EmTerminalType.Goal, "cycle 3 exact goal stop");
|
|
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
|
exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops");
|
|
AssertExactStopStabilization(exact.Trajectory, EmBoundaryType.Goal, "goal");
|
|
|
|
EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
|
|
EmFixtureFactory.CreateGearPairReferencePath(0.0075d));
|
|
ConfigureExactStopServiceScenario(gearRequest.Configuration);
|
|
EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
|
gearRequest, CancellationToken.None);
|
|
VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear-switch exact stop");
|
|
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
|
gear.Trajectory.Metadata.LongitudinalMode, "gear switch stops exactly");
|
|
AssertExactStopStabilization(gear.Trajectory, EmBoundaryType.GearSwitchApproach, "gear switch");
|
|
for (int index = 0; index < gear.Trajectory.Points.Count; index++)
|
|
{
|
|
Verification.Equal(TravelDirection.Forward, gear.Trajectory.Points[index].Direction,
|
|
"gear-switch publication excludes the next direction point " + index);
|
|
}
|
|
}
|
|
|
|
private static void VerifiesFullScopePublishesItsRequestScope()
|
|
{
|
|
EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
|
|
CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null,
|
|
EmPlanningScope.FullDirectionSegment);
|
|
ConfigureExactStopServiceScenario(request.Configuration);
|
|
double[] strictFullPrimal = CreateStrictFullScopePrimal(request.Configuration);
|
|
EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success, null,
|
|
strictFullPrimal)).Plan(
|
|
request, CancellationToken.None);
|
|
VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication");
|
|
Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope,
|
|
"service metadata preserves full scope");
|
|
Verification.NearlyEqual(0d, result.Trajectory.Points[0].PathS, "full publication starts at projection");
|
|
Verification.NearlyEqual(0.0075d, result.Trajectory.Points[result.Trajectory.Points.Count - 1].PathS,
|
|
"full publication reaches the real segment boundary");
|
|
}
|
|
|
|
private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
|
|
{
|
|
configuration.Scheduling.DistanceHorizonMeters = 5d;
|
|
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
|
|
}
|
|
|
|
private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
|
|
{
|
|
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
|
|
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
|
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
|
}
|
|
|
|
private static double[] CreateStrictFullScopePrimal(EmPlannerConfiguration configuration)
|
|
{
|
|
var path = new LateralPath(new[]
|
|
{
|
|
new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d),
|
|
new LateralPathPoint(0.0075d, 0.0075d, 0d, 0d, 0d, 0d, 0.0075d, 0d, 0d, 0d, 0d, 0d),
|
|
}, true);
|
|
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(path, TravelDirection.Forward, 0.05d,
|
|
EmTerminalType.Goal, configuration, out PathSpeedLimit speedLimit, out string failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "full scope test envelope: " + failureReason);
|
|
status = new FullDirectionSegmentScheduleBuilder().TryBuild(path, speedLimit, 0.05d, 0d,
|
|
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond, configuration,
|
|
out LongitudinalKnotSchedule schedule, out failureReason);
|
|
Verification.Equal(EmPlanningStatus.Success, status, "full scope test schedule: " + failureReason);
|
|
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.05d, 0d,
|
|
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
|
|
EmPlanningScope.FullDirectionSegment, schedule, Array.Empty<double>(), Array.Empty<double>());
|
|
int motionIntervalCount = schedule.TerminalHoldStartIndex;
|
|
Verification.True(motionIntervalCount >= 3, "full scope test schedule has three motion intervals");
|
|
int terminalFirstInterval = motionIntervalCount - 3;
|
|
var terminalTimes = new double[4];
|
|
for (int index = 1; index < terminalTimes.Length; index++)
|
|
terminalTimes[index] = terminalTimes[index - 1] +
|
|
schedule.KnotTimes[terminalFirstInterval + index] -
|
|
schedule.KnotTimes[terminalFirstInterval + index - 1];
|
|
var motionTimes = new double[motionIntervalCount + 1];
|
|
for (int index = 0; index < motionTimes.Length; index++)
|
|
motionTimes[index] = schedule.KnotTimes[index];
|
|
var influence = new double[3, 3];
|
|
for (int interval = 0; interval < 3; interval++)
|
|
{
|
|
var basis = new double[3];
|
|
basis[interval] = 1d;
|
|
LongitudinalCandidate response = LongitudinalCandidate.Integrate(terminalTimes, 0d, 0d, 0d, basis);
|
|
int terminalIndex = response.S.Count - 1;
|
|
influence[0, interval] = response.A[terminalIndex];
|
|
influence[1, interval] = response.U[terminalIndex];
|
|
influence[2, interval] = response.S[terminalIndex];
|
|
}
|
|
var validator = new LongitudinalSolutionValidator();
|
|
for (int firstJerkStep = -20; firstJerkStep <= 0; firstJerkStep++)
|
|
{
|
|
for (int secondJerkStep = terminalFirstInterval >= 2 ? -20 : 0;
|
|
secondJerkStep <= (terminalFirstInterval >= 2 ? 20 : 0); secondJerkStep++)
|
|
{
|
|
for (int thirdJerkStep = terminalFirstInterval >= 3 ? -20 : 0;
|
|
thirdJerkStep <= (terminalFirstInterval >= 3 ? 20 : 0); thirdJerkStep++)
|
|
{
|
|
var jerk = new double[motionIntervalCount];
|
|
jerk[0] = firstJerkStep;
|
|
if (terminalFirstInterval >= 2)
|
|
jerk[1] = secondJerkStep;
|
|
if (terminalFirstInterval >= 3)
|
|
jerk[2] = thirdJerkStep;
|
|
LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d,
|
|
0d, jerk);
|
|
double[] target =
|
|
{
|
|
-baseline.A[baseline.A.Count - 1],
|
|
-baseline.U[baseline.U.Count - 1],
|
|
0.0075d - baseline.S[baseline.S.Count - 1],
|
|
};
|
|
if (!TrySolveThreeByThree(influence, target, out double[] terminalJerk))
|
|
throw new InvalidOperationException("Full scope strict candidate terminal system is singular.");
|
|
for (int interval = 0; interval < 3; interval++)
|
|
jerk[terminalFirstInterval + interval] = terminalJerk[interval];
|
|
LongitudinalCandidate motion = LongitudinalCandidate.Integrate(motionTimes, 0d, 0.05d, 0d,
|
|
jerk);
|
|
LongitudinalCandidate candidate = AppendFullStopTail(schedule.KnotTimes, motionIntervalCount,
|
|
motion);
|
|
if (!validator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate strict, out _))
|
|
continue;
|
|
return ToPrimal(strict);
|
|
}
|
|
}
|
|
}
|
|
throw new InvalidOperationException("Unable to construct a strict full-scope test candidate: hold=" +
|
|
motionIntervalCount + ";times=" + string.Join(",", schedule.KnotTimes));
|
|
}
|
|
|
|
private static LongitudinalCandidate AppendFullStopTail(IReadOnlyList<double> times, int motionIntervalCount,
|
|
LongitudinalCandidate motion)
|
|
{
|
|
var pathS = new double[times.Count];
|
|
var speed = new double[times.Count];
|
|
var acceleration = new double[times.Count];
|
|
var jerk = new double[times.Count - 1];
|
|
for (int index = 0; index <= motionIntervalCount; index++)
|
|
{
|
|
pathS[index] = index == motionIntervalCount ? 0.0075d : motion.S[index];
|
|
speed[index] = index == motionIntervalCount ? 0d : motion.U[index];
|
|
acceleration[index] = index == motionIntervalCount ? 0d : motion.A[index];
|
|
}
|
|
for (int index = motionIntervalCount + 1; index < times.Count; index++)
|
|
pathS[index] = 0.0075d;
|
|
for (int index = 0; index < motion.J.Count; index++)
|
|
jerk[index] = motion.J[index];
|
|
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
|
|
}
|
|
|
|
private static double[] ToPrimal(LongitudinalCandidate candidate)
|
|
{
|
|
var layout = new LongitudinalVariableLayout(candidate.S.Count);
|
|
var primal = new double[layout.VariableCount];
|
|
for (int index = 0; index < candidate.S.Count; index++)
|
|
{
|
|
primal[layout.S(index)] = candidate.S[index];
|
|
primal[layout.U(index)] = candidate.U[index];
|
|
primal[layout.A(index)] = candidate.A[index];
|
|
}
|
|
for (int index = 0; index < candidate.J.Count; index++)
|
|
primal[layout.J(index)] = candidate.J[index];
|
|
return primal;
|
|
}
|
|
|
|
private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
|
|
out double[] solution)
|
|
{
|
|
var augmented = new double[3, 4];
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
augmented[row, column] = matrix[row, column];
|
|
augmented[row, 3] = rightHandSide[row];
|
|
}
|
|
for (int column = 0; column < 3; column++)
|
|
{
|
|
int pivot = column;
|
|
for (int row = column + 1; row < 3; row++)
|
|
{
|
|
if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
|
|
pivot = row;
|
|
}
|
|
if (Math.Abs(augmented[pivot, column]) < 1e-12d)
|
|
{
|
|
solution = Array.Empty<double>();
|
|
return false;
|
|
}
|
|
if (pivot != column)
|
|
{
|
|
for (int index = column; index < 4; index++)
|
|
{
|
|
double temporary = augmented[column, index];
|
|
augmented[column, index] = augmented[pivot, index];
|
|
augmented[pivot, index] = temporary;
|
|
}
|
|
}
|
|
double divisor = augmented[column, column];
|
|
for (int index = column; index < 4; index++)
|
|
augmented[column, index] /= divisor;
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
if (row == column)
|
|
continue;
|
|
double factor = augmented[row, column];
|
|
for (int index = column; index < 4; index++)
|
|
augmented[row, index] -= factor * augmented[column, index];
|
|
}
|
|
}
|
|
solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
|
|
return true;
|
|
}
|
|
|
|
private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
|
|
{
|
|
int exactAnchor = -1;
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
if (trajectory.Points[index].BoundaryType == boundaryType)
|
|
{
|
|
exactAnchor = index;
|
|
break;
|
|
}
|
|
}
|
|
Verification.True(exactAnchor >= 0, name + " has a real boundary anchor");
|
|
Verification.NearlyEqual(0d, trajectory.Points[exactAnchor].SignedLongitudinalVelocity,
|
|
name + " speed is zero");
|
|
Verification.True(trajectory.Points.Count > exactAnchor + 1,
|
|
name + " anchor is followed by a QP stabilization point");
|
|
Verification.NearlyEqual(trajectory.Points[exactAnchor].PathS, trajectory.Points[exactAnchor + 1].PathS,
|
|
name + " stabilization keeps the stop position");
|
|
Verification.NearlyEqual(0d, trajectory.Points[exactAnchor + 1].SignedLongitudinalVelocity,
|
|
name + " stabilization speed is zero");
|
|
}
|
|
|
|
private static void VerifiesRequestAndStateFailuresPublishNoTrajectory()
|
|
{
|
|
EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false,
|
|
PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics()));
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(invalidSmoothing,
|
|
CancellationToken.None), EmPlanningStatus.InvalidReferencePath, "invalid smoothing");
|
|
|
|
EmPlanningRequest stale = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
stale = ReplaceState(stale, new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null,
|
|
stale.RequestedAtUtc.AddSeconds(-1d), stale.VehicleState.SequenceId));
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stale,
|
|
CancellationToken.None), EmPlanningStatus.StaleVehicleState, "stale state");
|
|
|
|
EmPlanningRequest directionMismatch = CreateRequest(TravelDirection.Reverse, 0.02d, false, false);
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(directionMismatch,
|
|
CancellationToken.None), EmPlanningStatus.StateDirectionMismatch, "state direction mismatch");
|
|
}
|
|
|
|
private static void VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory()
|
|
{
|
|
EmPlanningRequest projectionFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(3d, 0d, 0d), 0d, null,
|
|
projectionFailure.RequestedAtUtc, projectionFailure.VehicleState.SequenceId));
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure,
|
|
CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure");
|
|
|
|
EmPlanningRequest corridorFailure = CreateRequest(TravelDirection.Forward, 0d, false, false,
|
|
referencePath: null, map: CreateMap(true));
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure,
|
|
CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible");
|
|
|
|
EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false,
|
|
referencePath: CreateReferencePath(TravelDirection.Forward, false, 0.0055d));
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stoppingFailure,
|
|
CancellationToken.None), EmPlanningStatus.StoppingDistanceInsufficient, "stopping distance insufficient");
|
|
|
|
EmPlanningRequest regular = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LateralInfeasible)).Plan(regular,
|
|
CancellationToken.None), EmPlanningStatus.LateralInfeasible, "lateral infeasible");
|
|
EmPlanningResult longitudinalFallback = new EmPlanningService(
|
|
new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular, CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, longitudinalFallback.Status,
|
|
"longitudinal infeasible uses the validated fallback seed");
|
|
Verification.True(longitudinalFallback.Trajectory != null,
|
|
"longitudinal fallback still publishes a complete trajectory");
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular,
|
|
CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable");
|
|
}
|
|
|
|
private static void VerifiesTimeoutFallbackAndCancellationSemantics()
|
|
{
|
|
EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithoutFallback)).Plan(request,
|
|
CancellationToken.None), EmPlanningStatus.SolverTimedOut, "timeout without fallback");
|
|
|
|
EmPlanningResult fallback = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithFallback)).Plan(
|
|
request, CancellationToken.None);
|
|
Verification.Equal(EmPlanningStatus.SuccessWithFallback, fallback.Status, "timeout uses only strict fallback");
|
|
Verification.True(fallback.Trajectory != null && fallback.Trajectory.Points.Count > 0,
|
|
"timeout fallback publishes a complete trajectory");
|
|
|
|
using (var cancellation = new CancellationTokenSource())
|
|
{
|
|
cancellation.Cancel();
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(request,
|
|
cancellation.Token), EmPlanningStatus.Cancelled, "cancellation");
|
|
}
|
|
}
|
|
|
|
private static void VerifiesPublicationFailureAndDebugIsolation()
|
|
{
|
|
EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.PublicationValidationFailure,
|
|
validationFailure.Map)).Plan(validationFailure, CancellationToken.None), EmPlanningStatus.ValidationFailed,
|
|
"publication validation failure");
|
|
|
|
EmPlanningRequest debugRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
debugRequest.Configuration.Solver.NativeVerbose = true;
|
|
EmPlanningResult debugIsolated = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success),
|
|
new ThrowingDebugSink()).Plan(debugRequest, CancellationToken.None);
|
|
VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
|
|
}
|
|
|
|
private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference()
|
|
{
|
|
EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
|
|
|
|
var withoutPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
|
|
EmPlanningResult withoutPrevious = new EmPlanningService(withoutPreviousSolver).Plan(request, CancellationToken.None);
|
|
VerifySuccess(withoutPrevious, request, EmTerminalType.Goal, "no previous longitudinal seed");
|
|
|
|
EmTrajectory validPrevious = CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Forward,
|
|
request.SegmentIndex);
|
|
EmPlanningRequest withPreviousRequest = ReplacePreviousTrajectory(request, validPrevious);
|
|
var withPreviousSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
|
|
EmPlanningResult withPrevious = new EmPlanningService(withPreviousSolver).Plan(withPreviousRequest,
|
|
CancellationToken.None);
|
|
VerifySuccess(withPrevious, withPreviousRequest, EmTerminalType.Goal, "valid previous longitudinal seed");
|
|
|
|
QuadraticProgram withoutPreviousProblem = withoutPreviousSolver.LastLongitudinalProblem
|
|
?? throw new InvalidOperationException("The no-seed longitudinal QP was not captured.");
|
|
QuadraticProgram withPreviousProblem = withPreviousSolver.LastLongitudinalProblem
|
|
?? throw new InvalidOperationException("The seeded longitudinal QP was not captured.");
|
|
var layout = new LongitudinalVariableLayout((withPreviousProblem.VariableCount + 1) / 4);
|
|
Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)) >
|
|
MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
|
|
"valid previous seed adds a nonzero previous-S soft-reference Hessian term");
|
|
Verification.True(MatrixValue(withPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)) >
|
|
MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
|
|
"valid previous seed adds a nonzero previous-U soft-reference Hessian term");
|
|
Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.S(1)] -
|
|
withoutPreviousProblem.LinearCost[layout.S(1)]) > 1e-12d,
|
|
"valid previous seed adds a nonzero previous-S soft-reference linear term");
|
|
Verification.True(Math.Abs(withPreviousProblem.LinearCost[layout.U(1)] -
|
|
withoutPreviousProblem.LinearCost[layout.U(1)]) > 1e-12d,
|
|
"valid previous seed adds a nonzero previous-U soft-reference linear term");
|
|
|
|
EmPlanningRequest incompatibleRequest = ReplacePreviousTrajectory(request,
|
|
CreateLongitudinalPreviousTrajectory(request.EffectiveAtUtc, TravelDirection.Reverse, request.SegmentIndex));
|
|
var incompatibleSolver = new ScriptedPipelineSolver(PipelineSolverMode.Success);
|
|
EmPlanningResult incompatible = new EmPlanningService(incompatibleSolver).Plan(incompatibleRequest,
|
|
CancellationToken.None);
|
|
VerifySuccess(incompatible, incompatibleRequest, EmTerminalType.Goal, "incompatible previous seed");
|
|
QuadraticProgram incompatibleProblem = incompatibleSolver.LastLongitudinalProblem
|
|
?? throw new InvalidOperationException("The incompatible-seed longitudinal QP was not captured.");
|
|
Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.S(1), layout.S(1)),
|
|
MatrixValue(incompatibleProblem.UpperTriangularP, layout.S(1), layout.S(1)),
|
|
"incompatible previous seed omits previous-S soft-reference term");
|
|
Verification.NearlyEqual(MatrixValue(withoutPreviousProblem.UpperTriangularP, layout.U(1), layout.U(1)),
|
|
MatrixValue(incompatibleProblem.UpperTriangularP, layout.U(1), layout.U(1)),
|
|
"incompatible previous seed omits previous-U soft-reference term");
|
|
}
|
|
|
|
private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType,
|
|
string name)
|
|
{
|
|
Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
|
|
name + " successful status: " + result.FailureReason);
|
|
Verification.True(result.Trajectory != null && result.Trajectory.Metadata.TerminalType == terminalType,
|
|
name + " terminal type");
|
|
string identifiers = "map=" + request.Map.SnapshotId + ";reference=" + request.ReferencePathId + ";state=" +
|
|
request.VehicleState.SequenceId + ";previous=" + request.PreviousTrajectoryId + ";segment=" + request.SegmentIndex;
|
|
Verification.True(result.FailureReason.IndexOf(identifiers, StringComparison.Ordinal) >= 0,
|
|
name + " preserves request identifiers in deterministic diagnostics");
|
|
}
|
|
|
|
private static void VerifyFailure(EmPlanningResult result, EmPlanningStatus expected, string name)
|
|
{
|
|
Verification.Equal(expected, result.Status, name + " status: " + result.FailureReason);
|
|
Verification.True(result.Trajectory == null, name + " publishes no partial trajectory");
|
|
}
|
|
|
|
private static void VerifySameTrajectory(EmPlanningResult left, EmPlanningResult right, string name)
|
|
{
|
|
Verification.Equal(left.Status, right.Status, name + " status");
|
|
Verification.Equal(left.Trajectory.Points.Count, right.Trajectory.Points.Count, name + " point count");
|
|
for (int index = 0; index < left.Trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint first = left.Trajectory.Points[index];
|
|
EmTrajectoryPoint second = right.Trajectory.Points[index];
|
|
Verification.NearlyEqual(first.X, second.X, name + " X " + index);
|
|
Verification.NearlyEqual(first.Y, second.Y, name + " Y " + index);
|
|
Verification.NearlyEqual(first.TimeFromStart, second.TimeFromStart, name + " time " + index);
|
|
Verification.NearlyEqual(first.SignedLongitudinalVelocity, second.SignedLongitudinalVelocity,
|
|
name + " signed speed " + index);
|
|
}
|
|
}
|
|
|
|
private static EmPlanningRequest CreateRequest(TravelDirection direction, double signedSpeed, bool endsAtGearSwitch,
|
|
bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null,
|
|
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
|
|
{
|
|
DateTimeOffset requestedAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Solver.MaximumOuterIterations = 2;
|
|
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
|
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
|
|
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 0.20d;
|
|
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.20d;
|
|
configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond = 0.20d;
|
|
if (rolling)
|
|
configuration.Scheduling.DistanceHorizonMeters = 0.30d;
|
|
return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
|
|
new VehicleParameters
|
|
{
|
|
LengthMeters = 0.10d,
|
|
WidthMeters = 0.10d,
|
|
SafetyMarginMeters = 0d,
|
|
MaximumCurvaturePerMeter = 1d,
|
|
},
|
|
new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
|
|
null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
|
|
EmMotionModel.NonholonomicForwardReverse, planningScope);
|
|
}
|
|
|
|
private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)
|
|
{
|
|
return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration,
|
|
source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
|
|
source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel,
|
|
source.PlanningScope);
|
|
}
|
|
|
|
private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory)
|
|
{
|
|
return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState,
|
|
source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
|
|
source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel,
|
|
source.PlanningScope);
|
|
}
|
|
|
|
private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc,
|
|
TravelDirection direction, int segmentIndex)
|
|
{
|
|
var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L,
|
|
"previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
|
|
EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
|
|
double sign = direction == TravelDirection.Forward ? 1d : -1d;
|
|
return new EmTrajectory(metadata, new[]
|
|
{
|
|
new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 0d, 0d, segmentIndex, 0.001d, 0.001d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
new EmTrajectoryPoint(sign * 0.001d, 0d, 0d, sign * 0.01d, 6d, 0d, segmentIndex, 0.001d, 0.001d,
|
|
direction, EmBoundaryType.None, 0d, 0d),
|
|
});
|
|
}
|
|
|
|
private static double MatrixValue(SparseCscMatrix matrix, int row, int column)
|
|
{
|
|
for (int index = matrix.ColumnPointers[column]; index < matrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (matrix.RowIndices[index] == row)
|
|
return matrix.Values[index];
|
|
}
|
|
return 0d;
|
|
}
|
|
|
|
private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch,
|
|
double lengthMeters = 2d)
|
|
{
|
|
double endX = direction == TravelDirection.Forward ? lengthMeters : -lengthMeters;
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, direction, 0d, 0d, 0d, 1d, false,
|
|
SmoothedPathPointSource.Anchor),
|
|
new SmoothedPathPoint(endX, 0d, 0d, 0d, lengthMeters, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
|
|
endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor),
|
|
};
|
|
var segments = new List<SmoothedPathSegment>
|
|
{
|
|
new SmoothedPathSegment(0, direction, 0, 1, false, endsAtGearSwitch),
|
|
};
|
|
var metrics = new PathQualityMetrics(true, lengthMeters, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
|
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
|
|
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
|
}
|
|
|
|
private static PlanningGridMap CreateMap(bool blockStart, double halfExtentMeters = 3d)
|
|
{
|
|
IMapObstacleSource[] sources = blockStart
|
|
? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true,
|
|
new IMapObstacle[] { new AxisAlignedRectangleObstacle(-20f, 20f, -20f, 20f) }) }
|
|
: Array.Empty<IMapObstacleSource>();
|
|
PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm((float)(-1000d * halfExtentMeters), (float)(1000d * halfExtentMeters), -1000f, 1000f),
|
|
ResolutionMm = 20f,
|
|
ObstacleSources = sources,
|
|
AllowExplicitEmptyMap = !blockStart,
|
|
});
|
|
Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
|
|
"service map builds: " + result.FailureReason);
|
|
return result.Map!;
|
|
}
|
|
|
|
private enum PipelineSolverMode
|
|
{
|
|
Success,
|
|
LateralInfeasible,
|
|
LongitudinalInfeasible,
|
|
SolverUnavailable,
|
|
TimeoutWithoutFallback,
|
|
TimeoutWithFallback,
|
|
PublicationValidationFailure,
|
|
}
|
|
|
|
private sealed class ScriptedPipelineSolver : IQpSolver
|
|
{
|
|
private readonly PipelineSolverMode mode;
|
|
private readonly PlanningGridMap? mapToCorrupt;
|
|
private readonly IReadOnlyList<double>? strictFullPrimal;
|
|
private int longitudinalCallCount;
|
|
|
|
public QuadraticProgram? LastLongitudinalProblem { get; private set; }
|
|
|
|
public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null,
|
|
IReadOnlyList<double>? strictFullPrimal = null)
|
|
{
|
|
this.mode = mode;
|
|
this.mapToCorrupt = mapToCorrupt;
|
|
this.strictFullPrimal = strictFullPrimal;
|
|
}
|
|
|
|
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
bool longitudinal = IsLongitudinalProblem(problem);
|
|
if (mode == PipelineSolverMode.SolverUnavailable)
|
|
return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
|
|
if (!longitudinal)
|
|
{
|
|
if (mode == PipelineSolverMode.LateralInfeasible)
|
|
return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
|
|
if (mode == PipelineSolverMode.TimeoutWithoutFallback)
|
|
return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
|
|
return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
|
|
}
|
|
LastLongitudinalProblem = problem;
|
|
if (mode == PipelineSolverMode.LongitudinalInfeasible)
|
|
return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
|
|
if (strictFullPrimal != null && strictFullPrimal.Count == problem.VariableCount)
|
|
{
|
|
longitudinalCallCount++;
|
|
return Result(QpSolveStatus.Solved, strictFullPrimal);
|
|
}
|
|
if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
|
|
CorruptMapAtOrigin(mapToCorrupt);
|
|
if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
|
|
return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
|
|
longitudinalCallCount++;
|
|
return Result(QpSolveStatus.Solved,
|
|
TryCreateStrictExactStopPrimal(problem, out double[] strictPrimal) ? strictPrimal : warmStart);
|
|
}
|
|
|
|
private static void CorruptMapAtOrigin(PlanningGridMap? map)
|
|
{
|
|
if (map == null || !map.TryWorldToGrid(0d, 0d, out int row, out int column))
|
|
throw new InvalidOperationException("Unable to corrupt the publication test map.");
|
|
FieldInfo occupiedField = typeof(PlanningGridMap).GetField("_occupied", BindingFlags.Instance | BindingFlags.NonPublic)!
|
|
?? throw new InvalidOperationException("Planning map occupancy storage was unavailable.");
|
|
FieldInfo distanceField = typeof(PlanningGridMap).GetField("_conservativeDistances",
|
|
BindingFlags.Instance | BindingFlags.NonPublic) ?? throw new InvalidOperationException("Planning map distance storage was unavailable.");
|
|
byte[] occupied = (byte[])occupiedField.GetValue(map)!;
|
|
double[] distances = (double[])distanceField.GetValue(map)!;
|
|
int index = row * map.Cols + column;
|
|
occupied[index] = 1;
|
|
distances[index] = 0d;
|
|
}
|
|
|
|
private static bool TryCreateStrictExactStopPrimal(QuadraticProgram problem, out double[] primal)
|
|
{
|
|
primal = Array.Empty<double>();
|
|
int variableCount = problem.VariableCount;
|
|
int knotCount = (variableCount + 1) / 4;
|
|
var layout = new LongitudinalVariableLayout(knotCount);
|
|
int stabilizationStart = FindExactStopTailStart(problem, layout);
|
|
if (stabilizationStart < 3)
|
|
return false;
|
|
var times = new double[knotCount];
|
|
for (int index = 0; index < knotCount - 1; index++)
|
|
{
|
|
if (!TryReadDynamicsDuration(problem, layout, index, out double duration))
|
|
return false;
|
|
times[index + 1] = times[index] + duration;
|
|
}
|
|
|
|
var motionTimes = new double[stabilizationStart + 1];
|
|
Array.Copy(times, motionTimes, motionTimes.Length);
|
|
double initialPathS = ReadFixedVariable(problem, layout.S(0));
|
|
double initialSpeed = ReadFixedVariable(problem, layout.U(0));
|
|
double initialAcceleration = ReadFixedVariable(problem, layout.A(0));
|
|
double terminalPathS = ReadFixedVariable(problem, layout.S(stabilizationStart));
|
|
var preferredJerk = new double[stabilizationStart];
|
|
LongitudinalCandidate baseline = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
|
|
initialAcceleration, preferredJerk);
|
|
var influence = new double[3, stabilizationStart];
|
|
for (int interval = 0; interval < stabilizationStart; interval++)
|
|
{
|
|
var basis = new double[stabilizationStart];
|
|
basis[interval] = 1d;
|
|
LongitudinalCandidate response = LongitudinalCandidate.Integrate(motionTimes, 0d, 0d, 0d, basis);
|
|
int terminalIndex = response.S.Count - 1;
|
|
influence[0, interval] = response.A[terminalIndex];
|
|
influence[1, interval] = response.U[terminalIndex];
|
|
influence[2, interval] = response.S[terminalIndex];
|
|
}
|
|
double[] target =
|
|
{
|
|
-baseline.A[baseline.A.Count - 1],
|
|
-baseline.U[baseline.U.Count - 1],
|
|
terminalPathS - baseline.S[baseline.S.Count - 1],
|
|
};
|
|
var jerk = new double[knotCount - 1];
|
|
if (stabilizationStart >= 4)
|
|
{
|
|
int terminalFirstInterval = stabilizationStart - 3;
|
|
var terminalInfluence = new double[3, 3];
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
terminalInfluence[row, column] = influence[row, terminalFirstInterval + column];
|
|
}
|
|
if (!TrySolveThreeByThree(terminalInfluence, target, out double[] terminalJerk))
|
|
return false;
|
|
for (int interval = 0; interval < 3; interval++)
|
|
jerk[terminalFirstInterval + interval] = terminalJerk[interval];
|
|
}
|
|
else
|
|
{
|
|
var gram = new double[3, 3];
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
{
|
|
for (int interval = 0; interval < stabilizationStart; interval++)
|
|
gram[row, column] += influence[row, interval] * influence[column, interval];
|
|
}
|
|
}
|
|
if (!TrySolveThreeByThree(gram, target, out double[] multipliers))
|
|
return false;
|
|
for (int interval = 0; interval < stabilizationStart; interval++)
|
|
{
|
|
jerk[interval] = preferredJerk[interval];
|
|
for (int row = 0; row < 3; row++)
|
|
jerk[interval] += influence[row, interval] * multipliers[row];
|
|
}
|
|
}
|
|
var motionJerk = new double[stabilizationStart];
|
|
Array.Copy(jerk, motionJerk, motionJerk.Length);
|
|
LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(motionTimes, initialPathS, initialSpeed,
|
|
initialAcceleration, motionJerk);
|
|
primal = CreateExactStopPrimal(layout, knotCount, stabilizationStart, terminalPathS, candidate);
|
|
return true;
|
|
}
|
|
|
|
private static double[] CreateExactStopPrimal(LongitudinalVariableLayout layout, int knotCount,
|
|
int stabilizationStart, double terminalPathS, LongitudinalCandidate candidate)
|
|
{
|
|
var primal = new double[layout.VariableCount];
|
|
for (int index = 0; index < knotCount; index++)
|
|
{
|
|
bool isTerminalTail = index >= stabilizationStart;
|
|
primal[layout.S(index)] = isTerminalTail ? terminalPathS : candidate.S[index];
|
|
primal[layout.U(index)] = isTerminalTail ? 0d : candidate.U[index];
|
|
primal[layout.A(index)] = isTerminalTail ? 0d : candidate.A[index];
|
|
}
|
|
for (int index = 0; index < candidate.J.Count; index++)
|
|
primal[layout.J(index)] = candidate.J[index];
|
|
return primal;
|
|
}
|
|
|
|
private static int FindExactStopTailStart(QuadraticProgram problem, LongitudinalVariableLayout layout)
|
|
{
|
|
for (int index = 1; index < layout.KnotCount; index++)
|
|
{
|
|
if (TryReadFixedVariable(problem, layout.S(index), out _) &&
|
|
TryReadFixedVariable(problem, layout.U(index), out _) &&
|
|
TryReadFixedVariable(problem, layout.A(index), out _))
|
|
{
|
|
return index;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
private static bool TryReadDynamicsDuration(QuadraticProgram problem, LongitudinalVariableLayout layout,
|
|
int interval, out double duration)
|
|
{
|
|
duration = 0d;
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
if (Math.Abs(problem.LowerBounds[row]) > 1e-12d || Math.Abs(problem.UpperBounds[row]) > 1e-12d ||
|
|
CountRowEntries(problem, row) != 3 ||
|
|
Math.Abs(ReadCoefficient(problem, row, layout.A(interval + 1)) - 1d) > 1e-12d ||
|
|
Math.Abs(ReadCoefficient(problem, row, layout.A(interval)) + 1d) > 1e-12d)
|
|
{
|
|
continue;
|
|
}
|
|
double jerkCoefficient = ReadCoefficient(problem, row, layout.J(interval));
|
|
if (jerkCoefficient >= -1e-12d)
|
|
continue;
|
|
duration = -jerkCoefficient;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static int CountRowEntries(QuadraticProgram problem, int row)
|
|
{
|
|
int count = 0;
|
|
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
|
{
|
|
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
|
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
|
count++;
|
|
}
|
|
}
|
|
return count;
|
|
}
|
|
|
|
private static double ReadCoefficient(QuadraticProgram problem, int row, int column)
|
|
{
|
|
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
|
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (problem.ConstraintMatrix.RowIndices[index] == row)
|
|
return problem.ConstraintMatrix.Values[index];
|
|
}
|
|
return 0d;
|
|
}
|
|
|
|
private static bool TrySolveThreeByThree(double[,] matrix, IReadOnlyList<double> rightHandSide,
|
|
out double[] solution)
|
|
{
|
|
var augmented = new double[3, 4];
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
for (int column = 0; column < 3; column++)
|
|
augmented[row, column] = matrix[row, column];
|
|
augmented[row, 3] = rightHandSide[row];
|
|
}
|
|
for (int column = 0; column < 3; column++)
|
|
{
|
|
int pivot = column;
|
|
for (int row = column + 1; row < 3; row++)
|
|
{
|
|
if (Math.Abs(augmented[row, column]) > Math.Abs(augmented[pivot, column]))
|
|
pivot = row;
|
|
}
|
|
if (Math.Abs(augmented[pivot, column]) < 1e-12d)
|
|
{
|
|
solution = Array.Empty<double>();
|
|
return false;
|
|
}
|
|
if (pivot != column)
|
|
{
|
|
for (int index = column; index < 4; index++)
|
|
{
|
|
double temporary = augmented[column, index];
|
|
augmented[column, index] = augmented[pivot, index];
|
|
augmented[pivot, index] = temporary;
|
|
}
|
|
}
|
|
double divisor = augmented[column, column];
|
|
for (int index = column; index < 4; index++)
|
|
augmented[column, index] /= divisor;
|
|
for (int row = 0; row < 3; row++)
|
|
{
|
|
if (row == column)
|
|
continue;
|
|
double factor = augmented[row, column];
|
|
for (int index = column; index < 4; index++)
|
|
augmented[row, index] -= factor * augmented[column, index];
|
|
}
|
|
}
|
|
solution = new[] { augmented[0, 3], augmented[1, 3], augmented[2, 3] };
|
|
return true;
|
|
}
|
|
|
|
private static bool IsLongitudinalProblem(QuadraticProgram problem)
|
|
{
|
|
if (problem.VariableCount < 7 || (problem.VariableCount + 1) % 4 != 0)
|
|
return false;
|
|
int knotCount = (problem.VariableCount + 1) / 4;
|
|
return problem.ConstraintCount >= 8 * knotCount - 2;
|
|
}
|
|
|
|
private static double ReadFixedVariable(QuadraticProgram problem, int variable)
|
|
{
|
|
if (TryReadFixedVariable(problem, variable, out double value))
|
|
return value;
|
|
throw new InvalidOperationException("Expected a fixed ST variable constraint.");
|
|
}
|
|
|
|
private static bool TryReadFixedVariable(QuadraticProgram problem, int variable, out double value)
|
|
{
|
|
for (int row = 0; row < problem.ConstraintCount; row++)
|
|
{
|
|
int entryCount = 0;
|
|
double coefficient = 0d;
|
|
for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
|
|
{
|
|
for (int index = problem.ConstraintMatrix.ColumnPointers[column];
|
|
index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
|
|
{
|
|
if (problem.ConstraintMatrix.RowIndices[index] != row)
|
|
continue;
|
|
entryCount++;
|
|
if (column == variable)
|
|
coefficient = problem.ConstraintMatrix.Values[index];
|
|
}
|
|
}
|
|
if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
|
|
Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
|
|
{
|
|
value = problem.LowerBounds[row] / coefficient;
|
|
return true;
|
|
}
|
|
}
|
|
value = 0d;
|
|
return false;
|
|
}
|
|
|
|
private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal)
|
|
{
|
|
return new QpSolveResult(status, primal, 0d, 0d, 0d, 1, TimeSpan.Zero, status.ToString(), string.Empty);
|
|
}
|
|
}
|
|
|
|
private sealed class ThrowingDebugSink : IEmPlannerDebugSink
|
|
{
|
|
public void Write(string message)
|
|
{
|
|
throw new InvalidOperationException("debug sink failure");
|
|
}
|
|
}
|
|
}
|