From b9874763c3294458def030f492c751b79798865f Mon Sep 17 00:00:00 2001 From: Ahmed Aredah Date: Fri, 6 Oct 2023 10:43:48 -0400 Subject: [PATCH] add trajectory optimization functionality --- src/NeTrainSim/main.cpp | 46 ++++++- src/NeTrainSim/simulator.cpp | 43 +++--- src/NeTrainSim/traindefinition/train.cpp | 128 ++++++++++++++---- src/NeTrainSim/traindefinition/train.h | 22 ++- .../log.txt | 0 src/dependencies/shpelib | 1 + 6 files changed, 190 insertions(+), 50 deletions(-) create mode 100644 src/build-NeTrainSim-Desktop_Qt_6_4_2_MinGW_64_bit-Debug/log.txt create mode 160000 src/dependencies/shpelib diff --git a/src/NeTrainSim/main.cpp b/src/NeTrainSim/main.cpp index 7c20779..5adcb6f 100644 --- a/src/NeTrainSim/main.cpp +++ b/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> 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); diff --git a/src/NeTrainSim/simulator.cpp b/src/NeTrainSim/simulator.cpp index cafc4d4..1c42926 100644 --- a/src/NeTrainSim/simulator.cpp +++ b/src/NeTrainSim/simulator.cpp @@ -627,24 +627,25 @@ void Simulator::playTrainOneTimeStep(std::shared_ptr 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, diff --git a/src/NeTrainSim/traindefinition/train.cpp b/src/NeTrainSim/traindefinition/train.cpp index 8f293e4..5b21fa9 100644 --- a/src/NeTrainSim/traindefinition/train.cpp +++ b/src/NeTrainSim/traindefinition/train.cpp @@ -17,10 +17,15 @@ using namespace std; unsigned int Train::NumberOfTrainsInSimulator = 0; -Train::Train(int simulatorID, string id, Vector trainPath, double trainStartTime_sec, double frictionCoeff, - Vector> locomotives, Vector> cars, bool optimize, - double desiredDecelerationRate_mPs, double operatorReactionTime_s, bool stopIfNoEnergy, - double maxAllowedJerk_mPcs) : QObject(nullptr) { +Train::Train(int simulatorID, string id, Vector trainPath, + double trainStartTime_sec, double frictionCoeff, + Vector> locomotives, + Vector> 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 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>(sizeOfPath, Vector(sizeOfPath)); @@ -70,12 +66,35 @@ Train::Train(int simulatorID, string id, Vector 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 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 allAcc = Vector(); // loop through all the gaps to next train/station for (int i = 0; i < gapToNextCriticalPoint.size(); i ++){ @@ -804,6 +834,7 @@ tuple 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 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 energyLN; + std::vector 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 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 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, 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() { diff --git a/src/NeTrainSim/traindefinition/train.h b/src/NeTrainSim/traindefinition/train.h index 5462b2d..ae5cfb9 100644 --- a/src/NeTrainSim/traindefinition/train.h +++ b/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 optimumThrottleLevels; + /** Holds the current coordinates of the tip of the train */ pair 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); /** diff --git a/src/build-NeTrainSim-Desktop_Qt_6_4_2_MinGW_64_bit-Debug/log.txt b/src/build-NeTrainSim-Desktop_Qt_6_4_2_MinGW_64_bit-Debug/log.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/dependencies/shpelib b/src/dependencies/shpelib new file mode 160000 index 0000000..0d1d39a --- /dev/null +++ b/src/dependencies/shpelib @@ -0,0 +1 @@ +Subproject commit 0d1d39a7f1b3a8a3a468b431d03df598df9b9b3c