multi-rate adaptation working

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/AbstractSimulationUnit.py

@@ -75,13 +75,25 @@ class AbstractSimulationUnit(object):
         return self.__input_vars
     
     def rollback(self, toStep):
+        l.debug(">%s.rollback(%d)", self._name, toStep)
         # Cleans the computed_time from toStep onwards.
         assert toStep <= len(self.__computed_time), "Cannot rollback to a future step"
-        Utils.trimList(self.__computed_time, toStep)
-        raise "To be finished"
+        
+        Utils.trimList(self.__computed_time, toStep+1)
+        
+        for var in self.__state.keys():
+            l.debug("Rolling back %s...", var)
+            Utils.trimList(self.__state[var], toStep+1)
+            l.debug("len(self.__state[var])=%d", len(self.__state[var]))
+        
+        
+        l.debug("<%s.rollback()", self._name)
+    
+    def commit(self, step):
+        pass
     
     def __recordTime(self, time, step, iteration, step_size):
-        assert step <= len(self.__computed_time)
+        assert step <= len(self.__computed_time), "step = " + str(step) + " len(self.__computed_time)=" + str(len(self.__computed_time))
         if step == len(self.__computed_time):
             assert iteration == 0
             self.__computed_time.append([time + step_size])

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/MultiRateAdaptationUnit.py

@@ -75,6 +75,7 @@ class MultiRateAdaptationUnit(AbstractSimulationUnit):
             self._y.append(None)
         
         self._step = 0
+        self._last_commited_step = 0
         
         GaussSeidelMaster.start_initialize(self._order, self._units)
         
@@ -91,6 +92,26 @@ class MultiRateAdaptationUnit(AbstractSimulationUnit):
 
         l.debug("<%s.MultiRateAdaptationUnit.exitInitMode()", self._name)
     
+    def rollback(self, toStep):
+        l.debug(">%s.MultiRateAdaptationUnit.rollback()", self._name)
+        AbstractSimulationUnit.rollback(self, toStep)
+        
+        for unit in self._units:
+            unit.rollback(self._last_commited_step)
+        
+        self._step = self._last_commited_step
+        
+        l.debug("<%s.MultiRateAdaptationUnit.rollback()", self._name)
+    
+    def commit(self, step):
+        l.debug(">%s.MultiRateAdaptationUnit.commit()", self._name)
+        self._last_commited_step = self._step
+        
+        for unit in self._units:
+            unit.commit(self._step)
+        
+        l.debug("<%s.MultiRateAdaptationUnit.commit()", 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)
@@ -100,6 +121,8 @@ class MultiRateAdaptationUnit(AbstractSimulationUnit):
         
         next_external_cosim_time = time+step_size
         
+        l.debug("%s: Next cosim synch time=%f", self._name, next_external_cosim_time)
+        
         max_step_size = step_size/self._start_rate
         
         H = max_step_size
@@ -107,7 +130,7 @@ class MultiRateAdaptationUnit(AbstractSimulationUnit):
         l.debug("max_step_size=%f", max_step_size)
         
         internal_time = time;
-
+        
         u = []
         H_proposed = []
         
@@ -123,20 +146,25 @@ class MultiRateAdaptationUnit(AbstractSimulationUnit):
             
             self._step = self._step + 1
             
-            if (internal_time + H) >= next_external_cosim_time:
+            if (internal_time + H) > next_external_cosim_time:
                 H = next_external_cosim_time - internal_time
+                l.debug("Adjusting step size to meet synch point: H=%f", H)
+            
+            assert H > 0.0
             
             (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)
+            if internal_time > next_external_cosim_time or \
+                    self._isClose(internal_time, next_external_cosim_time):
+                do_next_step = False
+                l.debug("Finished step: %d.", self._step)
+            
             
         l.debug("<%s._doInternalSteps() = (%s, %f)", self._name, STEP_ACCEPT, step_size)
         return (STEP_ACCEPT, step_size)

SemanticAdaptationForFMI/FMIAbstraction/src/abstract_units/Utils.py

@@ -33,7 +33,7 @@ class Utils(object):
         if step == len(target[var]):
             target[var].append([value])
         elif step < len(target[var]):
-            assert step == len(target[var]) - 1, "Makes no sense to rewrite past steps, without rolling back first."
+            assert step == len(target[var]) - 1, "Makes no sense to rewrite past steps, without rolling back first. len(target[var])={}".format(len(target[var]))
             if iteration == len(target[var][step]):
                 target[var][step].append(value)
             elif iteration < len(target[var][step]):
@@ -48,8 +48,10 @@ class Utils(object):
     
     @staticmethod
     def trimList(listToTrim, target_size):
-        assert target_size <= len(listToTrim)
         assert target_size >= 0
+        if target_size > len(listToTrim):
+            return
+        
         while target_size < len(listToTrim):
             del listToTrim[-1]
         assert target_size == len(listToTrim)

SemanticAdaptationForFMI/FMIAbstraction/src/case_study/units/de_based/EnvironmentStatechartFMU_CTInOut.py

@@ -23,7 +23,7 @@ class EnvironmentStatechartFMU_CTInOut(StatechartSimulationUnit_CTInOut):
         output_vars = [self.out_up , self.out_down]
         
         initial_state = "Initial"
-                
+        
         def state_transition(current_state, inputs, previous_inputs, elapsed, inputs_available):
             l.debug(">%s.state_transition(%s, %s, %f)", self._name, current_state, inputs, elapsed)
             

SemanticAdaptationForFMI/FMIAbstraction/src/master/GaussSeidelMaster.py

@@ -64,12 +64,17 @@ class GaussSeidelMaster():
             min_H = min(H_proposed)
             if min_H < H:
                 l.debug("Rolling back step: %d at time %f", step, t)
+                for unit in units:
+                    unit.rollback(step)
                 ok = False
                 last_rollback_step = step
                 l.debug("Decreasing step size from %f to %f...", H, min_H)            
                 H = min_H
             else:
                 ok = True
+        
+        for unit in units:
+            unit.commit(step)
             
         if (step - last_rollback_step) > min_steps_before_increase \
             and H < max_step_size: