gauss seidel master

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/AbstractSimulationUnit.py

@@ -50,6 +50,8 @@ class AbstractSimulationUnit(object):
         self.__algebraic_vars = algebraic_functions.keys()
         self.__state_and_input_vars = list(state_vars)
         self.__state_and_input_vars.extend(input_vars)
+        self.__output_vars = list(state_vars)
+        self.__output_vars.extend(self.__algebraic_vars)
         
         assert len(self.__state_and_input_vars) == len(self.__input_vars) + len(self.__state_vars)
         
@@ -196,7 +198,7 @@ class AbstractSimulationUnit(object):
         
     
     def getValues(self, step, iteration, var_names=None, zoh=False):
-        var_names = self.__state.keys() if var_names==None else var_names
+        var_names = self.__output_vars if var_names==None else var_names
         l.debug(">%s.getValues(%d, %d, %s)", self._name, step, iteration, var_names)
         if self.__areAlgVarsDirty(step, iteration, var_names):
             self.__computeOutputs(step, iteration, whichOnes=var_names)

SemanticAdaptationForFMI/FMIAbstraction/src/case_study/scenarios/ControlledScenario_AccurateCrossingAdaptation_GS.py

@@ -0,0 +1,223 @@
+"""
+This scenario covers the following adaptations:
+- Step delay and decouple adaptation to all power outputs and to the obstacle output, to break the algebraic loops.
+- Accurate threshold crossing adaptation to the armature current output.
+
+The master algorithm's policy is as follows:
+Each time he asks an FMU to do a step, the FMU may either accept or reject the step.
+If the FMU accepts the step, the master moves onto the next one.
+If the FMU does not accept the state, the master rollsback every other FMU and retries with the step proposed by the FMU.
+
+After a while, if all FMU continuously accept the steps, the master will gradually increase the step.
+"""
+
+import logging
+
+from bokeh.plotting import figure, output_file, show
+
+from case_study.units.adaptations.AccurateControllerArmatureAdaptation import AccurateControllerArmatureAdaptation
+from case_study.units.adaptations.StepDelay import StepDelay
+from case_study.units.ct_based.ObstacleFMU import ObstacleFMU
+from case_study.units.ct_based.PowerFMU import PowerFMU
+from case_study.units.ct_based.WindowFMU import WindowFMU
+from case_study.units.de_based.DriverControllerStatechartFMU_CTInOut import DriverControllerStatechartFMU_CTInOut
+from case_study.units.de_based.EnvironmentStatechartFMU_CTInOut import EnvironmentStatechartFMU_CTInOut
+from master.GaussSeidelMaster import GaussSeidelMaster
+
+
+NUM_RTOL = 1e-08
+NUM_ATOL = 1e-08
+
+l = logging.getLogger()
+l.setLevel(logging.DEBUG)
+
+START_STEP_SIZE = 0.001
+MAX_STEP_SIZE = 0.001
+MIN_STEPS_BEFORE_INCREASE_STEP_SIZE = 10
+STEP_SIZE_INCREASE_RATE = 10 # e.g, 10%
+
+FMU_START_RATE = 10
+STOP_TIME = 6;
+
+environment = EnvironmentStatechartFMU_CTInOut("env", NUM_RTOL, NUM_ATOL)
+
+controller = DriverControllerStatechartFMU_CTInOut("controller", NUM_RTOL, NUM_ATOL)
+
+power = PowerFMU("power", NUM_RTOL, NUM_ATOL, START_STEP_SIZE/FMU_START_RATE, 
+                     J=0.085, 
+                     b=5, 
+                     K=7.45, 
+                     R=0.15, 
+                     L=0.036,
+                     V_a=12)
+
+armature_threshold = 5 # This was the initial threshold, but did not work.
+adapt_armature = AccurateControllerArmatureAdaptation("arm_adapt", NUM_RTOL, NUM_ATOL, armature_threshold, True)
+
+window = WindowFMU("window", NUM_RTOL, NUM_ATOL, START_STEP_SIZE/FMU_START_RATE,
+                     r=0.11, 
+                     b = 10)
+
+obstacle = ObstacleFMU("obstacle", NUM_RTOL, NUM_ATOL, START_STEP_SIZE/FMU_START_RATE, 
+                     c=1e5, 
+                     fixed_x=0.45)
+
+delay_up = StepDelay("delay_up")
+delay_down = StepDelay("delay_down")
+delay_reaction_torque = StepDelay("delay_reaction_torque")
+delay_height = StepDelay("delay_height")
+
+units = [
+         environment,                   # 0
+         controller,                    # 1
+         power,                         # 2
+         adapt_armature,                # 3
+         window,                        # 4
+         obstacle,                      # 5
+         delay_up,                      # 6
+         delay_down,                    # 7
+         delay_reaction_torque,         # 8
+         delay_height                   # 9
+         ]
+
+order = [
+         6,
+         7,
+         8,
+         9,
+         2,
+         5,
+         4,
+         3,
+         0,
+         1
+         ]
+
+initial_state = [
+                 None,
+                 None,
+                 {power.omega: 0.0,power.theta: 0.0, power.i: 0.0},
+                 {adapt_armature.out_obj: 0.0},
+                 None,
+                 None,
+                 {delay_up.out_v: 0.0},
+                 {delay_down.out_v: 0.0},
+                 {delay_reaction_torque.out_v: 0.0},
+                 {delay_height.out_v: 0.0}
+                 ]
+
+def coupling_controller(y):
+    envOut = y[0]
+    adaptArmOut = y[3]
+    return {controller.in_dup : envOut[environment.out_up],
+            controller.in_ddown : envOut[environment.out_down],
+            controller.in_obj : adaptArmOut[adapt_armature.out_obj]}
+
+def coupling_power(y):
+    dupOut = y[6]
+    ddownOut = y[7]
+    dtauOut = y[8]
+    
+    return {power.up : dupOut[delay_up.out_v],
+            power.down : ddownOut[delay_down.out_v],
+            power.tau : dtauOut[delay_reaction_torque.out_v]}
+
+def coupling_adapt_armature(y):
+    pOut = y[2]
+    return {adapt_armature.armature_current: pOut[power.i]}
+
+def coupling_window(y):
+    pOut = y[2]
+    oOut = y[5]
+    return {window.omega_input: pOut[power.omega],
+            window.theta_input: pOut[power.theta],
+            window.F_obj: oOut[obstacle.F]
+            }
+
+def coupling_obstacle(y):
+    dheightOut = y[9]
+    return {obstacle.x: dheightOut[delay_height.out_v]}
+
+def coupling_delay_up(y):
+    cOut = y[1]
+    return {delay_up.in_v: cOut[controller.out_up]} if cOut != None else None
+
+def coupling_delay_down(y):
+    cOut = y[1]
+    return {delay_down.in_v: cOut[controller.out_down]} if cOut != None else None
+
+def coupling_delay_torque(y):
+    wOut = y[4]
+    return {delay_reaction_torque.in_v: wOut[window.tau]} if wOut != None else None
+
+def coupling_delay_height(y):
+    wOut = y[4]
+    return {delay_height.in_v: wOut[window.x]} if wOut != None else None
+
+
+couplings = [
+             lambda y: None,
+             coupling_controller,
+             coupling_power,
+             coupling_adapt_armature,
+             coupling_window,
+             coupling_obstacle,
+             coupling_delay_up,
+             coupling_delay_down,
+             coupling_delay_torque,
+             coupling_delay_height
+             ]
+
+
+
+trace_i = [0.0]
+trace_omega = [0.0]
+trace_x = [0.0]
+trace_F = [0.0]
+times = [0.0]
+
+def plot(t, s, y):
+    pOut = y[2]
+    wOut = y[4]
+    oOut = y[5]
+    
+    times.append(t)
+    trace_omega.append(pOut[power.omega])
+    trace_i.append(pOut[power.i])
+    trace_x.append(wOut[window.x])
+    trace_F.append(oOut[obstacle.F])
+
+
+master = GaussSeidelMaster()
+
+master.simulate(order, units, couplings, initial_state, 
+                    START_STEP_SIZE, MAX_STEP_SIZE, STEP_SIZE_INCREASE_RATE, MIN_STEPS_BEFORE_INCREASE_STEP_SIZE, 
+                    STOP_TIME, plot)
+
+
+color_pallete = [
+                "#e41a1c",
+                "#377eb8",
+                "#4daf4a",
+                "#984ea3",
+                "#ff7f00",
+                "#ffff33",
+                "#a65628",
+                "#f781bf"
+                 ]
+
+output_file("./results.html", title="Results")
+p = figure(title="Plot", x_axis_label='time', y_axis_label='')
+p.line(x=times, y=trace_omega, legend="trace_omega", color=color_pallete[0])
+p.line(x=times, y=trace_i, legend="trace_i", color=color_pallete[1])
+show(p)
+
+output_file("./results_x.html", title="Results")
+p = figure(title="Plot", x_axis_label='time', y_axis_label='')
+p.line(x=times, y=trace_x, legend="trace_x", color=color_pallete[2])
+show(p)
+
+output_file("./results_F.html", title="Results")
+p = figure(title="Plot", x_axis_label='time', y_axis_label='')
+p.line(x=times, y=trace_F, legend="trace_F", color=color_pallete[3])
+show(p)

SemanticAdaptationForFMI/FMIAbstraction/src/case_study/units/adaptations/AccurateControllerArmatureAdaptation.py

@@ -58,16 +58,35 @@ class AccurateControllerArmatureAdaptation(AbstractSimulationUnit):
         
         output_value = 0
         
-        l.debug("%s.previous_input=%f", self._name, previous_input)
-        l.debug("%s.current_input=%f", self._name, current_input)
+        l.debug("%s.previous_input=%.6f", self._name, previous_input)
+        l.debug("%s.current_input=%.6f", self._name, current_input)
         
         step_info = STEP_ACCEPT
         proposed_step_size = step_size
         
         if self.__crossUpward:
+            if previous_input < self.__threshold and current_input > self.__threshold:
+                l.debug("Crossing detected from %.6f to %.6f", 
+                        previous_input, current_input)
+                if self._biggerThan(current_input, self.__threshold):
+                    l.debug("But step too large.")
+                    step_info = STEP_DISCARD
+                    
+                    # Convert this into a zero crossing problem
+                    negative_value = previous_input - self.__threshold
+                    positive_value = current_input - self.__threshold
+                    
+                    # Estimate the step size by regula-falsi
+                    proposed_step_size = (step_size * (- negative_value)) / (positive_value - negative_value)
+                    assert proposed_step_size > 0.0, "Problem with the signals and tolerances. All hope is lost."
+                else:
+                    l.debug("And step size is acceptable.")
+                    output_value = 1
+                    
+            """
             if (not self._biggerThan(previous_input, self.__threshold)) \
                     and self._biggerThan(current_input, self.__threshold):
-                l.debug("Crossing detected from %f to %f", 
+                l.debug("Crossing detected from %.6f to %.6f", 
                         previous_input, current_input)
                 l.debug("But step too large.")
                 step_info = STEP_DISCARD
@@ -86,6 +105,7 @@ class AccurateControllerArmatureAdaptation(AbstractSimulationUnit):
                         previous_input, current_input)
                 l.debug("And step size is acceptable.")
                 output_value = 1
+            """
                 
         
         l.debug("%s.output_value=%s", self._name, output_value)

SemanticAdaptationForFMI/FMIAbstraction/src/case_study/units/adaptations/InacurateControllerArmatureAdaptation_CT.py

@@ -75,8 +75,8 @@ class InacurateControllerArmatureAdaptation_CT(AbstractSimulationUnit):
         #assert self._biggerThan(step_size, 0), "step_size too small: {0}".format(step_size)
         assert iteration == 0, "Fixed point iterations not supported yet."
         
-        current_input = self.getValues(step-1, iteration, self._getInputVars())[self.armature_current]
-        previous_input = self.getValues(step-2, iteration, self._getInputVars())[self.armature_current]
+        current_input = self.getValues(step, iteration, self._getInputVars())[self.armature_current]
+        previous_input = self.getValues(step-1, iteration, self._getInputVars())[self.armature_current]
         
         output_value = 0
         

SemanticAdaptationForFMI/FMIAbstraction/src/case_study/units/adaptations/StepDelay.py

@@ -0,0 +1,45 @@
+import logging
+
+from abstract_units.AbstractSimulationUnit import AbstractSimulationUnit, \
+    STEP_ACCEPT
+
+l = logging.getLogger()
+
+class StepDelay(AbstractSimulationUnit):
+    
+    def __init__(self, name):
+        
+        self.in_v = "in_v"
+        self.out_v = "out_v"
+        input_vars = [self.in_v]
+        state_vars = [self.out_v]
+        
+        algebraic_functions = {}
+        
+        AbstractSimulationUnit.__init__(self, name, algebraic_functions, state_vars, input_vars)
+    
+    def _doInternalSteps(self, time, step, iteration, step_size):
+        l.debug(">%s._doInternalSteps(%f, %d, %d, %f)", self._name, time, step, iteration, step_size)
+        
+        assert step_size > 0.0, "step_size too small: {0}".format(step_size)
+        assert iteration == 0, "Fixed point iterations not supported yet."
+        
+        previous_input = self.getValues(step-1, iteration, self._getInputVars())[self.in_v]
+        
+        l.debug("%s.previous_input=%f", self._name, previous_input)
+        
+        l.debug("%s.output_value=%s", self._name, previous_input)
+        self.setValues(step, iteration, {self.out_v: previous_input})
+        
+        l.debug("<%s._doInternalSteps() = (%s, %d)", self._name, STEP_ACCEPT, step_size)
+        return (STEP_ACCEPT, step_size)
+    
+    def enterInitMode(self):
+        l.debug(">%s.StepDelay.enterInitMode()", self._name)
+        AbstractSimulationUnit.enterInitMode(self)
+        
+        # Add a dummy value at time 0, just to fill in the array
+        self.setValues(0, 0, {self.in_v: 0.0})
+        
+        l.debug("<%s.StepDelay.enterInitMode()", self._name)
+    

SemanticAdaptationForFMI/FMIAbstraction/src/master/GaussSeidelMaster.py

@@ -0,0 +1,92 @@
+'''
+This follows algorithm 4 in the paper.
+'''
+import logging
+
+l = logging.getLogger()
+
+class GaussSeidelMaster():
+    def simulate(self, order, units, coupling, initial_state,
+                 start_step_size, max_step_size, step_increase_rate, min_steps_before_increase,
+                 stop_time,
+                 plot):
+        
+        t = 0
+        H = start_step_size
+        step = 0
+        
+        l.debug("Entering init mode...")
+        
+        for j in order:
+            unit = units[j];
+            unit.enterInitMode()
+        
+        y = [] 
+        u = []
+        H_proposed = []
+        for unit in units:
+            y.append(None)
+            u.append(None)
+            H_proposed.append(None)
+        
+        l.debug("Setting initial states...")
+        
+        for sigmaj in order:
+            if initial_state[sigmaj] != None:
+                units[sigmaj].setValues(step,0,initial_state[sigmaj])
+        
+        for sigmaj in order:
+            u[sigmaj] = coupling[sigmaj](y)
+            if u[sigmaj] != None:
+                units[sigmaj].setValues(step,0,u[sigmaj])
+            y[sigmaj] = units[sigmaj].getValues(step,0)
+        
+        step=1
+        last_rollback_step = 0
+        
+        l.debug("Exiting init mode...")
+        for j in order:
+            unit = units[j];
+            unit.exitInitMode()
+        
+        plot(t, step, y)
+        
+        l.debug("Going to step mode...")
+        while t < stop_time:
+            
+            l.debug("New cosim step: %d at time %f", step, t)
+            
+            ok = False
+            
+            while not ok:
+                for sigmaj in order:
+                    u[sigmaj] = coupling[sigmaj](y)
+                    if u[sigmaj] != None:
+                        units[sigmaj].setValues(step,0,u[sigmaj])
+                    (_, H_hat) = units[sigmaj].doStep(t, step, 0, H)
+                    H_proposed[sigmaj] = H_hat
+                    y[sigmaj] = units[sigmaj].getValues(step,0)
+                
+                min_H = min(H_proposed)
+                if min_H < H:
+                    l.debug("Rolling back step: %d at time %f", step, t)
+                    ok = False
+                    last_rollback_step = step
+                    l.debug("Decreasing step size from %f to %f...", H, min_H)            
+                    H = min_H
+                else:
+                    ok = True
+                
+            if (step - last_rollback_step) > min_steps_before_increase \
+                and H < max_step_size:
+                new_step_size = H + H * (step_increase_rate/100.0)
+                l.debug("Increasing step size from %f to %f...", H, new_step_size)            
+                H = new_step_size
+                if H > max_step_size:
+                    l.debug("Max step size attained: %f.", max_step_size)  
+                    H = max_step_size
+            
+            t = t + H
+            plot(t, step, y)
+            step = step + 1
+