Merge pull request #9 from teamparodis/develop

Develop
teamparodis · Nov 29, 2023 · e36d93b · e36d93b
2 parents af54b32 + e6e8cb4
commit e36d93b
Show file tree

Hide file tree

Showing 7 changed files with 42 additions and 14 deletions.
diff --git a/@Agent/Agent.m b/@Agent/Agent.m
@@ -177,6 +177,8 @@ function doNegotiation(this, externalData)
                 this.callbackPostOptimization(this, this.simulation)
             end
 
+            this.updatePredictions(true);
+
             if isfield(this.status, 'solverCode') && this.status.solverCode ~= 0
                 if strcmpi( this.config.pauseOnError, 'on') || (this.config.pauseOnError == this.status.solverCode)
                     message = sprintf("Paused execution, solver returned code %i: %s", this.status.solverCode, yalmiperror(this.status.solverCode));
@@ -262,18 +264,24 @@ function doStep(this, externalData)
             this.status.slackVariables = slacks;
         end
 
-        function updatePredictions(this)
+        function updatePredictions(this, fromNegotiation)
             % updatePredictions  Sets state and eval predictions according
             %                    to currently available input and state predictions from
             %                    current agent state
 
-            this.status.xPred = this.predictTrajectory(this.status.uPred, this.status.dPred, this.history.x(:, end));
+            if nargin < 2
+                fromNegotiation = false;
+            end
 
-            % call each eval function with pred = true and store result
-            this.status.evalPred = this.evaluateEvalFunctions(true);
+            this.status.xPred = this.predictTrajectory(this.status.uPred, this.status.dPred, this.history.x(:, end));
 
-            % call each cost function and retrieve horizon
-            this.status.costsPred = this.evaluateCostFunctions(this.status.xPred, this.status.uPred, this.status.dPred);
+            if ~fromNegotiation
+                % call each eval function with pred = true and store result
+                this.status.evalPred = this.evaluateEvalFunctions(true);
+
+                % call each cost function and retrieve horizon
+                this.status.costsPred = this.evaluateCostFunctions(this.status.xPred, this.status.uPred, this.status.dPred);
+            end
         end
 
         function updateHistory(this, externalData)
@@ -657,6 +665,7 @@ function clearHistory( this, after_k )
             evalNames = fieldnames(this.history.evalValues);
             for idx = 1:length(evalNames)
                 this.history.evalValues.(evalNames{idx})(:, after_k+1:end) = [];
+
                 this.virtualHistory.evalValues.(evalNames{idx})(:, after_k+1:end) = [];
             end
 
@@ -726,6 +735,8 @@ function clearStatus(this, clearPreviousStatus)
             this.status.evalPred = struct;
             this.status.paramValues = struct;
             this.status.pareto = struct;
+            this.status.costsPred = struct;
+            this.status.slackVariables = struct;
 
             if clearPreviousStatus
                 this.previousStatus = [];

diff --git a/@ParetoController/determineASBI.m b/@ParetoController/determineASBI.m
@@ -94,7 +94,7 @@
 
 % skip the solution if the problem was infeasible
 if feasibilityCode ~= 0
-    msg = "Yalmip error in Simulation step " + agent.sim.status.k + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
+    msg = "Yalmip error in Simulation step " + agent.simulation.k_sim + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
     if paretoObj.config.printSolverStatus
         agent.log(msg); %possible: yalmiperror
     else

diff --git a/@ParetoController/determineAWDS.m b/@ParetoController/determineAWDS.m
@@ -102,7 +102,7 @@
 
 % skip the solution if the problem was infeasible
 if feasibilityCode ~= 0
-    msg = "Yalmip error in Simulation step " + agent.sim.status.k + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
+    msg = "Yalmip error in Simulation step " + agent.simulation.k_sim + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
     if paretoObj.config.printSolverStatus
         agent.log(msg); %possible: yalmiperror
     else

diff --git a/@ParetoController/determineFPBI.m b/@ParetoController/determineFPBI.m
@@ -26,7 +26,7 @@
     [optOut, feasibilityCode] = optimizer(planePoints(iParetoStep,:));
 
     if feasibilityCode ~= 0
-        msg = "Yalmip error in Simulation step " + agent.sim.status.k + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
+        msg = "Yalmip error in Simulation step " + agent.simulation.k_sim + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
         if paretoObj.config.printSolverStatus
             agent.log(msg); %possible: yalmiperror
         else

diff --git a/@ParetoController/determineNBI.m b/@ParetoController/determineNBI.m
@@ -39,7 +39,7 @@
     [optOut, feasibilityCode] = optimizer(planePoints(i,:));
 
     if feasibilityCode ~= 0
-        msg = "Yalmip error in Simulation step " + agent.sim.status.k + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
+        msg = "Yalmip error in Simulation step " + agent.simulation.k_sim + " in Pareto step " + paretoObj.paretoCurrentStep + ": " + (feasibilityCode);
         if paretoObj.config.printSolverStatus
             agent.log(msg); %possible: yalmiperror
         else

diff --git a/@Simulation/Simulation.m b/@Simulation/Simulation.m
@@ -340,6 +340,16 @@ function prepareSimulation(this, k_start)
                 end
 
                 k_local = (this.T_s_max / agent.config.T_s(1) * k_start);
+
+                % if simulation doesn't start at 0, then
+                % prefill simulation time in agent's history correctly
+                if k_start > 0
+                    %agent.history.simulationTime = (0:k_local)*agent.config.T_s(1);
+                    %agent.virtualHistory.simulationTime = (0:k_local)*agent.config.T_s(1);
+                    agent.history.simulationTime = [k_local*agent.config.T_s(1)];
+                    agent.virtualHistory.simulationTime = [k_local*agent.config.T_s(1)];
+                end
+
                 agent.clearHistory(k_local);
                 agent.clearStatus(true);
                 agent.status.k = k_local;
@@ -632,6 +642,7 @@ function restoreSimulation(this, directory, k_start)
                     % restore cost function values
                     costNames = fieldnames(agent.controller.costFunctionIndexes);
                     costs_history = struct;
+                    costs_history_virtual = struct;
                     for i = 1:length(costNames)
                         costName = costNames{i};
 
@@ -649,6 +660,7 @@ function restoreSimulation(this, directory, k_start)
                     % store each eval fun seperately
                     evalNames = fieldnames(agent.config.evalFuns);
                     eval_history = struct;
+                    eval_history_virtual = struct;
                     for i = 1:length(evalNames)
                         evalName = evalNames{i};
                         evalHistoryFile = agentDir + "eval_" + evalName + ".csv";
@@ -665,6 +677,7 @@ function restoreSimulation(this, directory, k_start)
 
                         else
                             eval_history.(evalName) = NaN(1, k);
+                            eval_history_virtual.(evalName) = NaN(1, k);
                         end
                     end
                 end
@@ -861,8 +874,8 @@ function debugDisp(this, message)
 
     methods (Static)
         function printHeader()
-            version = '1.1.0';
-            releaseDate = '2021-08-06';
+            version = '1.1.1';
+            releaseDate = '2023-11-29';
             message = [
                 "-------------------------------------------------------------";
                 "PARODIS - Pareto Optimal MPC for Distributed Systems";

diff --git a/functions/eval_parameter.m b/functions/eval_parameter.m
@@ -12,13 +12,17 @@
 function [value] = eval_parameter_evaluation(param, index, agent, predict, scenario)
 
 N_pred = length(agent.config.T_s);
+if isempty(scenario)
+    scenario = 1; 
+end
+
+value = agent.status.paramValues.(param){scenario}(index, :);
 if predict
-    value = agent.status.paramValues.(param){scenario}(index, :);
     if length(value) > N_pred
         value = value(1:N_pred);
     end
 else
-    value = NaN;
+    value = value(1);
 end
 
 end