core: implement etcs braking simulator

Signed-off-by: Erashin <[email protected]>

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope/part/EnvelopePart.java

@@ -596,19 +596,19 @@ public EnvelopePart slice(
      * Returns a new EnvelopePart, where all positions are shifted by positionDelta. Resulting
      * positions are clipped to [minPosition; maxPosition].
      */
-    public EnvelopePart copyAndShift(double positionDelta, double minPosition, double maxPosition) {
+    public EnvelopePart copyAndShift(double positionDelta, double minPosition, double maxPosition, double speedDelta) {
         var newPositions = new DoubleArrayList();
         var newSpeeds = new DoubleArrayList();
         var newTimeDeltas = new DoubleArrayList();
         newPositions.add(positions[0] + positionDelta);
-        newSpeeds.add(speeds[0]);
+        newSpeeds.add(speeds[0] + speedDelta);
         for (int i = 1; i < positions.length; i++) {
             var p = Math.max(minPosition, Math.min(maxPosition, positions[i] + positionDelta));
             if (newPositions.get(newPositions.size() - 1) != p) {
                 // Positions that are an epsilon away may be overlapping after the shift, we only
                 // add the distinct ones
                 newPositions.add(p);
-                newSpeeds.add(speeds[i]);
+                newSpeeds.add(speeds[i] + speedDelta);
                 newTimeDeltas.add(timeDeltas[i - 1]);
             }
         }

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/EnvelopeSimPath.java

@@ -106,6 +106,29 @@ public double getAverageGrade(double begin, double end) {
         return (getCumGrade(end) - getCumGrade(begin)) / (end - begin);
     }
 
+    @Override
+    public double getMinGrade(double begin, double end) {
+        int indexBegin = getIndexBeforePos(begin);
+        int indexEnd = getIndexBeforePos(end);
+        var lowestGradient = gradeValues[indexBegin];
+        for (int i = indexBegin; i < indexEnd; i++) {
+            var grad = gradeValues[i];
+            if (grad < lowestGradient) lowestGradient = grad;
+        }
+        return lowestGradient;
+    }
+
+    /** For a given position, return the index of the position just before in gradePositions */
+    public int getIndexBeforePos(double position) {
+        if (position <= gradePositions[0]) return 0;
+        if (position >= gradePositions[gradePositions.length - 1]) return gradePositions.length - 1;
+        for (int i = 0; i < gradePositions.length; i++) {
+            var pos = gradePositions[i];
+            if (pos > position) return i - 1;
+        }
+        return gradePositions.length - 1;
+    }
+
     private RangeMap<Double, Electrification> getModeAndProfileMap(
             String powerClass, Range<Double> range, boolean ignoreElectricalProfiles) {
         if (ignoreElectricalProfiles) powerClass = null;

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/PhysicsPath.java

@@ -6,4 +6,7 @@ public interface PhysicsPath {
 
     /** The average slope on a given range, in meters per kilometers */
     double getAverageGrade(double begin, double end);
+
+    /** The lowest slope on a given range, in meters per kilometers */
+    double getMinGrade(double begin, double end);
 }

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/PhysicsRollingStock.java

@@ -19,6 +19,26 @@ public interface PhysicsRollingStock {
     /** The first derivative of the resistance to movement at a given speed, in kg/s */
     double getRollingResistanceDeriv(double speed);
 
+    double getTractionCutOff();
+
+    double getTBs1();
+
+    double getTBs2();
+
+    double getTBe();
+
+    /** The emergency braking acceleration which can be applied at a given speed, in m/s^2 */
+    double getSafeBrakingAcceleration(double speed);
+
+    /** The service braking acceleration which can be applied at a given speed, in m/s^2 */
+    double getServiceBrakingAcceleration(double speed);
+
+    /** The normal service braking acceleration which can be applied at a given speed, in m/s^2 */
+    double getNormalServiceBrakingAcceleration(double speed);
+
+    /** The gradient acceleration correction applied for guidance computation in ETCS 2, in m/s^2 */
+    double getGradientAccelerationCorrection(double grade, double speed);
+
     /** Get the effort the train can apply at a given speed, in newtons */
     static double getMaxEffort(double speed, TractiveEffortPoint[] tractiveEffortCurve) {
         int index = 0;
@@ -50,6 +70,11 @@ static double getMaxEffort(double speed, TractiveEffortPoint[] tractiveEffortCur
         return previousPoint.maxEffort() + coeff * (Math.abs(speed) - previousPoint.speed());
     }
 
+    /** The gradient acceleration of the rolling stock taking its rotating mass into account, in m/s^2 */
+    static double getGradientAcceleration(double grade, double rotatingMassPercentage) {
+        return -9.81 * grade / (1000.0 + 10.0 * rotatingMassPercentage);
+    }
+
     /** The maximum constant deceleration, in m/s^2 */
     double getDeceleration();
 

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/TrainPhysicsIntegrator.java

@@ -1,9 +1,15 @@
 package fr.sncf.osrd.envelope_sim;
 
+import static fr.sncf.osrd.envelope_sim.PhysicsRollingStock.getGradientAcceleration;
+import static fr.sncf.osrd.envelope_sim.PhysicsRollingStock.getMaxEffort;
+import static fr.sncf.osrd.envelope_sim.etcs.ConstantsKt.mRotatingMax;
+import static fr.sncf.osrd.envelope_sim.etcs.ConstantsKt.mRotatingMin;
+
 import com.google.common.collect.RangeMap;
+import fr.sncf.osrd.envelope_sim.etcs.BrakingType;
 
 /**
- * An utility class to help simulate the train, using numerical integration. It's used when
+ * A utility class to help simulate the train, using numerical integration. It's used when
  * simulating the train, and it is passed to speed controllers so they can take decisions about what
  * action to make. Once speed controllers took a decision, this same class is used to compute the
  * next position and speed of the train.
@@ -25,18 +31,21 @@ public final class TrainPhysicsIntegrator {
     private final double directionSign;
 
     private final RangeMap<Double, PhysicsRollingStock.TractiveEffortPoint[]> tractiveEffortCurveMap;
+    private final BrakingType brakingType;
 
     private TrainPhysicsIntegrator(
             PhysicsRollingStock rollingStock,
             PhysicsPath path,
             Action action,
             double directionSign,
-            RangeMap<Double, PhysicsRollingStock.TractiveEffortPoint[]> tractiveEffortCurveMap) {
+            RangeMap<Double, PhysicsRollingStock.TractiveEffortPoint[]> tractiveEffortCurveMap,
+            BrakingType brakingType) {
         this.rollingStock = rollingStock;
         this.path = path;
         this.action = action;
         this.directionSign = directionSign;
         this.tractiveEffortCurveMap = tractiveEffortCurveMap;
+        this.brakingType = brakingType;
     }
 
     /** Simulates train movement */
@@ -46,8 +55,19 @@ public static IntegrationStep step(
             double initialSpeed,
             Action action,
             double directionSign) {
+        return step(context, initialLocation, initialSpeed, action, directionSign, BrakingType.CONSTANT);
+    }
+
+    /** Simulates train movement */
+    public static IntegrationStep step(
+            EnvelopeSimContext context,
+            double initialLocation,
+            double initialSpeed,
+            Action action,
+            double directionSign,
+            BrakingType brakingType) {
         var integrator = new TrainPhysicsIntegrator(
-                context.rollingStock, context.path, action, directionSign, context.tractiveEffortCurveMap);
+                context.rollingStock, context.path, action, directionSign, context.tractiveEffortCurveMap, brakingType);
         return integrator.step(context.timeStep, initialLocation, initialSpeed, directionSign);
     }
 
@@ -65,39 +85,66 @@ private IntegrationStep step(double timeStep, double initialLocation, double ini
     }
 
     private IntegrationStep step(double timeStep, double position, double speed) {
+        if (action == Action.BRAKE) return newtonStep(timeStep, speed, getDeceleration(speed, position), directionSign);
+
         double tractionForce = 0;
         var tractiveEffortCurve = tractiveEffortCurveMap.get(Math.min(Math.max(0, position), path.getLength()));
         assert tractiveEffortCurve != null;
-        double maxTractionForce = PhysicsRollingStock.getMaxEffort(speed, tractiveEffortCurve);
+        double maxTractionForce = getMaxEffort(speed, tractiveEffortCurve);
         double rollingResistance = rollingStock.getRollingResistance(speed);
-        double weightForce = getWeightForce(rollingStock, path, position);
-
-        if (action == Action.ACCELERATE) tractionForce = maxTractionForce;
-
-        boolean isBraking = (action == Action.BRAKE);
+        double averageGrade = getAverageGrade(rollingStock, path, position);
+        double weightForce = getWeightForce(rollingStock, averageGrade);
 
         if (action == Action.MAINTAIN) {
             tractionForce = rollingResistance - weightForce;
             if (tractionForce <= maxTractionForce) return newtonStep(timeStep, speed, 0, directionSign);
             else tractionForce = maxTractionForce;
         }
 
-        double acceleration = computeAcceleration(
-                rollingStock, rollingResistance, weightForce, speed, tractionForce, isBraking, directionSign);
+        if (action == Action.ACCELERATE) tractionForce = maxTractionForce;
+        double acceleration =
+                computeAcceleration(rollingStock, rollingResistance, weightForce, speed, tractionForce, directionSign);
         return newtonStep(timeStep, speed, acceleration, directionSign);
     }
 
-    /** Compute the weight force of a rolling stock at a given position on a given path */
-    public static double getWeightForce(PhysicsRollingStock rollingStock, PhysicsPath path, double headPosition) {
+    private double getDeceleration(double speed, double position) {
+        assert (action == Action.BRAKE);
+        if (brakingType == BrakingType.CONSTANT) return rollingStock.getDeceleration();
+        var grade = getMinGrade(rollingStock, path, position);
+        var mRotating = grade >= 0 ? mRotatingMax : mRotatingMin;
+        var gradientAcceleration = getGradientAcceleration(grade, mRotating);
+        return switch (brakingType) {
+            case ETCS_EBD -> -rollingStock.getSafeBrakingAcceleration(speed) + gradientAcceleration;
+            case ETCS_SBD -> -rollingStock.getServiceBrakingAcceleration(speed) + gradientAcceleration;
+            case ETCS_GUI -> -rollingStock.getNormalServiceBrakingAcceleration(speed)
+                    + gradientAcceleration
+                    + rollingStock.getGradientAccelerationCorrection(gradientAcceleration, speed);
+            default -> throw new UnsupportedOperationException("Braking type not supported: " + brakingType);
+        };
+    }
+
+    /** Compute the average grade of a rolling stock at a given position on a given path in m/km */
+    public static double getAverageGrade(PhysicsRollingStock rollingStock, PhysicsPath path, double headPosition) {
         var tailPosition = Math.min(Math.max(0, headPosition - rollingStock.getLength()), path.getLength());
         headPosition = Math.min(Math.max(0, headPosition), path.getLength());
-        var averageGrade = path.getAverageGrade(tailPosition, headPosition);
+        return path.getAverageGrade(tailPosition, headPosition);
+    }
+
+    /** Compute the weight force of a rolling stock at a given position on a given path */
+    public static double getWeightForce(PhysicsRollingStock rollingStock, double grade) {
         // get an angle from a meter per km elevation difference
         // the curve's radius is taken into account in meanTrainGrade
-        var angle = Math.atan(averageGrade / 1000.0); // from m/km to m/m
+        var angle = Math.atan(grade / 1000.0); // from m/km to m/m
         return -rollingStock.getMass() * 9.81 * Math.sin(angle);
     }
 
+    /** Compute the min grade of a rolling stock at a given position on a given path in m/km */
+    public static double getMinGrade(PhysicsRollingStock rollingStock, PhysicsPath path, double headPosition) {
+        var tailPosition = Math.min(Math.max(0, headPosition - rollingStock.getLength()), path.getLength());
+        headPosition = Math.min(Math.max(0, headPosition), path.getLength());
+        return path.getMinGrade(tailPosition, headPosition);
+    }
+
     /**
      * Compute the acceleration given a rolling stock, different forces, a speed, and a direction
      */
@@ -107,15 +154,10 @@ public static double computeAcceleration(
             double weightForce,
             double currentSpeed,
             double tractionForce,
-            boolean isBraking,
             double directionSign) {
 
         assert tractionForce >= 0.;
 
-        if (isBraking) {
-            return rollingStock.getDeceleration();
-        }
-
         if (currentSpeed == 0 && directionSign > 0) {
             // If we are stopped and if the forces are not enough to compensate the opposite force,
             // the rolling resistance and braking force don't apply and the speed stays at 0

core/envelope-sim/src/main/java/fr/sncf/osrd/envelope_sim/overlays/EnvelopeDeceleration.java

@@ -4,6 +4,7 @@
 import fr.sncf.osrd.envelope_sim.Action;
 import fr.sncf.osrd.envelope_sim.EnvelopeSimContext;
 import fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator;
+import fr.sncf.osrd.envelope_sim.etcs.BrakingType;
 
 public class EnvelopeDeceleration {
     /** Generate a deceleration curve overlay */
@@ -12,15 +13,25 @@ public static void decelerate(
             double startPosition,
             double startSpeed,
             InteractiveEnvelopePartConsumer consumer,
-            double direction) {
+            double direction,
+            BrakingType brakingType) {
         if (!consumer.initEnvelopePart(startPosition, startSpeed, direction)) return;
         double position = startPosition;
         double speed = startSpeed;
         while (true) {
-            var step = TrainPhysicsIntegrator.step(context, position, speed, Action.BRAKE, direction);
+            var step = TrainPhysicsIntegrator.step(context, position, speed, Action.BRAKE, direction, brakingType);
             position += step.positionDelta;
             speed = step.endSpeed;
             if (!consumer.addStep(position, speed, step.timeDelta)) break;
         }
     }
+
+    public static void decelerate(
+            EnvelopeSimContext context,
+            double startPosition,
+            double startSpeed,
+            InteractiveEnvelopePartConsumer consumer,
+            double direction) {
+        decelerate(context, startPosition, startSpeed, consumer, direction, BrakingType.CONSTANT);
+    }
 }

core/envelope-sim/src/main/kotlin/fr/sncf/osrd/envelope_sim/etcs/Constants.kt

@@ -0,0 +1,11 @@
+package fr.sncf.osrd.envelope_sim.etcs
+
+/** National Default Value: Available Adhesion */
+const val mNvavadh = 0.0
+
+/** National Default Value: Emergency Brake Confidence Level in % */
+const val mNvebcl = 1 - 1E-9
+
+const val tDriver = 4.0 // s
+const val mRotatingMax = 15.0 // %
+const val mRotatingMin = 2.0 // %