power case study adaptations. Almost done

SemanticAdaptationForFMI/FMIAbstraction/src/sampleunits/InacurateControllerArmatureAdaptation.py

@@ -0,0 +1,98 @@
+import logging
+
+import numpy
+
+from units.AbstractSimulationUnit import AbstractSimulationUnit, STEP_ACCEPT
+
+
+l = logging.getLogger()
+
+class InacurateControllerArmatureAdaptation(AbstractSimulationUnit):
+    """
+    This is the adaptation of the armature signal coming from the power system and into the controller statechart.
+    It therefore gets a continuous input and outputs events.
+    The input will be the armature continuous signal.
+    The output will be the event "obj" whenever the absolute value of 
+        the armature signal exceeds a certain threshold.
+        
+    Note that we could have coded a generic signal crossing adaptation unit 
+        and there is a whole range of pathological cases that we are not dealing with now.
+        For these, see the implementation of the AbstractZeroCrossingBlock 
+        of the DiracCBD Simulator
+        
+    The detailed behaviour of this block is as follows:
+    ______________________________
+    f = InacurateControllerArmatureAdaptation(...)
+    f.enterInitMode()
+    f.setValues(...,armature)
+        The FMU records this value in its internal state.
+    "" = f.getValues(...)
+        The empty event is returned because at time t=0, there can be no crossing. 
+        Because there was no doStep call in between, the event returned here can only be None.
+    f.exitInitMode()
+    
+    f.setValues(..., None)
+        The FMU records the value as the current input in its internal state.
+    f.doStep(..., H)
+        The FMU checks whether the current input has crossed the given threshold, 
+            taking into account the previous value of the input, 
+            already stored in the internal state.
+        The output value is then calculated and the current input moved into the internal state.
+    "someReaction" = f.getValues(...)
+        Gets the output event. Can either be absent, or the obj event.
+    ______________________________
+    
+    """
+    
+    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.previous_input = "previous_input"
+        self.out_event = "out_event"
+        input_vars = [self.armature_current]
+        state_vars = [self.previous_input, self.out_event]
+        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, cosim_step_size):
+        l.debug(">%s._doInternalSteps(%f, %d, %d, %f)", self._name, time, step, iteration, cosim_step_size)
+        
+        assert self._biggerThan(cosim_step_size, 0), "cosim_step_size too small: {0}".format(cosim_step_size)
+        assert iteration == 0, "Fixed point iterations not supported yet."
+        
+        previous_input = self.getValues(step-1, iteration, self._getStateVars())[self.previous_input]
+        current_input = self.getValues(step, iteration, self._getInputVars())[self.armature_current]
+        
+        output_event = ""
+        
+        l.debug("previous_input=%f", previous_input)
+        l.debug("current_input=%f", current_input)
+        
+        if (not self._biggerThan(previous_input, self.__threshold)) \
+                and self._biggerThan(current_input, self.__threshold) \
+                and self.__crossUpward:
+            output_event = "obj"
+        
+        l.debug("output_event=%f", previous_input)
+        self.setValues(step, iteration, {self.out_event: output_event})
+        
+        # Commit the new state and discard previous.
+        self.setValues(step, iteration, {self.previous_input: current_input})
+        
+        l.debug("<%s._doInternalSteps() = (%s, %d)", self._name, STEP_ACCEPT, cosim_step_size)
+        return (STEP_ACCEPT, cosim_step_size)

SemanticAdaptationForFMI/FMIAbstraction/src/sampleunits/ObstacleFMU.py

@@ -13,10 +13,10 @@ class ObstacleFMU(CTSimulationUnit_Euler):
         
         state_derivatives = {}
         
-        output_functions = {
+        algebraic_functions = {
                             self.F : calc_F
                             }
         
-        CTSimulationUnit_Euler.__init__(self, name, num_rtol, num_atol, internal_step_size, state_derivatives, output_functions, input_vars)
+        CTSimulationUnit_Euler.__init__(self, name, num_rtol, num_atol, internal_step_size, state_derivatives, algebraic_functions, input_vars)
     
     

SemanticAdaptationForFMI/FMIAbstraction/src/sampleunits/PowerFMU.py

@@ -32,6 +32,8 @@ class PowerFMU(CTSimulationUnit_Euler):
                              self.i: der_i
                              }
         
+        algebraic_functions = []
+        
         CTSimulationUnit_Euler.__init__(self, name, num_rtol, num_atol, internal_step_size, state_derivatives, {}, input_vars)
     
     

SemanticAdaptationForFMI/FMIAbstraction/src/sampleunits/PowerInputAdaptation.py

@@ -0,0 +1,58 @@
+import logging
+
+from units.AbstractSimulationUnit import AbstractSimulationUnit, STEP_ACCEPT
+
+l = logging.getLogger()
+
+class PowerInputAdaptation(AbstractSimulationUnit):
+    """
+    This is the adaptation of the events coming out of the Controller Statechart.
+    It gets as input an event, and output two signals, to be coupled to the power system.
+    Whenever it gets an input, it stores the appropriate signals for the output until another event comes along.
+    It's basically a ZOH.
+    """
+    
+    def __init__(self, name):
+        
+        self.out_down = "out_down"
+        self.out_up = "out_up"
+        self.in_event = "in_event"
+        self.__current_down = "__current_down"
+        self.__current_up = "__current_up"
+        input_vars = [self.in_event]
+        state_vars = [self.__current_down, self.__current_up]
+        
+        def calc_up(x, u):
+            in_ev = x[self.in_event]
+            output = x[self.__current_up]
+            if in_ev != "":
+                output = 1.0 if in_ev=="up" else 0.0
+            return output
+        
+        def calc_down(x, u):
+            in_ev = x[self.in_event]
+            output = x[self.__current_down]
+            if in_ev != "":
+                output = 1.0 if in_ev=="down" else 0.0
+            return output
+        
+        algebraic_functions = {self.out_up : calc_up, self.out_down: calc_down}
+        
+        AbstractSimulationUnit.__init__(self, name, algebraic_functions, state_vars, input_vars)
+    
+    def _doInternalSteps(self, time, step, iteration, cosim_step_size):
+        l.debug(">%s._doInternalSteps(%f, %d, %d, %f)", self._name, time, step, iteration, cosim_step_size)
+        
+        assert iteration == 0, "Fixed point iterations not supported yet."
+        
+        current_output = self.getValues(step, iteration, [self.out_up, self.out_down])
+        
+        l.debug("current_output=%f", current_output)
+        
+        # Store the values into the state
+        self.setValues(step, iteration, {self.__current_down: current_output[self.out_down],
+                                         self.__current_up: current_output[self.out_up] })
+        
+        
+        l.debug("<%s._doInternalSteps() = (%s, %d)", self._name, STEP_ACCEPT, cosim_step_size)
+        return (STEP_ACCEPT, cosim_step_size)

SemanticAdaptationForFMI/FMIAbstraction/src/sampleunits/WindowFMU.py

@@ -30,12 +30,12 @@ class WindowFMU(CTSimulationUnit_Euler):
                              self.omega: der_omega
                              }
         
-        output_functions = {
+        algebraic_functions = {
                             self.v : calc_v,
                             self.x : calc_x,
                             self.tau: calc_tau
                             }
         
-        CTSimulationUnit_Euler.__init__(self, name, num_rtol, num_atol, internal_step_size, state_derivatives, output_functions, input_vars)
+        CTSimulationUnit_Euler.__init__(self, name, num_rtol, num_atol, internal_step_size, state_derivatives, algebraic_functions, input_vars)
     
     

SemanticAdaptationForFMI/FMIAbstraction/src/scenarios/ControlledScenario.py

@@ -0,0 +1,200 @@
+import logging
+
+from bokeh.plotting import figure, output_file, show
+
+from sampleunits.DriverControllerStatechartFMU import DriverControllerStatechartFMU
+from sampleunits.EnvironmentStatechartFMU import EnvironmentStatechartFMU
+from sampleunits.InacurateControllerArmatureAdaptation import InacurateControllerArmatureAdaptation
+from sampleunits.ObstacleFMU import ObstacleFMU
+from sampleunits.PowerFMU import PowerFMU
+from sampleunits.PowerInputAdaptation import PowerInputAdaptation
+from sampleunits.WindowFMU import WindowFMU
+
+
+NUM_RTOL = 1e-08
+NUM_ATOL = 1e-08
+
+l = logging.getLogger()
+l.setLevel(logging.DEBUG)
+
+cosim_step_size = 0.001
+num_internal_steps = 10
+stop_time = 6.0;
+
+environment = EnvironmentStatechartFMU("env", NUM_RTOL, NUM_ATOL)
+
+controller = DriverControllerStatechartFMU("controller", NUM_RTOL, NUM_ATOL)
+
+power = PowerFMU("power", NUM_RTOL, NUM_ATOL, cosim_step_size/num_internal_steps, 
+                     J=0.085, 
+                     b=5, 
+                     K=1.8, 
+                     R=0.15, 
+                     L=0.036,
+                     V=12)
+
+armature_threshold = 30.0
+adapt_armature = InacurateControllerArmatureAdaptation("arm_adapt", NUM_RTOL, NUM_ATOL, armature_threshold, True)
+
+adapt_power_input = PowerInputAdaptation("power_adapt")
+
+window_radius = 0.017
+
+window = WindowFMU("window", NUM_RTOL, NUM_ATOL, cosim_step_size/num_internal_steps, 
+                     J=0.085, 
+                     r=window_radius, 
+                     b = 150, 
+                     c = 1e3) # c = 1e5 makes this an unstable system.
+
+obstacle = ObstacleFMU("obstacle", NUM_RTOL, NUM_ATOL, cosim_step_size/num_internal_steps, 
+                     c=1e5, 
+                     fixed_x=0.45)
+
+environment.enterInitMode()
+controller.enterInitMode()
+power.enterInitMode()
+adapt_armature.enterInitMode()
+adapt_power_input.enterInitMode()
+window.enterInitMode()
+obstacle.enterInitMode()
+
+# Solve initialisation.
+"""
+The initialisation may impose a completely different order for the execution of the FMUs.
+In this case we know that there is no direct feed-through between the power input and its outputs,
+    so we can safely set its initial state, get its output, and then compute all the other FMUs,
+    before setting the new inputs to the power.
+"""
+
+pOut = power.setValues(0, 0, {
+                                 power.omega: 0.0, 
+                                 power.theta: 0.0, 
+                                 power.i: 0.0
+                                 })
+pOut = power.getValues(0, 0, [power.omega, power.theta, power.i])
+
+window.setValues(0, 0, {window.omega_input: pOut[power.omega],
+                            window.theta_input: pOut[power.theta],
+                            window.theta: 0.0,
+                            window.omega: 0.0
+                            })
+wOut = window.getValues(0, 0, [window.tau, window.x])
+
+obstacle.setValues(0, 0, {obstacle.x: wOut[window.x]})
+oOut = obstacle.getValues(0, 0, [obstacle.F])
+
+
+adapt_armature.setValues(0, 0, {adapt_armature.armature_current: pOut[power.i]})
+
+adaptArmOut = adapt_armature.getValues(0, 0, [adapt_armature.out_event])
+
+envOut = environment.getValues(0, 0, [environment.out_event])
+
+# coupling equation for the input event of the controller
+controller_in = adaptArmOut[adapt_armature.out_event] + envOut[environment.out_event]
+if adaptArmOut[adapt_armature.out_event] != "" and envOut[environment.out_event] != "":
+    controller_in = adaptArmOut[adapt_armature.out_event]
+controller.setValues(0, 0, {controller.in_event : controller_in})
+
+cOut = controller.getValues(0, 0, [controller.out_event])
+
+adapt_power_input.setValues(0, 0, {adapt_power_input.in_event : cOut[controller.out_event]})
+adaptPowerOut = adapt_power_input.getValues(0, 0, [adapt_power_input.out_up, adapt_power_input.out_down])
+
+# Coupling equation for power
+power_tau = - ( wOut[window.tau] + window_radius * oOut[obstacle.F])
+
+# Finally set the other power inputs
+power.setValues(0, 0, {power.tau: power_tau, 
+                       power.up: adaptPowerOut.out_up,
+                       power.down: adaptPowerOut.out_down})
+
+
+environment.exitInitMode()
+controller.exitInitMode()
+power.exitInitMode()
+adapt_armature.exitInitMode()
+adapt_power_input.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
+
+for step in range(1, int(stop_time / cosim_step_size) + 1):
+    
+    pOut = power.getValues(step, 0, [power.omega, power.theta, power.i])
+    
+    window.setValues(step, 0, {window.omega_input: pOut[power.omega],
+                            window.theta_input: pOut[power.theta],
+                            window.theta: 0.0,
+                            window.omega: 0.0
+                            })
+    wOut = window.getValues(step, 0, [window.tau, window.x])
+    
+    obstacle.setValues(step, 0, {obstacle.x: wOut[window.x]})
+    oOut = obstacle.getValues(step, 0, [obstacle.F])
+    
+    adapt_armature.setValues(step, 0, {adapt_armature.armature_current: pOut[power.i]})
+
+    adaptArmOut = adapt_armature.getValues(step, 0, [adapt_armature.out_event])
+    
+    envOut = environment.getValues(step, 0, [environment.out_event])
+    
+    # coupling equation for the input event of the controller
+    controller_in = adaptArmOut[adapt_armature.out_event] + envOut[environment.out_event]
+    if adaptArmOut[adapt_armature.out_event] != "" and envOut[environment.out_event] != "":
+        controller_in = adaptArmOut[adapt_armature.out_event]
+    controller.setValues(step, 0, {controller.in_event : controller_in})
+    
+    cOut = controller.getValues(step, 0, [controller.out_event])
+    
+    adapt_power_input.setValues(step, 0, {adapt_power_input.in_event : cOut[controller.out_event]})
+    adaptPowerOut = adapt_power_input.getValues(step, 0, [adapt_power_input.out_up, adapt_power_input.out_down])
+    
+    # Coupling equation for power
+    power_tau = - ( wOut[window.tau] + window_radius * oOut[obstacle.F])
+    
+    # Finally set the other power inputs
+    power.setValues(step, 0, {power.tau: power_tau, 
+                           power.up: adaptPowerOut.out_up,
+                           power.down: adaptPowerOut.out_down})
+    
+    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 += cosim_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/units/AbstractSimulationUnit.py

@@ -32,19 +32,19 @@ class AbstractSimulationUnit(object):
     communication step size.
     """
     
-    def __init__(self, name, output_functions, state_vars, input_vars):
-        l.debug(">AbstractSimulationUnit(%s, %s, %s, %s)", name, output_functions, state_vars, input_vars)
+    def __init__(self, name, algebraic_functions, state_vars, input_vars):
+        l.debug(">AbstractSimulationUnit(%s, %s, %s, %s)", name, algebraic_functions, state_vars, input_vars)
         
         self._name = name
         self.__state = {}
-        self.__outputs_dirty = {}
+        self.__alg_value_dirty = {}
         self.__computed_time = []
         self.__mode = INSTANTIATED_MODE
         
-        self.__out_functions = output_functions
+        self.__algebraic_functions = algebraic_functions
         self.__state_vars = state_vars
         self.__input_vars = input_vars
-        self.__output_vars = output_functions.keys()
+        self.__output_vars = algebraic_functions.keys()
         self.__state_and_input_vars = list(state_vars)
         self.__state_and_input_vars.extend(input_vars)
         
@@ -54,7 +54,7 @@ class AbstractSimulationUnit(object):
             self.__state[var] = []
         for var in self.__output_vars:
             self.__state[var] = []
-            self.__outputs_dirty[var] = False
+            self.__alg_value_dirty[var] = False
         for var in input_vars:
             self.__state[var] = []
         
@@ -91,9 +91,9 @@ class AbstractSimulationUnit(object):
         
         """ This is not done here because it may not be possible to do it for 
                 the current step as it has not been computed.
-        if self.__outputs_dirty:
+        if self.__alg_value_dirty:
             self.__computeOutputs(step, current_iteration)
-            self.__outputs_dirty = False
+            self.__alg_value_dirty = False
         """
         
         # Record time, even if the step is not accepted.
@@ -121,11 +121,11 @@ class AbstractSimulationUnit(object):
         l.debug("outputs to update = %s", self.__output_vars)
         
         for var in self.__output_vars:
-            new_value = self.__out_functions[var](current_state_snaptshot, current_input_snaptshot)
+            new_value = self.__algebraic_functions[var](current_state_snaptshot, current_input_snaptshot)
             Utils.copyValueToStateTrace(self.__state, var, step, iteration, new_value)
         
         l.debug("Updated portion of the state:\n%s", 
-                    self._printState(step, iteration, self.__out_functions.keys()))
+                    self._printState(step, iteration, self.__algebraic_functions.keys()))
         
         l.debug("<__computeOutputs()")
         
@@ -139,17 +139,17 @@ class AbstractSimulationUnit(object):
             strState += var + "=" + str(self.__state[var][step][iteration]) + "\n"
         return strState
     
-    def __markOutputsDirty(self, step, iteration, whichOnes=None):
+    def __markOutputs(self, step, iteration, dirty, whichOnes=None):
         output_vars = self.__output_vars if whichOnes==None else whichOnes
         for out_var in output_vars:
-            if self.__outputs_dirty.has_key(out_var):
-                self.__outputs_dirty[out_var] = True
+            if self.__alg_value_dirty.has_key(out_var):
+                self.__alg_value_dirty[out_var] = dirty
             
     def __areOutputsDirty(self, step, iteration, whichOnes=None):
         var_names = self.__output_vars if whichOnes==None else whichOnes
         for out_var in var_names:
-            if self.__outputs_dirty.has_key(out_var):
-                if self.__outputs_dirty[out_var]:
+            if self.__alg_value_dirty.has_key(out_var):
+                if self.__alg_value_dirty[out_var]:
                     return True
         return False
         
@@ -157,7 +157,7 @@ class AbstractSimulationUnit(object):
         l.debug(">%s.setValues(%d, %d, %s)", self._name, step, iteration, values)
         Utils.copyMapToStateTrace(self.__state, step, iteration, values)
         
-        self.__markOutputsDirty(step, iteration);
+        self.__markOutputs(step, iteration, dirty=True);
         
         l.debug("New state: \n%s", self._printState(step,iteration, values.keys()))
         l.debug("<%s.setValues()", self._name)
@@ -167,7 +167,7 @@ class AbstractSimulationUnit(object):
         l.debug(">%s.getValues(%d, %d, %s)", self._name, step, iteration, var_names)
         if self.__areOutputsDirty(step, iteration, var_names):
             self.__computeOutputs(step, iteration)
-            self.__markOutputsDirty(step, iteration, var_names);
+            self.__markOutputs(step, iteration, dirty=False, whichOnes=var_names);
         
         values = {}
         for var in var_names:
@@ -191,7 +191,7 @@ class AbstractSimulationUnit(object):
         l.debug(">%s.exitInitMode()", self._name)
         if self.__areOutputsDirty(0,0):
             self.__computeOutputs(0,0)
-            self.__markOutputsDirty(0,0)
+            self.__markOutputs(0,0, dirty=False)
         self.__mode = STEP_MODE
         l.debug("<%s.exitInitMode()", self._name)
         

SemanticAdaptationForFMI/FMIAbstraction/src/units/CTSimulationUnit.py

@@ -19,9 +19,9 @@ class CTSimulationUnit(AbstractSimulationUnit):
     """
     
     def __init__(self, name, num_rtol, num_atol, 
-                                  state_derivatives, output_functions, 
+                                  state_derivatives, algebraic_functions, 
                                   input_vars):
-        AbstractSimulationUnit.__init__(self, name, output_functions, state_derivatives.keys(), input_vars)
+        AbstractSimulationUnit.__init__(self, name, algebraic_functions, state_derivatives.keys(), input_vars)
         
         self._num_rtol = num_rtol
         self._num_atol = num_atol

SemanticAdaptationForFMI/FMIAbstraction/src/units/CTSimulationUnit_Euler.py

@@ -17,10 +17,10 @@ class CTSimulationUnit_Euler(CTSimulationUnit):
     """
     
     def __init__(self, name, num_rtol, num_atol, internal_step_size,
-                 state_derivatives, output_functions, 
+                 state_derivatives, algebraic_functions, 
                  input_vars):
         CTSimulationUnit.__init__(self, name, num_rtol, num_atol,
-                                  state_derivatives, output_functions,
+                                  state_derivatives, algebraic_functions,
                                   input_vars)
         
         self.__internal_step_size = internal_step_size

SemanticAdaptationForFMI/FMIAbstraction/src/units/StatechartSimulationUnit.py

@@ -73,7 +73,7 @@ class StatechartSimulationUnit(AbstractSimulationUnit):
         
         self.__last_transition_time = 0.0
         
-        AbstractSimulationUnit.__init__(self, name, {}, [self.__current_state, self.out_event], [self.__in_event])
+        AbstractSimulationUnit.__init__(self, name, {}, [self.__current_state, self.out_event], [self.in_event])
     
     def _isClose(self, a, b):
         return numpy.isclose(a,b, self._num_rtol, self._num_atol)