multi-rate adaptation (not running yet)

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/AbstractSimulationUnit.py

@@ -93,19 +93,10 @@ class AbstractSimulationUnit(object):
                 assert iteration == len(self.__computed_time[step]) - 1, "Weird use of the iteration records. Either rewrite the last iteration, or keep tracking them."
                 self.__computed_time[step][iteration] = time + step_size
                 
-        """
-        if iteration == 0:
-            assert step == len(self.__computed_time)
-            self.__computed_time.append([time + step_size])
-        else:
-            assert step == len(self.__computed_time) - 1
-            assert iteration == len(self.__computed_time[step])
-            self.__computed_time[step].append(time + step_size)
-        """
        
     def doStep(self, time, step, current_iteration, step_size):
         l.debug(">%s.doStep(t=%f, s=%d, i=%d, H=%f)", self._name,  time, step, current_iteration, step_size)
-        assert self._mode == STEP_MODE
+        assert self._mode == STEP_MODE, "Wrong mode: " + self._mode
         assert step_size > 0
         
         """ This is not done here because it may not be possible to do it for 
@@ -145,7 +136,7 @@ class AbstractSimulationUnit(object):
         
         for var in var_names:
             new_value = self.__algebraic_functions[var](current_state_snaptshot, current_input_snaptshot)
-            Utils.copyValueToStateTrace(self.__state, var, step, iteration, new_value)
+            Utils.copyValueToStateTrace(self.__state, var, step, iteration, new_value, ensureExists=True)
         
         l.debug("Updated portion of the state:\n%s", 
                     self._printState(step, iteration, var_names))
@@ -187,17 +178,21 @@ class AbstractSimulationUnit(object):
                     return True
         return False
         
-    def setValues(self, step, iteration, values):
+    def setValues(self, step, iteration, values, ensureExists=True):
         l.debug(">%s.setValues(%d, %d, %s)", self._name, step, iteration, values)
-        Utils.copyMapToStateTrace(self.__state, step, iteration, values)
-        
-        self.__markAlgVars(step, iteration, dirty=True);
         
-        l.debug("New state: \n%s", self._printState(step,iteration, values.keys()))
+        if values != None:
+            Utils.copyMapToStateTrace(self.__state, step, iteration, values, ensureExists)
+            
+            self.__markAlgVars(step, iteration, dirty=True);
+            
+            l.debug("New state: \n%s", self._printState(step,iteration, values.keys()))
+        else:
+            l.debug("No values to set.")
         l.debug("<%s.setValues()", self._name)
         
     
-    def getValues(self, step, iteration, var_names=None, zoh=False):
+    def getValues(self, step, iteration, var_names=None, zoh=False, ensureExists=False):
         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):
@@ -206,18 +201,21 @@ class AbstractSimulationUnit(object):
         
         values = {}
         for var in var_names:
-            assert self.__state.has_key(var)
-            stepToAccess = step
-            iterationToAccess = iteration
-            if stepToAccess >= len(self.__state[var]) and zoh:
-                assert stepToAccess == len(self.__state[var]), "Inconsistent state at step %d.".format(step)
-                stepToAccess = step-1
-                iterationToAccess = -1
-                l.debug("Getting old value for %s at step %d.", var, stepToAccess)
-                
-            assert stepToAccess < len(self.__state[var]), "State is not initialized: " + str(self.__state)
-            assert iterationToAccess < len(self.__state[var][stepToAccess])
-            values[var] = self.__state[var][stepToAccess][iterationToAccess]
+            if ensureExists:
+                assert self.__state.has_key(var)
+            
+            if self.__state.has_key(var):
+                stepToAccess = step
+                iterationToAccess = iteration
+                if stepToAccess >= len(self.__state[var]) and zoh:
+                    assert stepToAccess == len(self.__state[var]), "Inconsistent state at step %d.".format(step)
+                    stepToAccess = step-1
+                    iterationToAccess = -1
+                    l.debug("Getting old value for %s at step %d.", var, stepToAccess)
+                    
+                assert stepToAccess < len(self.__state[var]), "State is not initialized: " + str(self.__state)
+                assert iterationToAccess < len(self.__state[var][stepToAccess])
+                values[var] = self.__state[var][stepToAccess][iterationToAccess]
         
         l.debug("<%s.getValues()=%s", self._name, values)
         return values

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/MultiRateAdaptationUnit.py

@@ -0,0 +1,144 @@
+import logging
+
+import numpy
+
+from abstract_units.AbstractSimulationUnit import AbstractSimulationUnit, \
+    STEP_ACCEPT
+from master.GaussSeidelMaster import GaussSeidelMaster
+
+l = logging.getLogger()
+
+class MultiRateAdaptationUnit(AbstractSimulationUnit):
+    
+    def __init__(self, name, num_rtol, num_atol, 
+                                  order, units, coupling, external_coupling, start_rate, initial_state):
+        
+        algebraic_functions = {}
+        
+        state_vars = []
+        
+        input_vars = []
+        
+        AbstractSimulationUnit.__init__(self, name, algebraic_functions, state_vars, input_vars)
+        
+        self._num_rtol = num_rtol
+        self._num_atol = num_atol
+        
+        self._start_rate = start_rate
+        
+        self._initial_state = initial_state
+        self._units = units
+        self._coupling = coupling
+        self._external_coupling = external_coupling
+        self._order = order
+        
+        self._y = None
+        
+        self._inputs = None
+    
+    def _isClose(self, a, b):
+        return numpy.isclose(a,b, self._num_rtol, self._num_atol)
+    
+    def _biggerThan(self, a, b):
+        return not numpy.isclose(a,b, self._num_rtol, self._num_atol) and a > b
+    
+    def setValues(self, step, iteration, values):
+        l.debug(">%s.MultiRateAdaptationUnit.setValues(%d, %d, %s)", self._name, step, iteration, values)
+        
+        self._inputs = values
+        
+        l.debug("<%s.MultiRateAdaptationUnit.setValues()", self._name)
+    
+    def getValues(self, step, iteration, var_names=None, zoh=False):
+        l.debug(">%s.MultiRateAdaptationUnit.getValues(%d, %d, %s)", self._name, step, iteration, var_names)
+        
+        u = []
+        for _ in self._units:
+            u.append(None)
+        
+        GaussSeidelMaster.compute_outputs(self._step, 
+                                          self._units, self._order, self._coupling, 
+                                          self._y, u)
+        
+        output_values = self._external_coupling(self._y)
+        
+        l.debug("<%s.MultiRateAdaptationUnit.getValues()=%s", self._name, output_values)
+        return output_values
+    
+    def enterInitMode(self):
+        l.debug(">%s.MultiRateAdaptationUnit.enterInitMode()", self._name)
+        
+        AbstractSimulationUnit.enterInitMode(self)
+        
+        self._y = []
+        for _ in self._units:
+            self._y.append(None)
+        
+        self._step = 0
+        
+        GaussSeidelMaster.start_initialize(self._order, self._units)
+        
+        GaussSeidelMaster.set_initial_states(self._units, self._order, self._initial_state)
+        
+        l.debug("<%s.MultiRateAdaptationUnit.enterInitMode()", self._name)
+    
+    def exitInitMode(self):
+        l.debug(">%s.MultiRateAdaptationUnit.exitInitMode()", self._name)
+        
+        AbstractSimulationUnit.exitInitMode(self)
+        
+        GaussSeidelMaster.finish_initialize(self._order, self._units)
+
+        l.debug("<%s.MultiRateAdaptationUnit.exitInitMode()", self._name)
+    
+    
+    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."
+        
+        next_external_cosim_time = time+step_size
+        
+        max_step_size = step_size/self._start_rate
+        
+        H = max_step_size
+        
+        l.debug("max_step_size=%f", max_step_size)
+        
+        internal_time = time;
+
+        u = []
+        H_proposed = []
+        
+        for _ in self._units:
+            u.append(None)
+            H_proposed.append(None)
+        
+        do_next_step = True
+        while do_next_step:
+            
+            last_rollback_step = self._step
+            min_steps_before_increase = 1e5 # Never increase the step
+            
+            self._step = self._step + 1
+            
+            if (internal_time + H) >= next_external_cosim_time:
+                H = next_external_cosim_time - internal_time
+            
+            (H,_) = GaussSeidelMaster.do_cosim_step(internal_time, self._step, self._order, self._units, self._coupling, 
+                                            u, self._y, H_proposed, 
+                                            H, last_rollback_step,
+                                            min_steps_before_increase, max_step_size, 0.0)
+            
+            if (internal_time + H) >= next_external_cosim_time:
+                do_next_step = False
+            
+            l.debug("step size taken by internal units=%f", H)
+            internal_time = internal_time + H
+            l.debug("internal_time=%f", internal_time)
+            
+        l.debug("<%s._doInternalSteps() = (%s, %f)", self._name, STEP_ACCEPT, step_size)
+        return (STEP_ACCEPT, step_size)
+
+                

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/Utils.py

@@ -7,9 +7,9 @@ Created on Mar 5, 2016
 class Utils(object):
     
     @staticmethod
-    def copyMapToStateTrace(target, step, iteration, source):
+    def copyMapToStateTrace(target, step, iteration, source, ensureExists):
         for var in source:
-            Utils.copyValueToStateTrace(target, var, step, iteration, source[var])
+            Utils.copyValueToStateTrace(target, var, step, iteration, source[var], ensureExists)
     
     @staticmethod
     def getValuesUpToDate(source, vars_to_select, step, iteration):
@@ -23,8 +23,12 @@ class Utils(object):
         return result
     
     @staticmethod
-    def copyValueToStateTrace(target, var, step, iteration, value):
-        assert target.has_key(var)
+    def copyValueToStateTrace(target, var, step, iteration, value, ensureExists):
+        if ensureExists:
+            assert target.has_key(var)
+        elif not target.has_key(var):
+            return
+        
         assert step <= len(target[var])
         if step == len(target[var]):
             target[var].append([value])

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

@@ -16,7 +16,7 @@ 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.adaptations.StepDelay import DecoupleAdaptation
 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
@@ -62,10 +62,10 @@ obstacle = ObstacleFMU("obstacle", NUM_RTOL, NUM_ATOL, START_STEP_SIZE/FMU_START
                      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")
+delay_up = DecoupleAdaptation("delay_up")
+delay_down = DecoupleAdaptation("delay_down")
+delay_reaction_torque = DecoupleAdaptation("delay_reaction_torque")
+delay_height = DecoupleAdaptation("delay_height")
 
 units = [
          environment,                   # 0

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

@@ -0,0 +1,149 @@
+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 DecoupleAdaptation
+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
+from case_study.units.adaptations.MultiRateAdaptation_Power_Window_Obstacle import MultiRateAdaptation_Power_Window_Obstacle
+
+
+NUM_RTOL = 1e-08
+NUM_ATOL = 1e-08
+
+l = logging.getLogger()
+l.setLevel(logging.DEBUG)
+
+START_STEP_SIZE = 0.01
+MAX_STEP_SIZE = 0.01
+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)
+
+plant = MultiRateAdaptation_Power_Window_Obstacle("plant", NUM_RTOL, NUM_ATOL, FMU_START_RATE, START_STEP_SIZE)
+
+armature_threshold = 5 # This was the initial threshold, but did not work.
+adapt_armature = AccurateControllerArmatureAdaptation("arm_adapt", NUM_RTOL, NUM_ATOL, armature_threshold, True)
+
+delay_up = DecoupleAdaptation("delay_up")
+delay_down = DecoupleAdaptation("delay_down")
+
+units = [
+         environment,                   # 0
+         controller,                    # 1
+         plant,                         # 2
+         adapt_armature,                # 3
+         delay_up,                      # 4
+         delay_down                     # 5
+         ]
+
+order = [
+         4,
+         5,
+         0,
+         2,
+         3,
+         1
+         ]
+
+initial_state = [
+                 None,
+                 None,
+                 None,
+                 {adapt_armature.out_obj: 0.0},
+                 {delay_up.out_v: 0.0},
+                 {delay_down.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_plant(y):
+    dupOut = y[4]
+    ddownOut = y[5]
+    
+    return {plant.in_power_up : dupOut[delay_up.out_v],
+            plant.in_power_down : ddownOut[delay_down.out_v]
+            }
+
+def coupling_adapt_armature(y):
+    pOut = y[2]
+    return {adapt_armature.armature_current: pOut[plant.out_power_i]}
+
+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
+
+couplings = [
+             lambda y: None,
+             coupling_controller,
+             coupling_plant,
+             coupling_adapt_armature,
+             coupling_delay_up,
+             coupling_delay_down
+             ]
+
+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]
+    
+    times.append(t)
+    trace_i.append(pOut[plant.out_power_i])
+    trace_x.append(pOut[plant.out_window_x])
+    trace_F.append(pOut[plant.out_obs_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/MultiRateAdaptation_Power_Window_Obstacle.py

@@ -0,0 +1,151 @@
+import logging
+
+from abstract_units.MultiRateAdaptationUnit import MultiRateAdaptationUnit
+from case_study.units.adaptations.StepDelay import DecoupleAdaptation
+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.adaptations.ZOHAdaptation import ZOHAdaptation
+
+
+l = logging.getLogger()
+
+class MultiRateAdaptation_Power_Window_Obstacle(MultiRateAdaptationUnit):
+    """
+    This is the adaptation that realizes the strong coupling between the power,
+    window, and obstacle units.
+    """
+    
+    def __init__(self, name, num_rtol, num_atol, start_rate, start_step_size):
+        
+        self.in_power_up = "in_power_up"
+        self.in_power_down = "in_power_down"
+        self.out_power_i = "out_power_i"
+        self.out_window_x = "out_window_x"
+        self.out_obs_F = "out_obs_F"
+        
+        power = PowerFMU(name+":"+"power", num_rtol, num_atol, start_step_size/start_rate, 
+                     J=0.085, 
+                     b=5, 
+                     K=7.45, 
+                     R=0.15, 
+                     L=0.036,
+                     V_a=12)
+
+        window = WindowFMU(name+":"+"window", num_rtol, num_atol, start_step_size/start_rate,
+                             r=0.11, 
+                             b = 10)
+        
+        obstacle = ObstacleFMU(name+":"+"obstacle", num_rtol, num_atol, start_step_size/start_rate, 
+                             c=1e5, 
+                             fixed_x=0.45)
+        
+        delay_reaction_torque = DecoupleAdaptation(name+":"+"delay_reaction_torque")
+        delay_height = DecoupleAdaptation(name+":"+"delay_height")
+        
+        zoh_up = ZOHAdaptation(name+":"+"zoh_up", "in_ext_up", "out_ext_up")
+        zoh_down = ZOHAdaptation(name+":"+"zoh_down", "in_ext_down", "out_ext_down")
+        
+        units = [
+                 power,                  # 0
+                 window,                 # 1
+                 obstacle,               # 2
+                 delay_height,           # 3
+                 delay_reaction_torque,  # 4
+                 zoh_up,                 # 5
+                 zoh_down                # 6
+                 ]
+        
+        order = [
+                 5,
+                 6,
+                 3,
+                 4,
+                 2,
+                 0,
+                 1
+                 ]
+        
+        initial_state = [
+                         {power.omega: 0.0,power.theta: 0.0, power.i: 0.0},
+                         None,
+                         None,
+                         {delay_height.out_v: 0.0},
+                         {delay_reaction_torque.out_v: 0.0},
+                         None,
+                         None
+                         ]
+        
+        def coupling_power(y):
+            zohUpOut = y[5]
+            zohDownOut = y[6]
+            dtauOut = y[4]
+            
+            return {power.up : zohUpOut[zoh_up.output_var],
+                    power.down : zohDownOut[zoh_down.output_var],
+                    power.tau : dtauOut[delay_reaction_torque.out_v]}
+        
+        def coupling_window(y):
+            pOut = y[0]
+            oOut = y[2]
+            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[3]
+            return {obstacle.x: dheightOut[delay_height.out_v]}
+        
+        def coupling_delay_torque(y):
+            wOut = y[1]
+            return {delay_reaction_torque.in_v: wOut[window.tau]} if wOut != None else None
+        
+        def coupling_delay_height(y):
+            wOut = y[1]
+            return {delay_height.in_v: wOut[window.x]} if wOut != None else None
+        
+        def coupling_zoh_up(y):
+            last_known_inputs = self._inputs
+            output=None
+            if last_known_inputs != None:
+                output= {
+                         zoh_up.input_var : last_known_inputs[self.in_power_up]
+                         }
+            return output
+        
+        def coupling_zoh_down(y):
+            last_known_inputs = self._inputs
+            output=None
+            if last_known_inputs != None:
+                output= {
+                         zoh_down.input_var : last_known_inputs[self.in_power_down]
+                         }
+            return output
+        
+        def external_coupling(y):
+            pOut = y[0]
+            wOut = y[1]
+            oOut = y[2]
+            
+            return {
+                    self.out_power_i : pOut[power.i],
+                    self.out_window_x: wOut[window.x],
+                    self.out_obs_F: oOut[obstacle.F]
+                    }
+            
+        couplings = [
+                     coupling_power,
+                     coupling_window,
+                     coupling_obstacle,
+                     coupling_delay_height,
+                     coupling_delay_torque,
+                     coupling_zoh_up,
+                     coupling_zoh_down
+                     ]
+        
+        MultiRateAdaptationUnit.__init__(self, name, num_rtol, num_atol, 
+                                         order, units, couplings, external_coupling,
+                                         start_rate,
+                                         initial_state)
+    

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

@@ -5,7 +5,7 @@ from abstract_units.AbstractSimulationUnit import AbstractSimulationUnit, \
 
 l = logging.getLogger()
 
-class StepDelay(AbstractSimulationUnit):
+class DecoupleAdaptation(AbstractSimulationUnit):
     
     def __init__(self, name):
         
@@ -24,7 +24,7 @@ class StepDelay(AbstractSimulationUnit):
         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]
+        previous_input = self.getValues(step, iteration, self._getInputVars())[self.in_v]
         
         l.debug("%s.previous_input=%f", self._name, previous_input)
         
@@ -35,11 +35,11 @@ class StepDelay(AbstractSimulationUnit):
         return (STEP_ACCEPT, step_size)
     
     def enterInitMode(self):
-        l.debug(">%s.StepDelay.enterInitMode()", self._name)
+        l.debug(">%s.DecoupleAdaptation.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)
+        l.debug("<%s.DecoupleAdaptation.enterInitMode()", self._name)
     

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

@@ -0,0 +1,51 @@
+import logging
+
+from abstract_units.AbstractSimulationUnit import AbstractSimulationUnit, \
+    STEP_ACCEPT
+
+l = logging.getLogger()
+
+class ZOHAdaptation(AbstractSimulationUnit):
+    
+    def __init__(self, name, input_var, output_var):
+        
+        self.input_var = "in_v"
+        self.output_var = "out_v"
+        
+        self.__value_to_hold = 0.0
+        
+        input_vars = []
+        state_vars = []
+        
+        algebraic_functions = {}
+        
+        AbstractSimulationUnit.__init__(self, name, algebraic_functions, state_vars, input_vars)
+    
+    def setValues(self, step, iteration, values, ensureExists=True):
+        l.debug(">%s.ZOHAdaptation.setValues(%d, %d, %s)", self._name, step, iteration, values)
+        
+        if values.has_key(self.input_var):
+            self.__value_to_hold = values[self.input_var]
+        
+        l.debug("New state: \n%s", self.__value_to_hold)
+        l.debug("<%s.ZOHAdaptation.setValues()", self._name)
+        
+    
+    def getValues(self, step, iteration, var_names=None, zoh=False, ensureExists=False):
+        l.debug(">%s.ZOHAdaptation.getValues(%d, %d, %s)", self._name, step, iteration, var_names)
+        
+        values = {
+                  self.output_var : self.__value_to_hold
+                  }
+        
+        l.debug("<%s.ZOHAdaptation.getValues()=%s", self._name, values)
+        return values
+    
+    
+    def _doInternalSteps(self, time, step, iteration, step_size):
+        l.debug(">%s._doInternalSteps(%f, %d, %d, %f)", self._name, time, step, iteration, step_size)
+        
+        l.debug("<%s._doInternalSteps() = (%s, %d)", self._name, STEP_ACCEPT, step_size)
+        return (STEP_ACCEPT, step_size)
+    
+    

SemanticAdaptationForFMI/FMIAbstraction/src/master/GaussSeidelMaster.py

@@ -3,9 +3,89 @@ This follows algorithm 4 in the paper.
 '''
 import logging
 
+
 l = logging.getLogger()
 
 class GaussSeidelMaster():
+    
+    @staticmethod
+    def start_initialize(order, units):
+        l.debug(">GaussSeidelMaster.start_initialize()")
+        for j in order:
+            unit = units[j];
+            unit.enterInitMode()
+        l.debug("<GaussSeidelMaster.start_initialize()")
+    
+    @staticmethod
+    def finish_initialize(order, units):
+        l.debug(">GaussSeidelMaster.finish_initialize()")
+        for j in order:
+            unit = units[j];
+            unit.exitInitMode()
+        l.debug("<GaussSeidelMaster.finish_initialize()")
+    
+    @staticmethod
+    def compute_outputs(step, units, order, coupling, y, u):
+        l.debug(">GaussSeidelMaster.compute_outputs(%s, %s)", y, u)
+        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)
+        l.debug("<GaussSeidelMaster.compute_outputs(%s, %s)", y, u)
+        
+    
+    @staticmethod
+    def set_initial_states(units, order, initial_state):
+        l.debug(">GaussSeidelMaster.set_initial_states()")
+        for sigmaj in order:
+            if initial_state[sigmaj] != None:
+                units[sigmaj].setValues(0,0,initial_state[sigmaj])
+        l.debug("<GaussSeidelMaster.set_initial_states()")
+        
+    
+    @staticmethod
+    def do_cosim_step(t, step, order, units, coupling, 
+                      u, y, H_proposed, 
+                      H, last_rollback_step,
+                      min_steps_before_increase, max_step_size, step_increase_rate):
+        l.debug(">GaussSeidelMaster.do_cosim_step(%f, %d, %f)", t, step, H)
+        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
+                
+        l.debug("<GaussSeidelMaster.do_cosim_step()=%f , %s", H, H_proposed)
+        
+        assert H > 0.0
+        
+        return (H, last_rollback_step)
+    
     def simulate(self, order, units, coupling, initial_state,
                  start_step_size, max_step_size, step_increase_rate, min_steps_before_increase,
                  stop_time,
@@ -17,37 +97,28 @@ class GaussSeidelMaster():
         
         l.debug("Entering init mode...")
         
-        for j in order:
-            unit = units[j];
-            unit.enterInitMode()
+        GaussSeidelMaster.start_initialize(order,units)
         
         y = [] 
         u = []
         H_proposed = []
-        for unit in units:
+        for _ 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])
+        GaussSeidelMaster.set_initial_states(units, order, initial_state)
         
-        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)
+        GaussSeidelMaster.compute_outputs(step, units, order, coupling, y, u)
         
         step=1
         last_rollback_step = 0
         
-        l.debug("Exiting init mode...")
-        for j in order:
-            unit = units[j];
-            unit.exitInitMode()
+        l.debug("Finishing init mode...")
+        
+        GaussSeidelMaster.finish_initialize(order, units)
         
         plot(t, step, y)
         
@@ -56,6 +127,12 @@ class GaussSeidelMaster():
             
             l.debug("New cosim step: %d at time %f", step, t)
             
+            (H,last_rollback_step) = GaussSeidelMaster.do_cosim_step(t, step, order, units, coupling, 
+                                            u, y, H_proposed, 
+                                            H, last_rollback_step,
+                                            min_steps_before_increase, max_step_size, step_increase_rate)
+            
+            """
             ok = False
             
             while not ok:
@@ -85,6 +162,7 @@ class GaussSeidelMaster():
                 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)