feat: validate EM planner requests

2026-08-03 22:22:44 +08:00
parent e12eb31205
commit b326431d63
16 changed files with 734 additions and 11 deletions
@@ -0,0 +1,22 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class CorridorConfiguration
+{
+    public double LongitudinalSampleSpacingMeters { get; set; }
+    public double LateralSampleSpacingMeters { get; set; }
+    public double MaximumLateralOffsetMeters { get; set; }
+    public double AdditionalClearanceReserveMeters { get; set; }
+    public double MaximumCollisionCheckStepMeters { get; set; }
+
+    internal CorridorConfiguration Copy()
+    {
+        return new CorridorConfiguration
+        {
+            LongitudinalSampleSpacingMeters = LongitudinalSampleSpacingMeters,
+            LateralSampleSpacingMeters = LateralSampleSpacingMeters,
+            MaximumLateralOffsetMeters = MaximumLateralOffsetMeters,
+            AdditionalClearanceReserveMeters = AdditionalClearanceReserveMeters,
+            MaximumCollisionCheckStepMeters = MaximumCollisionCheckStepMeters,
+        };
+    }
+}
@@ -0,0 +1,111 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed partial class EmPlannerConfiguration
+{
+    public SchedulingConfiguration Scheduling { get; set; }
+    public FrenetConfiguration Frenet { get; set; }
+    public CorridorConfiguration Corridor { get; set; }
+    public LateralConfiguration Lateral { get; set; }
+    public LongitudinalConfiguration Longitudinal { get; set; }
+    public SolverConfiguration Solver { get; set; }
+    public ValidationConfiguration Validation { get; set; }
+
+    public static EmPlannerConfiguration CreateDefault()
+    {
+        return new EmPlannerConfiguration
+        {
+            Scheduling = new SchedulingConfiguration
+            {
+                ReplanPeriodSeconds = 0.20d,
+                TimeHorizonSeconds = 6d,
+                DistanceHorizonMeters = 5d,
+                OutputTimeStepSeconds = 0.05d,
+                SolverTimeoutSeconds = 0.10d,
+                HandoffLookaheadSeconds = 0.30d,
+                MaximumVehicleStateAgeSeconds = 0.20d,
+            },
+            Corridor = new CorridorConfiguration
+            {
+                LongitudinalSampleSpacingMeters = 0.10d,
+                LateralSampleSpacingMeters = 0.025d,
+                MaximumLateralOffsetMeters = 0.30d,
+                AdditionalClearanceReserveMeters = 0.02d,
+                MaximumCollisionCheckStepMeters = 0.025d,
+            },
+            Frenet = new FrenetConfiguration
+            {
+                MaximumProjectionDistanceMeters = 0.50d,
+                MinimumFrenetDenominator = 0.20d,
+                BoundaryAnchorToleranceMeters = 1e-8d,
+            },
+            Lateral = new LateralConfiguration
+            {
+                MaximumLateralStepPerIterationMeters = 0.05d,
+                MaximumLateralSlope = 0.50d,
+                MaximumLateralSecondDerivativePerMeter = 1d,
+                MaximumLateralThirdDerivativePerSquareMeter = 2d,
+                Weights = new LateralWeights
+                {
+                    ReferenceOffset = 10d,
+                    HeadingDeviation = 1d,
+                    SecondDerivative = 5d,
+                    ThirdDerivative = 10d,
+                    Curvature = 5d,
+                    CurvatureVariation = 20d,
+                    PreviousTrajectory = 5d,
+                    RollingTerminal = 10d,
+                },
+            },
+            Longitudinal = new LongitudinalConfiguration
+            {
+                MaximumForwardSpeedMetersPerSecond = 0.20d,
+                MaximumReverseSpeedMetersPerSecond = 0.20d,
+                MaximumAccelerationMetersPerSecondSquared = 0.20d,
+                MaximumDecelerationMetersPerSecondSquared = 0.30d,
+                MaximumJerkMetersPerSecondCubed = 0.50d,
+                MaximumLateralAccelerationMetersPerSecondSquared = 0.20d,
+                MaximumCurvatureRatePerMeterPerSecond = 0.50d,
+                StopSpeedToleranceMetersPerSecond = 0.01d,
+                ZeroSpeedHoldSeconds = 0.20d,
+                Weights = new LongitudinalWeights
+                {
+                    ReferenceSpeed = 10d,
+                    Acceleration = 1d,
+                    Jerk = 10d,
+                    PreviousTrajectory = 5d,
+                    TerminalAcceleration = 1d,
+                },
+            },
+            Solver = new SolverConfiguration
+            {
+                MaximumOuterIterations = 5,
+                MaximumOsqpIterations = 4000,
+                AbsoluteTolerance = 1e-5d,
+                RelativeTolerance = 1e-5d,
+                StrictResidualTolerance = 1e-5d,
+                WarmStart = true,
+                Polish = true,
+                NativeVerbose = false,
+            },
+            Validation = new ValidationConfiguration
+            {
+                SpatialToleranceMeters = 1e-8d,
+                KinematicTolerance = 1e-8d,
+            },
+        };
+    }
+
+    internal EmPlannerConfiguration Copy()
+    {
+        return new EmPlannerConfiguration
+        {
+            Scheduling = Scheduling == null ? null : Scheduling.Copy(),
+            Frenet = Frenet == null ? null : Frenet.Copy(),
+            Corridor = Corridor == null ? null : Corridor.Copy(),
+            Lateral = Lateral == null ? null : Lateral.Copy(),
+            Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(),
+            Solver = Solver == null ? null : Solver.Copy(),
+            Validation = Validation == null ? null : Validation.Copy(),
+        };
+    }
+}
@@ -0,0 +1,18 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class FrenetConfiguration
+{
+    public double MaximumProjectionDistanceMeters { get; set; }
+    public double MinimumFrenetDenominator { get; set; }
+    public double BoundaryAnchorToleranceMeters { get; set; }
+
+    internal FrenetConfiguration Copy()
+    {
+        return new FrenetConfiguration
+        {
+            MaximumProjectionDistanceMeters = MaximumProjectionDistanceMeters,
+            MinimumFrenetDenominator = MinimumFrenetDenominator,
+            BoundaryAnchorToleranceMeters = BoundaryAnchorToleranceMeters,
+        };
+    }
+}
@@ -0,0 +1,22 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class LateralConfiguration
+{
+    public double MaximumLateralStepPerIterationMeters { get; set; }
+    public double MaximumLateralSlope { get; set; }
+    public double MaximumLateralSecondDerivativePerMeter { get; set; }
+    public double MaximumLateralThirdDerivativePerSquareMeter { get; set; }
+    public LateralWeights Weights { get; set; }
+
+    internal LateralConfiguration Copy()
+    {
+        return new LateralConfiguration
+        {
+            MaximumLateralStepPerIterationMeters = MaximumLateralStepPerIterationMeters,
+            MaximumLateralSlope = MaximumLateralSlope,
+            MaximumLateralSecondDerivativePerMeter = MaximumLateralSecondDerivativePerMeter,
+            MaximumLateralThirdDerivativePerSquareMeter = MaximumLateralThirdDerivativePerSquareMeter,
+            Weights = Weights == null ? null : Weights.Copy(),
+        };
+    }
+}
@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class LateralWeights
+{
+    public double ReferenceOffset { get; set; }
+    public double HeadingDeviation { get; set; }
+    public double SecondDerivative { get; set; }
+    public double ThirdDerivative { get; set; }
+    public double Curvature { get; set; }
+    public double CurvatureVariation { get; set; }
+    public double PreviousTrajectory { get; set; }
+    public double RollingTerminal { get; set; }
+
+    internal LateralWeights Copy()
+    {
+        return new LateralWeights
+        {
+            ReferenceOffset = ReferenceOffset,
+            HeadingDeviation = HeadingDeviation,
+            SecondDerivative = SecondDerivative,
+            ThirdDerivative = ThirdDerivative,
+            Curvature = Curvature,
+            CurvatureVariation = CurvatureVariation,
+            PreviousTrajectory = PreviousTrajectory,
+            RollingTerminal = RollingTerminal,
+        };
+    }
+}
@@ -0,0 +1,32 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class LongitudinalConfiguration
+{
+    public double MaximumForwardSpeedMetersPerSecond { get; set; }
+    public double MaximumReverseSpeedMetersPerSecond { get; set; }
+    public double MaximumAccelerationMetersPerSecondSquared { get; set; }
+    public double MaximumDecelerationMetersPerSecondSquared { get; set; }
+    public double MaximumJerkMetersPerSecondCubed { get; set; }
+    public double MaximumLateralAccelerationMetersPerSecondSquared { get; set; }
+    public double MaximumCurvatureRatePerMeterPerSecond { get; set; }
+    public double StopSpeedToleranceMetersPerSecond { get; set; }
+    public double ZeroSpeedHoldSeconds { get; set; }
+    public LongitudinalWeights Weights { get; set; }
+
+    internal LongitudinalConfiguration Copy()
+    {
+        return new LongitudinalConfiguration
+        {
+            MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond,
+            MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond,
+            MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared,
+            MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared,
+            MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed,
+            MaximumLateralAccelerationMetersPerSecondSquared = MaximumLateralAccelerationMetersPerSecondSquared,
+            MaximumCurvatureRatePerMeterPerSecond = MaximumCurvatureRatePerMeterPerSecond,
+            StopSpeedToleranceMetersPerSecond = StopSpeedToleranceMetersPerSecond,
+            ZeroSpeedHoldSeconds = ZeroSpeedHoldSeconds,
+            Weights = Weights == null ? null : Weights.Copy(),
+        };
+    }
+}
@@ -0,0 +1,22 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class LongitudinalWeights
+{
+    public double ReferenceSpeed { get; set; }
+    public double Acceleration { get; set; }
+    public double Jerk { get; set; }
+    public double PreviousTrajectory { get; set; }
+    public double TerminalAcceleration { get; set; }
+
+    internal LongitudinalWeights Copy()
+    {
+        return new LongitudinalWeights
+        {
+            ReferenceSpeed = ReferenceSpeed,
+            Acceleration = Acceleration,
+            Jerk = Jerk,
+            PreviousTrajectory = PreviousTrajectory,
+            TerminalAcceleration = TerminalAcceleration,
+        };
+    }
+}
@@ -0,0 +1,26 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class SchedulingConfiguration
+{
+    public double ReplanPeriodSeconds { get; set; }
+    public double TimeHorizonSeconds { get; set; }
+    public double DistanceHorizonMeters { get; set; }
+    public double OutputTimeStepSeconds { get; set; }
+    public double SolverTimeoutSeconds { get; set; }
+    public double HandoffLookaheadSeconds { get; set; }
+    public double MaximumVehicleStateAgeSeconds { get; set; }
+
+    internal SchedulingConfiguration Copy()
+    {
+        return new SchedulingConfiguration
+        {
+            ReplanPeriodSeconds = ReplanPeriodSeconds,
+            TimeHorizonSeconds = TimeHorizonSeconds,
+            DistanceHorizonMeters = DistanceHorizonMeters,
+            OutputTimeStepSeconds = OutputTimeStepSeconds,
+            SolverTimeoutSeconds = SolverTimeoutSeconds,
+            HandoffLookaheadSeconds = HandoffLookaheadSeconds,
+            MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds,
+        };
+    }
+}
@@ -0,0 +1,28 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class SolverConfiguration
+{
+    public int MaximumOuterIterations { get; set; }
+    public int MaximumOsqpIterations { get; set; }
+    public double AbsoluteTolerance { get; set; }
+    public double RelativeTolerance { get; set; }
+    public double StrictResidualTolerance { get; set; }
+    public bool WarmStart { get; set; }
+    public bool Polish { get; set; }
+    public bool NativeVerbose { get; set; }
+
+    internal SolverConfiguration Copy()
+    {
+        return new SolverConfiguration
+        {
+            MaximumOuterIterations = MaximumOuterIterations,
+            MaximumOsqpIterations = MaximumOsqpIterations,
+            AbsoluteTolerance = AbsoluteTolerance,
+            RelativeTolerance = RelativeTolerance,
+            StrictResidualTolerance = StrictResidualTolerance,
+            WarmStart = WarmStart,
+            Polish = Polish,
+            NativeVerbose = NativeVerbose,
+        };
+    }
+}
@@ -0,0 +1,16 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class ValidationConfiguration
+{
+    public double SpatialToleranceMeters { get; set; }
+    public double KinematicTolerance { get; set; }
+
+    internal ValidationConfiguration Copy()
+    {
+        return new ValidationConfiguration
+        {
+            SpatialToleranceMeters = SpatialToleranceMeters,
+            KinematicTolerance = KinematicTolerance,
+        };
+    }
+}
@@ -12,17 +12,6 @@ public sealed class VehicleMotionState
        DateTimeOffset capturedAtUtc,
        long sequenceId)
    {
-        if (pose == null)
-            throw new ArgumentNullException(nameof(pose));
-        ContractNumeric.RequireFinite(pose.X, nameof(pose));
-        ContractNumeric.RequireFinite(pose.Y, nameof(pose));
-        ContractNumeric.RequireFinite(pose.Heading, nameof(pose));
-        ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond));
-        if (longitudinalAccelerationMetersPerSecondSquared.HasValue)
-            ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared));
-        if (sequenceId < 0)
-            throw new ArgumentOutOfRangeException(nameof(sequenceId));
-
        Pose = pose;
        SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
        LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
@@ -0,0 +1,13 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlannerDebugOptions
+{
+    public bool EnableSummary { get; set; }
+    public bool EnableProjectionTrace { get; set; }
+    public bool EnableCorridorTrace { get; set; }
+    public bool EnableLateralSolverTrace { get; set; }
+    public bool EnableLongitudinalSolverTrace { get; set; }
+    public bool EnableTrajectoryDump { get; set; }
+    public bool EnableVisualization { get; set; }
+    public IEmPlannerDebugSink Sink { get; set; }
+}
@@ -0,0 +1,30 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlanningDiagnostics
+{
+    private readonly List<string> _debugSinkFailures = new List<string>();
+
+    public IReadOnlyList<string> DebugSinkFailures
+    {
+        get { return new ReadOnlyCollection<string>(new List<string>(_debugSinkFailures)); }
+    }
+
+    public void WriteDebug(EmPlannerDebugOptions options, string message)
+    {
+        if (options == null || options.Sink == null)
+            return;
+
+        try
+        {
+            options.Sink.Write(message ?? string.Empty);
+        }
+        catch (Exception exception)
+        {
+            _debugSinkFailures.Add(exception.GetType().FullName + ": " + exception.Message);
+        }
+    }
+}
@@ -0,0 +1,6 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public interface IEmPlannerDebugSink
+{
+    void Write(string message);
+}
@@ -0,0 +1,165 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class EmPlanningRequestValidationResult
+{
+    internal EmPlanningRequestValidationResult(EmPlanningStatus status, string failureReason, EmPlanningRequestSnapshot snapshot)
+    {
+        Status = status;
+        FailureReason = failureReason ?? string.Empty;
+        Snapshot = snapshot;
+    }
+
+    public EmPlanningStatus Status { get; }
+
+    public string FailureReason { get; }
+
+    public bool IsValid { get { return Status == EmPlanningStatus.Success; } }
+
+    internal EmPlanningRequestSnapshot Snapshot { get; }
+}
+
+internal sealed class EmPlanningRequestSnapshot
+{
+    public EmPlanningRequestSnapshot(EmPlanningRequest request, EmPlannerConfiguration configuration)
+    {
+        Request = request;
+        Configuration = configuration;
+    }
+
+    public EmPlanningRequest Request { get; }
+
+    public EmPlannerConfiguration Configuration { get; }
+}
+
+public static class EmPlanningRequestValidator
+{
+    public static EmPlanningRequestValidationResult Validate(EmPlanningRequest request)
+    {
+        if (request == null)
+            return Invalid("Request is required.");
+        if (request.ReferencePath == null || request.Map == null || request.Vehicle == null || request.VehicleState == null || request.Configuration == null)
+            return Invalid("Request members ReferencePath, Map, Vehicle, VehicleState, and Configuration are required.");
+        if (request.MotionModel == EmMotionModel.CrabTranslation || request.MotionModel == EmMotionModel.InPlaceRotation)
+            return Result(EmPlanningStatus.UnsupportedMotionMode, "Only nonholonomic forward/reverse motion is supported.");
+        if (request.MotionModel != EmMotionModel.NonholonomicForwardReverse)
+            return Invalid("Motion model is invalid.");
+        if (!TryValidateConfiguration(request.Configuration, out string configurationFailure))
+            return Invalid(configurationFailure);
+        if (!request.Map.PlanningReady)
+            return Invalid("Planning map is not ready.");
+        if (!IsConsumable(request.ReferencePath.Status))
+            return Result(EmPlanningStatus.InvalidReferencePath, "Reference path status is not consumable.");
+        if (request.ReferencePath.Path == null || request.ReferencePath.Segments == null ||
+            request.SegmentIndex < 0 || request.SegmentIndex >= request.ReferencePath.Segments.Count)
+            return Result(EmPlanningStatus.InvalidReferencePath, "Requested direction segment is out of range.");
+        if (!IsValidVehicleState(request.VehicleState))
+            return Invalid("Vehicle state contains invalid values.");
+        if (!IsValidVehicle(request.Vehicle))
+            return Invalid("Vehicle geometry or curvature limit is invalid.");
+        if (request.RequestedAtUtc - request.VehicleState.CapturedAtUtc >
+            TimeSpan.FromSeconds(request.Configuration.Scheduling.MaximumVehicleStateAgeSeconds))
+            return Result(EmPlanningStatus.StaleVehicleState, "Vehicle state is stale.");
+
+        return new EmPlanningRequestValidationResult(
+            EmPlanningStatus.Success,
+            string.Empty,
+            new EmPlanningRequestSnapshot(request, request.Configuration.Copy()));
+    }
+
+    private static EmPlanningRequestValidationResult Invalid(string reason)
+    {
+        return Result(EmPlanningStatus.InvalidInput, reason);
+    }
+
+    private static EmPlanningRequestValidationResult Result(EmPlanningStatus status, string reason)
+    {
+        return new EmPlanningRequestValidationResult(status, reason, null);
+    }
+
+    private static bool IsConsumable(PathSmoothingStatus status)
+    {
+        return status == PathSmoothingStatus.Complete ||
+            status == PathSmoothingStatus.PartialImprovement ||
+            status == PathSmoothingStatus.NotNeeded ||
+            status == PathSmoothingStatus.Unchanged;
+    }
+
+    private static bool IsValidVehicleState(VehicleMotionState state)
+    {
+        return state.Pose != null &&
+            NumericGuard.IsFinite(state.Pose.X) &&
+            NumericGuard.IsFinite(state.Pose.Y) &&
+            NumericGuard.IsFinite(state.Pose.Heading) &&
+            NumericGuard.IsFinite(state.SignedLongitudinalSpeedMetersPerSecond) &&
+            (!state.LongitudinalAccelerationMetersPerSecondSquared.HasValue || NumericGuard.IsFinite(state.LongitudinalAccelerationMetersPerSecondSquared.Value)) &&
+            state.SequenceId >= 0;
+    }
+
+    private static bool IsValidVehicle(MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters vehicle)
+    {
+        return NumericGuard.IsPositiveFinite(vehicle.LengthMeters) &&
+            NumericGuard.IsPositiveFinite(vehicle.WidthMeters) &&
+            NumericGuard.IsFinite(vehicle.SafetyMarginMeters) && vehicle.SafetyMarginMeters >= 0d &&
+            VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _);
+    }
+
+    private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, out string failureReason)
+    {
+        failureReason = "Configuration is invalid.";
+        if (configuration.Scheduling == null || configuration.Corridor == null || configuration.Frenet == null ||
+            configuration.Lateral == null || configuration.Longitudinal == null || configuration.Solver == null ||
+            configuration.Validation == null || configuration.Lateral.Weights == null || configuration.Longitudinal.Weights == null)
+            return false;
+
+        SchedulingConfiguration scheduling = configuration.Scheduling;
+        CorridorConfiguration corridor = configuration.Corridor;
+        FrenetConfiguration frenet = configuration.Frenet;
+        LateralConfiguration lateral = configuration.Lateral;
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        SolverConfiguration solver = configuration.Solver;
+        ValidationConfiguration validation = configuration.Validation;
+
+        if (!Positive(scheduling.ReplanPeriodSeconds) || !Positive(scheduling.TimeHorizonSeconds) || !Positive(scheduling.DistanceHorizonMeters) ||
+            !Positive(scheduling.OutputTimeStepSeconds) || !Positive(scheduling.SolverTimeoutSeconds) || !Positive(scheduling.HandoffLookaheadSeconds) || !Positive(scheduling.MaximumVehicleStateAgeSeconds) ||
+            !Positive(corridor.LongitudinalSampleSpacingMeters) || !Positive(corridor.LateralSampleSpacingMeters) || !Positive(corridor.MaximumLateralOffsetMeters) ||
+            !NonNegative(corridor.AdditionalClearanceReserveMeters) || !Positive(corridor.MaximumCollisionCheckStepMeters) ||
+            !Positive(frenet.MaximumProjectionDistanceMeters) || !NumericGuard.IsFinite(frenet.MinimumFrenetDenominator) ||
+            frenet.MinimumFrenetDenominator <= 0d || frenet.MinimumFrenetDenominator >= 1d || !Positive(frenet.BoundaryAnchorToleranceMeters) ||
+            !Positive(lateral.MaximumLateralStepPerIterationMeters) || !Positive(lateral.MaximumLateralSlope) ||
+            !Positive(lateral.MaximumLateralSecondDerivativePerMeter) || !Positive(lateral.MaximumLateralThirdDerivativePerSquareMeter) ||
+            !Positive(longitudinal.MaximumForwardSpeedMetersPerSecond) || !Positive(longitudinal.MaximumReverseSpeedMetersPerSecond) ||
+            !Positive(longitudinal.MaximumAccelerationMetersPerSecondSquared) || !Positive(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
+            !Positive(longitudinal.MaximumJerkMetersPerSecondCubed) || !Positive(longitudinal.MaximumLateralAccelerationMetersPerSecondSquared) ||
+            !Positive(longitudinal.MaximumCurvatureRatePerMeterPerSecond) || !NonNegative(longitudinal.StopSpeedToleranceMetersPerSecond) ||
+            !Positive(longitudinal.ZeroSpeedHoldSeconds) || solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
+            !Positive(solver.AbsoluteTolerance) || !Positive(solver.RelativeTolerance) || !Positive(solver.StrictResidualTolerance) ||
+            !Positive(validation.SpatialToleranceMeters) || !Positive(validation.KinematicTolerance) ||
+            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
+            return false;
+
+        return true;
+    }
+
+    private static bool Positive(double value) { return NumericGuard.IsPositiveFinite(value); }
+
+    private static bool NonNegative(double value) { return NumericGuard.IsFinite(value) && value >= 0d; }
+
+    private static bool WeightsAreValid(LateralWeights weights)
+    {
+        return NonNegative(weights.ReferenceOffset) && NonNegative(weights.HeadingDeviation) &&
+            NonNegative(weights.SecondDerivative) && NonNegative(weights.ThirdDerivative) &&
+            NonNegative(weights.Curvature) && NonNegative(weights.CurvatureVariation) &&
+            NonNegative(weights.PreviousTrajectory) && NonNegative(weights.RollingTerminal);
+    }
+
+    private static bool WeightsAreValid(LongitudinalWeights weights)
+    {
+        return NonNegative(weights.ReferenceSpeed) && NonNegative(weights.Acceleration) && NonNegative(weights.Jerk) &&
+            NonNegative(weights.PreviousTrajectory) && NonNegative(weights.TerminalAcceleration);
+    }
+}