units tolerate jacobi iteration now, but Accurate zero crossing cannot be done.

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/AbstractSimulationUnit.py

@@ -195,7 +195,7 @@ class AbstractSimulationUnit(object):
         l.debug("<%s.setValues()", self._name)
         
     
-    def getValues(self, step, iteration, var_names=None):
+    def getValues(self, step, iteration, var_names=None, zoh=False):
         var_names = self.__state.keys() 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):
@@ -205,9 +205,17 @@ class AbstractSimulationUnit(object):
         values = {}
         for var in var_names:
             assert self.__state.has_key(var)
-            assert step < len(self.__state[var]), "State is not initialized: " + str(self.__state)
-            assert iteration < len(self.__state[var][step])
-            values[var] = self.__state[var][step][iteration]
+            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/CTSimulationUnit.py

@@ -9,6 +9,7 @@ import numpy
 
 from abstract_units.AbstractSimulationUnit import AbstractSimulationUnit, \
     STEP_ACCEPT
+from sympy.parsing.maxima import var_name
 
 
 l = logging.getLogger()
@@ -52,7 +53,8 @@ class CTSimulationUnit(AbstractSimulationUnit):
         next_state_idx = 0;
         previous_state_idx = 1;
         l.debug("Initialising input buffer...")
-        input_buffer = self.getValues(step, iteration, self._getInputVars())
+        # This tolerates both Jacobi and Gauss Seidel iteration.
+        input_buffer = self.getValues(step, iteration, self._getInputVars(), zoh=True)
         
         next_cosim_time = time+step_size
         

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/StatechartSimulationUnit_CTInOut.py

@@ -190,7 +190,8 @@ class StatechartSimulationUnit_CTInOut(AbstractSimulationUnit):
         assert iteration == 0, "Fixed point iterations involving this component are not supported."
         
         state_snapshot = self.getValues(step-1, iteration, [self.__current_state])
-        input_snapshot = self.getValues(step, iteration, self._getInputVars())
+        # zoh=True means that the unit tolerates both Gauss Seidel and Jacobi Iterations
+        input_snapshot = self.getValues(step, iteration, self._getInputVars(), zoh=True)
         previous_input_snaptshot = self.getValues(step-1, -1, self._getInputVars())
         
         (next_state, output_assignments) = self._doStepFunction(time+step_size, state_snapshot, input_snapshot, previous_input_snaptshot)

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

@@ -0,0 +1,263 @@
+"""
+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 abstract_units.AbstractSimulationUnit import STEP_DISCARD
+from case_study.units.adaptations.AccurateControllerArmatureAdaptation_Jacobi import AccurateControllerArmatureAdaptation_Jacobi
+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
+
+
+NUM_RTOL = 1e-08
+NUM_ATOL = 1e-08
+
+l = logging.getLogger()
+l.setLevel(logging.DEBUG)
+
+START_STEP_SIZE = 0.001
+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)
+
+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
+adapt_armature = AccurateControllerArmatureAdaptation_Jacobi("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)
+
+environment.enterInitMode()
+controller.enterInitMode()
+power.enterInitMode()
+adapt_armature.enterInitMode()
+window.enterInitMode()
+obstacle.enterInitMode()
+
+step = 0
+iteration = 0
+
+# Set environment initial inputs
+# Nothing to do
+
+# Get environment initial outputs
+envOut = environment.getValues(step, iteration, [environment.out_up, environment.out_down])
+
+# Set power initial state
+power.setValues(step, iteration, {
+                                 power.omega: 0.0, 
+                                 power.theta: 0.0, 
+                                 power.i: 0.0,
+                                 power.tau: 0.0, # Delayed input
+                                 power.up: 0.0, # Delayed input
+                                 power.down: 0.0    # Delayed input
+                                 })
+# Get power initial outputs
+pOut = power.getValues(step, iteration, [power.omega, power.theta, power.i])
+
+# Set obstacle initial inputs
+# Assume they are zero because the outputs of the window are delayed.
+obstacle.setValues(step, iteration, {obstacle.x: 0.0})
+# Get obstacle outputs
+oOut = obstacle.getValues(step, iteration, [obstacle.F])
+
+# Set window inputs
+window.setValues(step, iteration, {window.omega_input: pOut[power.omega],
+                            window.theta_input: pOut[power.theta],
+                            window.F_obj: oOut[obstacle.F]
+                            })
+# Get window outputs
+wOut = window.getValues(step, iteration, [window.x, window.tau])
+
+# Set adapt armature inputs and initial state
+adapt_armature.setValues(step, iteration, {adapt_armature.armature_current: pOut[power.i],
+                                adapt_armature.out_obj: 0.0 })
+# Get adapt armature outputs
+adaptArmOut = adapt_armature.getValues(step, iteration, [adapt_armature.out_obj])
+
+# Set the initial inputs to the controller
+controller.setValues(step, iteration, {controller.in_dup : envOut[environment.out_up],
+                            controller.in_ddown : envOut[environment.out_down],
+                            controller.in_obj : adaptArmOut[adapt_armature.out_obj]})
+# Get the controller output
+cOut = controller.getValues(step, iteration, [controller.out_up, controller.out_down])
+
+# Set the improved inputs to the components that got delayed inputs.
+power.setValues(step, iteration, {power.tau: wOut[window.tau], 
+                       power.up: cOut[controller.out_up],
+                       power.down: cOut[controller.out_down]})
+obstacle.setValues(step, iteration, {obstacle.x: wOut[window.x]})
+
+environment.exitInitMode()
+controller.exitInitMode()
+power.exitInitMode()
+adapt_armature.exitInitMode()
+window.exitInitMode()
+obstacle.exitInitMode()
+
+trace_i = [0.0]
+trace_omega = [0.0]
+trace_x = [0.0]
+trace_F = [0.0]
+times = [0.0]
+
+time = 0.0
+
+# This is always zero because we do not keep track of intermediate values in fixed point iterations
+iteration = 0 
+
+step_size = START_STEP_SIZE
+step = 1 # Step 0 has already been done by the initialization.
+last_rollback_step = 0
+
+while time < STOP_TIME:
+    step_completed = False
+    while not step_completed:
+        l.debug("Start step: %d at time %f", step, time)
+        """
+        This scenario will fail to execute because the adapt armature current is using the last two values to check the crossing.
+        When it checks the crossing, it is already too late and more steps need to be rolled back.
+        Not just one.
+        """
+        environment.doStep(time, step, iteration, step_size) 
+        power.doStep(time, step, iteration, step_size)
+        (step_info, proposed_step_size) = adapt_armature.doStep(time, step, iteration, step_size)
+        
+        if step_info == STEP_DISCARD:
+            l.debug("Rolling back step: %d at time %f", step, time)
+            """
+            In this abstraction, we don't need to do much.
+            But in real FMI, we would have to restore the state of the FMUs.
+            """
+            step_completed = False
+            last_rollback_step = step
+            l.debug("Decreasing step size from %f to %f...", step_size, proposed_step_size)            
+            step_size = proposed_step_size
+        else:
+            step_completed = True
+    
+    
+    envOut = environment.getValues(step, iteration, [environment.out_up, environment.out_down])
+    
+    cOut = controller.getValues(step-1, iteration, [controller.out_up, controller.out_down])
+    wOut = window.getValues(step-1, iteration, [window.x, window.tau])
+    
+    power.setValues(step, iteration, {
+                                     power.tau: wOut[window.tau], # Delayed input
+                                     power.up: cOut[controller.out_up], # Delayed input
+                                     power.down: cOut[controller.out_down]    # Delayed input
+                                     })
+    pOut = power.getValues(step, iteration, [power.omega, power.theta, power.i])
+    
+    """
+    We move the evaluation of the adapt armature component to as early as possible
+    because we know that it is the component that is most likely to reject the step size.
+    """
+    
+    # Set adapt armature inputs and initial state
+    adapt_armature.setValues(step, iteration, {adapt_armature.armature_current: pOut[power.i]})
+    
+    
+    # At this point, we know that the adapt_armature FMU has accepted the step size, so we can proceed.
+    
+    # Get adapt armature outputs
+    adaptArmOut = adapt_armature.getValues(step, iteration, [adapt_armature.out_obj])
+    
+    # The outputs of the window are delayed.
+    obstacle.setValues(step, iteration, {obstacle.x: wOut[window.x]}) # Delayed input
+    obstacle.doStep(time, step, iteration, step_size)
+    oOut = obstacle.getValues(step, iteration, [obstacle.F])
+    
+    window.setValues(step, iteration, {window.omega_input: pOut[power.omega],
+                                window.theta_input: pOut[power.theta],
+                                window.F_obj: oOut[obstacle.F]
+                                })
+    window.doStep(time, step, iteration, step_size)
+    wOut = window.getValues(step, iteration, [window.x, window.tau])
+    
+    
+    # Set the initial inputs to the controller
+    controller.setValues(step, iteration, {controller.in_dup : envOut[environment.out_up],
+                                controller.in_ddown : envOut[environment.out_down],
+                                controller.in_obj : adaptArmOut[adapt_armature.out_obj]})
+    controller.doStep(time, step, iteration, step_size)
+    
+    if (step - last_rollback_step) > MIN_STEPS_BEFORE_INCREASE_STEP_SIZE \
+                and step_size < MAX_STEP_SIZE:
+        new_step_size = step_size + step_size * (STEP_SIZE_INCREASE_RATE/100.0)
+        l.debug("Increasing step size from %f to %f...", step_size, new_step_size)            
+        step_size = new_step_size
+        if step_size > MAX_STEP_SIZE:
+            l.debug("Max step size attained: %f.", MAX_STEP_SIZE)  
+            step_size = MAX_STEP_SIZE
+    
+    
+    step += 1
+    time += step_size
+    times.append(time)
+    
+    trace_omega.append(pOut[power.omega])
+    trace_i.append(pOut[power.i])
+    trace_x.append(wOut[window.x])
+    trace_F.append(oOut[obstacle.F])
+
+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/scenarios/ControlledScenario_CTStatecharts_Jacobi.py

@@ -0,0 +1,240 @@
+"""
+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.
+- Inaccurate threshold crossing adaptation to the armature current output.
+"""
+
+import logging
+
+from bokeh.plotting import figure, output_file, show
+
+from case_study.units.adaptations.InacurateControllerArmatureAdaptation_CT import InacurateControllerArmatureAdaptation_CT
+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
+
+NUM_RTOL = 1e-08
+NUM_ATOL = 1e-08
+
+l = logging.getLogger()
+l.setLevel(logging.DEBUG)
+
+
+START_STEP_SIZE = 0.001
+FMU_START_RATE = 1
+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
+adapt_armature = InacurateControllerArmatureAdaptation_CT("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)
+
+environment.enterInitMode()
+controller.enterInitMode()
+power.enterInitMode()
+adapt_armature.enterInitMode()
+window.enterInitMode()
+obstacle.enterInitMode()
+
+step = 0
+iteration = 0
+
+# Set environment initial inputs
+# Nothing to do
+
+# Get environment initial outputs
+envOut = environment.getValues(step, iteration, [environment.out_up, environment.out_down])
+
+# Set power initial state
+power.setValues(step, iteration, {
+                                 power.omega: 0.0, 
+                                 power.theta: 0.0, 
+                                 power.i: 0.0,
+                                 power.tau: 0.0, # Delayed input
+                                 power.up: 0.0, # Delayed input
+                                 power.down: 0.0    # Delayed input
+                                 })
+# Get power initial outputs
+pOut = power.getValues(step, iteration, [power.omega, power.theta, power.i])
+
+# Set obstacle initial inputs
+# Assume they are zero because the outputs of the window are delayed.
+obstacle.setValues(step, iteration, {obstacle.x: 0.0})
+# Get obstacle outputs
+oOut = obstacle.getValues(step, iteration, [obstacle.F])
+
+# Set window inputs
+window.setValues(step, iteration, {window.omega_input: pOut[power.omega],
+                            window.theta_input: pOut[power.theta],
+                            window.F_obj: oOut[obstacle.F]
+                            })
+# Get window outputs
+wOut = window.getValues(step, iteration, [window.x, window.tau])
+
+# Set adapt armature inputs and initial state
+adapt_armature.setValues(step, iteration, {adapt_armature.armature_current: pOut[power.i],
+                                adapt_armature.out_obj: 0.0 })
+# Get adapt armature outputs
+adaptArmOut = adapt_armature.getValues(step, iteration, [adapt_armature.out_obj])
+
+# Set the initial inputs to the controller
+controller.setValues(step, iteration, {controller.in_dup : envOut[environment.out_up],
+                            controller.in_ddown : envOut[environment.out_down],
+                            controller.in_obj : adaptArmOut[adapt_armature.out_obj]})
+# Get the controller output
+cOut = controller.getValues(step, iteration, [controller.out_up, controller.out_down])
+
+# Set the improved inputs to the components that got delayed inputs.
+power.setValues(step, iteration, {power.tau: wOut[window.tau], 
+                       power.up: cOut[controller.out_up],
+                       power.down: cOut[controller.out_down]})
+obstacle.setValues(step, iteration, {obstacle.x: wOut[window.x]})
+
+environment.exitInitMode()
+controller.exitInitMode()
+power.exitInitMode()
+adapt_armature.exitInitMode()
+window.exitInitMode()
+obstacle.exitInitMode()
+
+trace_i = [0.0]
+trace_omega = [0.0]
+trace_x = [0.0]
+trace_F = [0.0]
+times = [0.0]
+
+time = 0.0
+
+# This is always zero because we do not keep track of intermediate values in fixed point iterations
+iteration = 0 
+
+for step in range(1, int(STOP_TIME / START_STEP_SIZE) + 1):
+    """
+    We have delays the following inputs:
+    - control inputs to the power
+    - window tau input to the power
+    - window height input to the obstacle
+    
+    The order of computation in the scenario is as follows:
+    environment
+    power
+    obstacle
+    window
+    armature adaptation
+    controller
+    
+    """
+    
+    # This is the jacobi type iteration
+    """
+    Notice that the units have the state and inputs defined at time t,
+    and we assume that they can compute their internal state up to time t+H.
+    """
+    environment.doStep(time, step, iteration, START_STEP_SIZE) 
+    power.doStep(time, step, iteration, START_STEP_SIZE)
+    obstacle.doStep(time, step, iteration, START_STEP_SIZE)
+    window.doStep(time, step, iteration, START_STEP_SIZE)
+    adapt_armature.doStep(time, step, iteration, START_STEP_SIZE)
+    controller.doStep(time, step, iteration, START_STEP_SIZE)
+    
+    # Set environment inputs
+    # Nothing to do
+    
+    # Get environment outputs
+    envOut = environment.getValues(step, iteration, [environment.out_up, environment.out_down])
+    
+    # get obstacle delayed inputs
+    #cOut = controller.getValues(step-1, iteration, [controller.out_up, controller.out_down])
+    wOut = window.getValues(step-1, iteration, [window.x])
+    
+    # Set obstacle inputs
+    # The outputs of the window are delayed.
+    obstacle.setValues(step, iteration, {obstacle.x: wOut[window.x]}) # Delayed input
+    
+    # Get obstacle outputs
+    oOut = obstacle.getValues(step, iteration, [obstacle.F])
+    
+    # Get power outputs
+    pOut = power.getValues(step, iteration, [power.omega, power.theta, power.i])
+    
+    # Set window inputs
+    window.setValues(step, iteration, {window.omega_input: pOut[power.omega],
+                                window.theta_input: pOut[power.theta],
+                                window.F_obj: oOut[obstacle.F]
+                                })
+    # Get window outputs
+    wOut = window.getValues(step, iteration, [window.tau, window.x])
+    
+    # Set adapt armature inputs and initial state
+    adapt_armature.setValues(step, iteration, {adapt_armature.armature_current: pOut[power.i]})
+    # Get adapt armature outputs
+    adaptArmOut = adapt_armature.getValues(step, iteration, [adapt_armature.out_obj])
+    
+    # Set the initial inputs to the controller
+    controller.setValues(step, iteration, {controller.in_dup : envOut[environment.out_up],
+                                controller.in_ddown : envOut[environment.out_down],
+                                controller.in_obj : adaptArmOut[adapt_armature.out_obj]})
+    # Get the controller output
+    cOut = controller.getValues(step, iteration, [controller.out_up, controller.out_down])
+    
+    # Set the inputs to the power.
+    power.setValues(step, iteration, {power.tau: wOut[window.tau], 
+                           power.up: cOut[controller.out_up],
+                           power.down: cOut[controller.out_down]})
+    
+    # Set the improved inputs to the obstacle (not supported by FMI without rolling back first)
+    # obstacle.setValues(step, iteration, {obstacle.x: wOut[window.x]})
+    
+    trace_omega.append(pOut[power.omega])
+    trace_i.append(pOut[power.i])
+    trace_x.append(wOut[window.x])
+    trace_F.append(oOut[obstacle.F])
+    time += START_STEP_SIZE
+    times.append(time)
+
+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

@@ -50,6 +50,7 @@ class AccurateControllerArmatureAdaptation(AbstractSimulationUnit):
         #assert self._biggerThan(step_size, 0), "step_size too small: {0}".format(step_size)
         assert iteration == 0, "Fixed point iterations not supported yet."
         
+        # This unit works best with a gauss seidel
         current_input = self.getValues(step, iteration, 
                                        self._getInputVars())[self.armature_current]
         previous_input = self.getValues(step-1, iteration, 

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

@@ -0,0 +1,98 @@
+import logging
+
+import numpy
+
+from abstract_units.AbstractSimulationUnit import AbstractSimulationUnit, \
+    STEP_ACCEPT, STEP_DISCARD
+
+
+l = logging.getLogger()
+
+class AccurateControllerArmatureAdaptation_Jacobi(AbstractSimulationUnit):
+    """
+    This is the adaptation of the armature signal coming from the power system and into the controller statechart.
+    The input will be the armature continuous signal.
+    The output will be a pulse signal obj that can either be 0, or 1 whenever the absolute value of 
+        the armature signal exceeds a certain threshold.
+    The discontinuity is accurately located by rejecting the step size whenever it is larger than a given tolerance.
+    
+    """
+    
+    def __init__(self, name, num_rtol, num_atol, threshold, upward):
+        
+        self._num_rtol = num_rtol
+        self._num_atol = num_atol
+        
+        assert upward, "Not implemented yet."
+        
+        self.__crossUpward = upward
+        
+        self.__threshold = threshold
+        
+        self.armature_current = "armature_current"
+        self.out_obj = "out_obj"
+        input_vars = [self.armature_current]
+        state_vars = [self.out_obj]
+        
+        algebraic_functions = {}
+        AbstractSimulationUnit.__init__(self, name, algebraic_functions, state_vars, input_vars)
+    
+    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 _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 self._biggerThan(step_size, 0), "step_size too small: {0}".format(step_size)
+        assert iteration == 0, "Fixed point iterations not supported yet."
+        
+        # This unit works only for Jacobi
+        current_input = self.getValues(step-1, iteration, 
+                                       self._getInputVars())[self.armature_current]
+        previous_input = self.getValues(step-2, iteration, 
+                                        self._getInputVars())[self.armature_current]
+        
+        output_value = 0
+        
+        l.debug("%s.previous_input=%f", self._name, previous_input)
+        l.debug("%s.current_input=%f", self._name, current_input)
+        
+        step_info = STEP_ACCEPT
+        proposed_step_size = step_size
+        
+        if self.__crossUpward:
+            if (not self._biggerThan(previous_input, self.__threshold)) \
+                    and self._biggerThan(current_input, self.__threshold):
+                l.debug("Crossing detected from %f to %f", 
+                        previous_input, current_input)
+                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."
+                
+            elif (not self._biggerThan(previous_input, self.__threshold))\
+                and self._isClose(current_input, self.__threshold):
+                l.debug("Crossing detected from %f to %f", 
+                        previous_input, current_input)
+                l.debug("And step size is acceptable.")
+                output_value = 1
+                
+        
+        l.debug("%s.output_value=%s", self._name, output_value)
+        self.setValues(step, iteration, {self.out_obj: output_value})
+        
+        l.debug("<%s._doInternalSteps() = (%s, %d)", 
+                self._name, step_info, proposed_step_size)
+        return (step_info, proposed_step_size)
+    
+    

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

@@ -71,7 +71,7 @@ class AlgebraicAdaptation_Power_Window_Obstacle(AbstractSimulationUnit):
         assert step_size > 0.0, "step_size too small: {0}".format(step_size)
         #assert self._biggerThan(step_size, 0), "step_size too small: {0}".format(step_size)
         assert iteration == 0, "Fixed point iterations not supported outside of this component."
-         
+        
         converged = False
         internal_iteration = 0
         

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, iteration, self._getInputVars())[self.armature_current]
-        previous_input = self.getValues(step-1, iteration, self._getInputVars())[self.armature_current]
+        current_input = self.getValues(step-1, iteration, self._getInputVars())[self.armature_current]
+        previous_input = self.getValues(step-2, iteration, self._getInputVars())[self.armature_current]
         
         output_value = 0