add trajectory optimization functionality

src/NeTrainSim/main.cpp

@@ -114,6 +114,21 @@ int main(int argc, char *argv[])
                                             QCoreApplication::translate("main", "[Optional] the simulator time step. \nDefault is '1.0'."), "simulatorTimeStep", "1.0");
     parser.addOption(timeStepOption);
 
+    const QCommandLineOption enableOptimization(QStringList() << "z" << "optimization",
+                                                QCoreApplication::translate("main", "[Optional] bool to enable single train trajectory optimization. \nDefault is 'false'"), "optimization", "false");
+    parser.addOption(enableOptimization);
+
+    const QCommandLineOption optimizationEvery(QStringList() << "y" << "optimizeEvery",
+                                               QCoreApplication::translate("main", "[Optional] the optimization frequency. \nDefault is '1'."), "optimizeEvery", "1");
+    parser.addOption(optimizationEvery);
+
+    const QCommandLineOption optimizationLookahead(QStringList() << "d" << "optimizerLookahead",
+                                                   QCoreApplication::translate("main", "[Optional] the forward lookahead distance for the optimizer. \nDefault is '1'."), "optimizerLookahead", "1");
+    parser.addOption(optimizationLookahead);
+
+    const QCommandLineOption optimizationSpeedPriorityFactor(QStringList() << "k" << "OptimizationSpeedFactor",
+                                                             QCoreApplication::translate("main", "[Optional] the speed priority factor in case of optimization. \n Default is '0.0'."), "OptimizationSpeedFactor", "0.0");
+    parser.addOption(optimizationSpeedPriorityFactor);
 
     // process all the arguments
     parser.process(app);
@@ -136,6 +151,10 @@ int main(int argc, char *argv[])
     std::string nodesFile, linksFile, trainsFile, exportLocation, summaryFilename, instaTrajFilename;
     bool exportInstaTraj = false;
     double timeStep = 1.0;
+    bool optimize = false;
+    double optimize_speedfactor = 0.0;
+    int optimizerFrequency = 0;
+    int lookahead = 0;
 
     // read values from the cmd
     // read required values
@@ -174,13 +193,35 @@ int main(int argc, char *argv[])
     if (checkParserValue(parser, timeStepOption, "", false)) { timeStep = parser.value(timeStepOption).toDouble(); }
     else { timeStep = 1.0; }
 
+    if (checkParserValue(parser, enableOptimization, "", false)){
+        stringstream ss(parser.value(enableOptimization).toStdString());
+        ss >> std::boolalpha >> optimize;
+    }
+    else { optimize = false; }
+
+    if (checkParserValue(parser, optimizationEvery, "", false)) { optimizerFrequency = parser.value(optimizationEvery).toInt(); }
+    else { optimizerFrequency = 1.0; }
+    if (checkParserValue(parser, optimizationLookahead, "", false)) { lookahead = parser.value(optimizationLookahead).toInt(); }
+    else { lookahead = 1.0; }
+
+    if (checkParserValue(parser, optimizationSpeedPriorityFactor, "", 0.0)) {optimize_speedfactor = parser.value(optimizationSpeedPriorityFactor).toDouble(); }
+    else { optimize_speedfactor = 0.0;}
+
+    qDebug() << "Optimize?: " << optimize
+             << ", Optimizer Frequency: " << optimizerFrequency
+             << ", Lookahead: " << lookahead
+             << ", Optimize SpeedFactor: " << optimize_speedfactor
+             << "\n";
+
+//    exit(0);
+
     Network* net;
     try {
         std::cout << "Reading Trains!                 \r";
         Vector<std::shared_ptr<Train>> trains = TrainsList::ReadAndGenerateTrains(trainsFile);
 
         if (trains.empty()) {
-            ErrorHandler::showError("No Trains Found!");
+            ErrorHandler::showError("No Trains Found!                    ");
             return -1;
         }
         for (auto &t: trains) {
@@ -191,6 +232,9 @@ int main(int argc, char *argv[])
             QEventLoop::connect(t.get(), &Train::suddenAccelerationOccurred,
                     [](const auto &msg){
                 ErrorHandler::showWarning(msg);});
+
+            t->setOptimization(optimize, optimize_speedfactor,
+                               optimizerFrequency, lookahead);
         }
         std::cout << "Reading Network!                \r";
         net = new Network(nodesFile, linksFile);

src/NeTrainSim/simulator.cpp

@@ -627,24 +627,25 @@ void Simulator::playTrainOneTimeStep(std::shared_ptr <Train> train)
 		}
 		// write the trajectory step data
 		if (this->exportTrajectory) {
-			std::stringstream exportLine;
-			exportLine << train->trainUserID << ","
-				<< this->simulationTime << ","
-				<< train->travelledDistance << ","
-				<< train->currentAcceleration << ","
-				<< train->currentSpeed << ","
-				<< currentFreeFlowSpeed << ","
-				<< train->energyStat << ","
-				<< train->maxDelayTimeStat << ","
-				<< train->delayTimeStat << ","
-				<< train->stoppedStat << ","
-				<< train->currentTractiveForce << ","
-				<< train->currentResistanceForces << ","
-				<< train->currentUsedTractivePower << ","
-				<< grades[0] << ","
-				<< curvatures[0] << ","
-                << train->locomotives[0]->currentLocNotch
-				<< std::endl;
+            std::stringstream exportLine;
+            exportLine << train->trainUserID << ","
+                       << this->simulationTime << ","
+                       << train->travelledDistance << ","
+                       << train->currentAcceleration << ","
+                       << train->currentSpeed << ","
+                       << currentFreeFlowSpeed << ","
+                       << train->energyStat << ","
+                       << train->maxDelayTimeStat << ","
+                       << train->delayTimeStat << ","
+                       << train->stoppedStat << ","
+                       << train->currentTractiveForce << ","
+                       << train->currentResistanceForces << ","
+                       << train->currentUsedTractivePower << ","
+                       << grades[0] << ","
+                       << curvatures[0] << ","
+                       << train->locomotives[0]->currentLocNotch << ","
+                       << train->optimize
+                       << std::endl;
 
 			// write the step trajectory data to the file
 			this->trajectoryFile << exportLine.str();
@@ -1005,7 +1006,7 @@ void Simulator::runSimulation() {
                    << "Stoppings,tractiveForce_N,"
                    << "ResistanceForces_N,CurrentUsedTractivePower_kw,"
                    << "GradeAtTip_Perc,CurvatureAtTip_Perc,"
-                   << "FirstLocoNotchPosition\n";
+                   << "FirstLocoNotchPosition,optimizationEnabled\n";
 		this->trajectoryFile << exportLine.str();
 	}
 
@@ -1087,6 +1088,10 @@ void Simulator::runSimulation() {
         << "  |_ Total Signals                                                              \x1D : " << Utils::thousandSeparator(this->network->networkSignals.size()) << "\n"
         << "  |_ Total Number of Trains on Network                                          \x1D : " << Utils::thousandSeparator(this->trains.size()) << "\n"
         << "  |_ Percentage of Links with Catenaries to All Links (%)                       \x1D : " << Utils::thousandSeparator(std::get<0>(networkStats)) << "\n"
+        << "  |_ Number of Trains with Enabled Trajectory Optimization                      \x1D : " << std::accumulate(this->trains.begin(), this->trains.end(), 0,
+                                                                                                                               [](int total, const auto& train) {
+                                                                                                                                   return total + (int)train->optimize;
+                                                                                                                               }) << "\n"
         << "  |_ Catenary Total Energy Consumed (KW.h)                                      \x1D : " << Utils::thousandSeparator(std::get<1>(networkStats) - std::get<2>(networkStats)) << "\n"
         << "        |_ Average Catenary Energy Consumption per Net Weight (KW.h/ton)        \x1D : " << Utils::thousandSeparator( (std::get<1>(networkStats) - std::get<2>(networkStats)) /
                                                                                                                             std::accumulate(this->trains.begin(), this->trains.end(), 0.0,

src/NeTrainSim/traindefinition/train.cpp

@@ -17,10 +17,15 @@ using namespace std;
 
 unsigned int Train::NumberOfTrainsInSimulator = 0;
 
-Train::Train(int simulatorID, string id, Vector<int> trainPath, double trainStartTime_sec, double frictionCoeff,
-    Vector<std::shared_ptr<Locomotive>> locomotives, Vector<std::shared_ptr<Car>> cars, bool optimize,
-    double desiredDecelerationRate_mPs, double operatorReactionTime_s, bool stopIfNoEnergy,
-             double maxAllowedJerk_mPcs) : QObject(nullptr) {
+Train::Train(int simulatorID, string id, Vector<int> trainPath,
+             double trainStartTime_sec, double frictionCoeff,
+             Vector<std::shared_ptr<Locomotive>> locomotives,
+             Vector<std::shared_ptr<Car>> cars, bool optimize,
+             double desiredDecelerationRate_mPs,
+             double operatorReactionTime_s, bool stopIfNoEnergy,
+             double maxAllowedJerk_mPcs, double optimization_k,
+             int runOptimizationEvery,
+             int optimizationLookaheadSteps) : QObject(nullptr) {
 
     this->d_des = desiredDecelerationRate_mPs;
     this->operatorReactionTime = operatorReactionTime_s;
@@ -46,15 +51,6 @@ Train::Train(int simulatorID, string id, Vector<int> trainPath, double trainStar
     this->setTrainWeight();
     this->resetTrain();
 
-    if (this->optimize){
-        double nMax = 8;
-        for (int n = 0; n <= nMax ; n++){
-            this->throttleLevels.push_back(std::pow( ( ((double)n) / nMax), 2.0));
-        }
-        this->lookAheadCounterToUpdate = DefaultLookAheadCounterToUpdate;
-        this->lookAheadStepCounter = DefaultLookAheadCounter;
-    }
-
     this->WeightCentroids = this->getTrainCentroids();
     int sizeOfPath = this->trainPath.size();
     this->betweenNodesLengths = Vector<Vector<double>>(sizeOfPath, Vector<double>(sizeOfPath));
@@ -70,12 +66,35 @@ Train::Train(int simulatorID, string id, Vector<int> trainPath, double trainStar
     for (auto& loco : this->locomotives) {
         this->ActiveLocos.push_back(loco);
     }
+
+    setOptimization(optimize, optimization_k,
+                    runOptimizationEvery, optimizationLookaheadSteps );
 };
 
 Train::~Train(){
     Train::NumberOfTrainsInSimulator--;
 }
 
+void Train::setOptimization(bool enable,
+                            double optimizationSpeedImportanceNormalizedWeight,
+                            int runOptimizationEvery,
+                            int optimizationLookaheadSteps)
+{
+    optimize = enable;
+    optimizeForSpeedNormalizedWeight =
+        optimizationSpeedImportanceNormalizedWeight;
+
+    for (int n = 0; n <= this->locomotives.front()->Nmax ; n++){
+        this->throttleLevels.push_back(
+            std::pow( ( ((double)n) / this->locomotives.front()->Nmax),
+                     2.0));
+    }
+    this->lookAheadCounterToUpdate = runOptimizationEvery;
+    this->mem_lookAheadCounterToUpdate = runOptimizationEvery;
+    this->lookAheadStepCounter = optimizationLookaheadSteps;
+    this->mem_lookAheadStepCounter = optimizationLookaheadSteps;
+}
+
 void Train::setTrainSimulatorID(int newID){
     this->id = newID;
 }
@@ -513,9 +532,9 @@ double Train::get_acceleration_an2(double gap, double minGap, double speed, doub
 double Train::accelerate(double gap, double mingap, double speed, double acceleration, double leaderSpeed, 
     double freeFlowSpeed, double deltaT, bool optimize, double throttleLevel) {
 
-    //    if (throttleLevel == -1) {
-//        throttleLevel = this->optimumThrottleLevel;
-//    };
+    if (throttleLevel == -1) {
+        throttleLevel = this->optimumThrottleLevel;
+    };
 
     //get the maximum acceleration that the train can go by
     double amax = this->getAccelerationUpperBound(speed, acceleration, freeFlowSpeed, optimize, throttleLevel);
@@ -576,6 +595,16 @@ double Train::getStepAcceleration(double timeStep, double freeFlowSpeed, Vector<
     // decrease the given quota of the future number of steps covered for virtual simulation to update
     if (this->optimize) {
         this->lookAheadCounterToUpdate -= 1;
+        // get the optimum throttle level to drive by
+        if (optimumThrottleLevels.size() > 0)
+        {
+            optimumThrottleLevel = optimumThrottleLevels.front();
+        }
+        if (optimumThrottleLevels.size() > 1)
+        {
+            // remove the used throttle level from the vector
+            optimumThrottleLevels.erase(optimumThrottleLevels.begin());
+        }
     }
 
     // set the min gap to the next train / station
@@ -783,7 +812,8 @@ tuple<double, double, double> Train::AStarOptimization(double prevSpeed, double
 
             // if there is no gap fed to the function, consider a finite gap.
             // this is only in case the train could not calculate the next step gap.
-            if (gapToNextCriticalPoint.size() > 0){
+            if (gapToNextCriticalPoint.size() > 0)
+            {
                 Vector<double> allAcc = Vector<double>();
                 // loop through all the gaps to next train/station
                 for (int i = 0; i < gapToNextCriticalPoint.size(); i ++){
@@ -804,6 +834,7 @@ tuple<double, double, double> Train::AStarOptimization(double prevSpeed, double
             // get the energy for the train if that particular throttle level is used till the end of the look ahead
             double energy = this->heuristicFunction(gapToNextCriticalPoint.back(), stepAcceleration, stepSpeed,
                                                     timeStep, resistance, currentSpeed);
+
             // append the step values to their corresponding vectors
             accelerationVec.push_back(stepAcceleration);
             speedVec.push_back(stepSpeed);
@@ -815,10 +846,47 @@ tuple<double, double, double> Train::AStarOptimization(double prevSpeed, double
     if (energyVec.size() == 0){
         return std::make_tuple(currentSpeed, currentAcceleration, prevThrottle);
     }
-    // get the minimum heuristic energy and the corresponding throttle level
-    int minI = energyVec.argmin();
-    // return the values corresponding to the min energy for the next step analysis
-    return std::make_tuple(speedVec[minI], accelerationVec[minI], throttleVec[minI]);
+
+    // Normalize energy and speed
+    double max_energy = energyVec.min();
+    double min_energy = energyVec.max();
+
+    double max_speed = speedVec.max();
+    double min_speed = speedVec.min();
+    auto normalize = [](double value, double min_value, double max_value) -> double {
+        return max_value != min_value ? (value - min_value) / (max_value - min_value) : 0.5;
+    };
+
+    Vector<double> energyLN;
+    std::vector<double> speedLN;
+
+    std::transform(energyVec.begin(), energyVec.end(), std::back_inserter(energyLN),
+                   [&normalize, &max_energy, &min_energy](double energy) {
+                       return normalize(energy, min_energy, max_energy);
+                   });
+
+    std::transform(speedVec.begin(), speedVec.end(), std::back_inserter(speedLN),
+                   [&normalize, &max_speed, &min_speed](double speed) {
+                       return normalize(speed, min_speed, max_speed);
+                   });
+
+    double weight_energy = 1.0 - optimizeForSpeedNormalizedWeight;
+    double weight_speed = 1 - weight_energy;
+
+    std::vector<double> weighted_sums;
+    for (size_t i = 0; i < energyLN.size(); ++i) {
+        double weighted_sum = weight_energy * energyLN[i] - weight_speed * std::log(speedLN[i] + 1e-6);
+        weighted_sums.push_back(weighted_sum);
+    }
+
+    int idx_optimum = std::distance(weighted_sums.begin(), std::min_element(weighted_sums.begin(), weighted_sums.end()));
+
+    return std::make_tuple(speedVec[idx_optimum], accelerationVec[idx_optimum], throttleVec[idx_optimum]);
+
+//    // get the minimum heuristic energy and the corresponding throttle level
+//    int minI = energyVec.argmin();
+//    // return the values corresponding to the min energy for the next step analysis
+//    return std::make_tuple(speedVec[minI], accelerationVec[minI], throttleVec[minI]);
 }
 
 
@@ -835,12 +903,14 @@ double Train::heuristicFunction(double distanceToEnd, double stepAcceleration, d
 }
 
 double Train::pickOptimalThrottleLevelAStar(Vector<double> throttleLevels, int lookAheadCounterToUpdate) {
-    int end = ( lookAheadCounterToUpdate <= throttleLevels.size()) ? lookAheadCounterToUpdate : throttleLevels.size();
-    if (end == 0){
-        return this->optimumThrottleLevel;
-    }
-    this->optimumThrottleLevel = *std::max_element(throttleLevels.begin(), throttleLevels.begin() + end);
-    return this->optimumThrottleLevel;
+    optimumThrottleLevels = throttleLevels;
+    return 0.0;
+//    int end = ( lookAheadCounterToUpdate <= throttleLevels.size()) ? lookAheadCounterToUpdate : throttleLevels.size();
+//    if (end == 0){
+//        return this->optimumThrottleLevel;
+//    }
+//    this->optimumThrottleLevel = *std::max_element(throttleLevels.begin(), throttleLevels.begin() + end);
+//    return this->optimumThrottleLevel;
 }
 
 pair<Vector<double>, double> Train::getTractivePower(double speed, double acceleration, double resistanceForces) {
@@ -1147,8 +1217,8 @@ bool Train::canProvideEnergy(double &EC, double &timeStep) {
 
 
 void Train::resetTrainLookAhead(){
-    this->lookAheadCounterToUpdate = DefaultLookAheadCounterToUpdate;
-    this->lookAheadStepCounter = DefaultLookAheadCounter;
+    this->lookAheadCounterToUpdate = mem_lookAheadCounterToUpdate;
+    this->lookAheadStepCounter = mem_lookAheadStepCounter;
 }
 
 void Train::resetTrain() {

src/NeTrainSim/traindefinition/train.h

@@ -147,6 +147,11 @@ class Train : public QObject {
     /** holds the waited time at any depot */
     double waitedTimeAtNode;
 
+    /** holds the weight of speed priority*/
+    double optimizeForSpeedNormalizedWeight;
+    /** holds the optimum throttle levels*/
+    Vector<double> optimumThrottleLevels;
+
     /** Holds the current coordinates of the tip of the train */
     pair<double, double> currentCoordinates;
 
@@ -220,8 +225,10 @@ class Train : public QObject {
     int NoPowerCountStep;
     /** The number of steps ahead the train is looking aheaf for optimization */
     int lookAheadStepCounter;
+    int mem_lookAheadStepCounter;
     /** The number of steps ahead the train should update its optimization at */
     int lookAheadCounterToUpdate;
+    int mem_lookAheadCounterToUpdate;
 
 
     /** Change this to true if you want the train to stop if it runs out of energy */
@@ -265,10 +272,23 @@ class Train : public QObject {
           double desiredDecelerationRate_mPs = DefaultDesiredDecelerationRate,
           double operatorReactionTime_s = DefaultOperatorReactionTime,
           bool stopIfNoEnergy = DefaultStopIfNoEnergy,
-          double maxAllowedJerk_mPcs = DefaultMaxAllowedJerk);
+          double maxAllowedJerk_mPcs = DefaultMaxAllowedJerk,
+          double optimization_k = 0.0,
+          int runOptimizationEvery = DefaultLookAheadCounterToUpdate,
+          int optimizationLookaheadSteps = DefaultLookAheadCounter);
 
     ~Train();
 
+    /**
+     * @brief setOptimization   enable or disable the single train trajectory optimization
+     * @param enable    bool value to enable optimization if true, false O.W.
+     * @param optimizationSpeedImportanceNormalizedWeight double value to set the importance of speed compared to the energy consumption [0,1].
+     */
+    void setOptimization(bool enable = false,
+                         double optimizationSpeedImportanceNormalizedWeight = 1.0,
+                         int runOptimizationEvery = DefaultLookAheadCounterToUpdate,
+                         int optimizationLookaheadSteps = DefaultLookAheadCounter);
+
     void setTrainSimulatorID(int newID);
 
     /**