core: implement etcs braking simulator

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

core/envelope-sim/build.gradle

@@ -20,6 +20,7 @@ dependencies {
 
     implementation project(':osrd-reporting')
     implementation project(":kt-osrd-sim-infra")
+    api project(":osrd-railjson")
     api project(":kt-osrd-utils")
     implementation libs.guava
     implementation libs.slf4j

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

@@ -1,5 +1,6 @@
 package fr.sncf.osrd.envelope;
 
+import static fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.GRAVITY_ACCELERATION;
 import static fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.areAccelerationsEqual;
 import static java.lang.Math.max;
 
@@ -217,7 +218,7 @@ public static double getPartMechanicalEnergyConsumed(
         var meanGrade = 0.001 * path.getAverageGrade(part.getBeginPos(), part.getEndPos());
         var altitudeDelta = meanGrade * part.getTotalDistance();
 
-        var workGravity = -mass * 9.81 * altitudeDelta;
+        var workGravity = -mass * GRAVITY_ACCELERATION * altitudeDelta;
 
         var kineticEnergyDelta =
                 0.5 * inertia * (part.getEndSpeed() * part.getEndSpeed() - part.getBeginSpeed() * part.getBeginSpeed());

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

@@ -106,6 +106,31 @@ public double getAverageGrade(double begin, double end) {
         return (getCumGrade(end) - getCumGrade(begin)) / (end - begin);
     }
 
+    @Override
+    public double getMinGrade(double begin, double end) {
+        // TODO: Optimise method by adding in a cache.
+        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) {
+        // TODO: Optimise method by using binary search.
+        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

@@ -4,6 +4,9 @@ public interface PhysicsPath {
     /** The length of the path, in meters */
     double getLength();
 
-    /** The average slope on a given range, in meters per kilometers */
+    /** The average slope on a given range, in m/km */
     double getAverageGrade(double begin, double end);
+
+    /** The lowest slope on a given range, in m/km */
+    double getMinGrade(double begin, double end);
 }

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

@@ -1,5 +1,11 @@
 package fr.sncf.osrd.envelope_sim;
 
+import static fr.sncf.osrd.envelope_sim.TrainPhysicsIntegrator.GRAVITY_ACCELERATION;
+import static fr.sncf.osrd.envelope_sim.etcs.ConstantsKt.mRotatingMax;
+import static fr.sncf.osrd.envelope_sim.etcs.ConstantsKt.mRotatingMin;
+
+import fr.sncf.osrd.railjson.schema.rollingstock.RJSEtcsBrakeParams;
+
 public interface PhysicsRollingStock {
     /** The mass of the train, in kilograms */
     double getMass();
@@ -19,6 +25,8 @@ public interface PhysicsRollingStock {
     /** The first derivative of the resistance to movement at a given speed, in kg/s */
     double getRollingResistanceDeriv(double speed);
 
+    RJSEtcsBrakeParams getRJSEtcsBrakeParams();
+
     /** 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 +58,16 @@ 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².
+     * Grade is in m/km.
+     * mRotating (Max or Min) is in %, as seen in ERA braking curves simulation tool v5.1.
+     */
+    static double getGradientAcceleration(double grade) {
+        var mRotating = grade >= 0 ? mRotatingMax : mRotatingMin;
+        return -GRAVITY_ACCELERATION * grade / (1000.0 + 10.0 * mRotating);
+    }
+
     /** The maximum constant deceleration, in m/s^2 */
     double getDeceleration();
 

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

@@ -1,14 +1,21 @@
 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 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.
  */
 public final class TrainPhysicsIntegrator {
+    // Gravity acceleration, in m/s²
+    public static final double GRAVITY_ACCELERATION = 9.81;
+
     // A position delta lower than this value will be considered zero
     // Going back and forth with Distance and double (meters) often causes 1e-3 errors,
     // we need the tolerance to be higher than this
@@ -25,18 +32,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 +56,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,37 +86,69 @@ 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 gradientAcceleration = getGradientAcceleration(grade);
+        return switch (brakingType) {
+                // See Subset referenced in RJSEtcsBrakeParams: §3.13.6.2.1.3.
+            case ETCS_EBD -> -rollingStock.getRJSEtcsBrakeParams().getSafeBrakingAcceleration(speed)
+                    + gradientAcceleration;
+                // See Subset referenced in RJSEtcsBrakeParams: §3.13.6.3.1.3.
+            case ETCS_SBD -> -rollingStock.getRJSEtcsBrakeParams().getServiceBrakingAcceleration(speed)
+                    + gradientAcceleration;
+                // See Subset referenced in RJSEtcsBrakeParams: §3.13.6.4.3.
+            case ETCS_GUI -> -rollingStock.getRJSEtcsBrakeParams().getNormalServiceBrakingAcceleration(speed)
+                    + gradientAcceleration
+                    + rollingStock.getRJSEtcsBrakeParams().getGradientAccelerationCorrection(grade, 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);
-        // get an angle from a meter per km elevation difference
+        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 m/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
-        return -rollingStock.getMass() * 9.81 * Math.sin(angle);
+        var angle = Math.atan(grade / 1000.0); // from m/km to m/m
+        return -rollingStock.getMass() * GRAVITY_ACCELERATION * 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);
     }
 
     /**
@@ -107,15 +160,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,6 @@
+package fr.sncf.osrd.envelope_sim.etcs
+
+/** See Subset referenced in ETCSBrakingSimulator: table in Appendix A.3.1. */
+const val tDriver = 4.0 // s
+const val mRotatingMax = 15.0 // %
+const val mRotatingMin = 2.0 // %