Output all CVODE steps

src/OMSimulatorLib/SystemSC.cpp

@@ -534,23 +534,23 @@ oms_status_enu_t oms::SystemSC::doStep()
   switch(solverMethod)
   {
     case oms_solver_sc_explicit_euler:
-      return doStepEuler();
+      return doStepEuler(time + maximumStepSize);
 
     case oms_solver_sc_cvode:
-      return doStepCVODE();
+      return doStepCVODE(time + maximumStepSize);
 
     default:
       return logError_InternalError;
   }
 }
 
-oms_status_enu_t oms::SystemSC::doStepEuler()
+oms_status_enu_t oms::SystemSC::doStepEuler(double stopTime)
 {
   fmi2Status fmistatus;
   oms_status_enu_t status;
 
   // Step 1: Initialize state variables and time
-  const fmi2Real end_time = std::min(time + maximumStepSize, getModel().getStopTime());
+  const fmi2Real end_time = std::min(time + maximumStepSize, stopTime);
   const fmi2Real event_time_tolerance = 1e-4;
 
   logDebug("doStepEuler: " + std::to_string(time) + " -> " + std::to_string(end_time));
@@ -766,13 +766,14 @@ oms_status_enu_t oms::SystemSC::doStepEuler()
   return oms_status_ok;
 }
 
-oms_status_enu_t oms::SystemSC::doStepCVODE()
+oms_status_enu_t oms::SystemSC::doStepCVODE(double stopTime)
 {
   fmi2Status fmistatus;
   oms_status_enu_t status;
-  int flag;
+  int flag = 0;
+  bool tnext_is_event = false;
 
-  const fmi2Real end_time = std::min(time + maximumStepSize, getModel().getStopTime());
+  const fmi2Real end_time = stopTime;
 
   // find next time event
   fmi2Real tnext = end_time+1.0;
@@ -788,16 +789,59 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
       time = end_time;
     }
   }
+
+  tnext_is_event = tnext <= end_time;
+  tnext = std::min(tnext, end_time);
+
   logDebug("tnext: " + std::to_string(tnext));
 
-  while (time < end_time)
-  {
+  //while (time < end_time)
+  //{
     logDebug("CVode: " + std::to_string(time) + " -> " + std::to_string(end_time));
     for (size_t j=0, k=0; j < fmus.size(); ++j)
       for (size_t i=0; i < nStates[j]; ++i, ++k)
         NV_Ith_S(solverData.cvode.y, k) = states[j][i];
 
-    flag = CVode(solverData.cvode.mem, std::min(tnext, end_time), solverData.cvode.y, &time, CV_NORMAL);
+    // This will force the solver to take a step only to tnext
+    // The 'tout' argument for CVode will integrate beyond it and return an interpolant
+    CVodeSetStopTime(solverData.cvode.mem, tnext);
+
+    // Advance integrator (to end of step or next root)
+    double cvode_time = time;
+    flag = CVode(solverData.cvode.mem, tnext, solverData.cvode.y, &cvode_time, CV_ONE_STEP);
+    if (flag < 0)
+      return logError("SUNDIALS_ERROR: CVode() failed with flag = " + std::to_string(flag));
+
+    // CVode will return an interpolant for roots. In that case it should always be reset at the root.
+    // Otherwise it will make use of rhs calculations made before the event.
+    // This will cause it to ignore any changes in the rhs from current time to cv_mem->cv_tn.
+    bool is_interpolated = flag == CV_ROOT_RETURN;
+
+    // Sanity check, should not be triggered.
+    // To avoid resorting to this, CV_NORMAL is used above when tnext is too close.
+    if (cvode_time > tnext)
+    {
+      logWarning("SystemSC::doStepCVODE: Stopping time overstepped by CVODE");
+
+      // This fails to do the necessary interpolation when the previous call has stopped at a root.
+      // flag = CVode(solverData.cvode.mem, tnext, solverData.cvode.y, &cvode_time, CV_NORMAL);
+      // if (flag < 0)
+      //   return logError("SUNDIALS_ERROR: CVode() failed with flag = " + std::to_string(flag));
+
+      // Issue with the approach below:
+      // - Internal time of CVODE stays at previously returned value.
+      // - This may cause it to skip a root.
+
+      // Interpolate states to value at tnext
+      int retval = CVodeGetDky(solverData.cvode.mem, tnext, 0, solverData.cvode.y);
+      if (retval != CV_SUCCESS)
+        return logError("SUNDIALS_ERROR: CVodeGetDky() failed with flag = " + std::to_string(retval));
+
+      flag = CV_TSTOP_RETURN;
+      cvode_time = tnext;
+    }
+
+    time = cvode_time;
 
     for (size_t i = 0, j=0; i < fmus.size(); ++i)
     {
@@ -809,7 +853,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
       if (oms_status_ok != status) return status;
     }
 
-    if (flag == CV_ROOT_RETURN || time == tnext)
+    if (flag == CV_ROOT_RETURN || tnext_is_event && time == tnext)
     {
       logDebug("event found!!! " + std::to_string(time));
 
@@ -839,15 +883,6 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
         if (fmi2OK != fmistatus) logError_FMUCall("fmi2_enterContinuousTimeMode", fmus[i]);
       }
 
-      for (size_t i = 0; i < fmus.size(); ++i)
-      {
-        if (0 == nStates[i])
-          continue;
-
-        status = fmus[i]->getContinuousStates(states[i]);
-        if (oms_status_ok != status) return status;
-      }
-
       // find next time event
       tnext = end_time+1.0;
       for (size_t i = 0; i < fmus.size(); ++i)
@@ -862,6 +897,10 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
           time = end_time;
         }
       }
+
+      tnext_is_event = tnext <= end_time;
+      tnext = std::min(tnext, end_time);
+
       logDebug("tnext: " + std::to_string(tnext));
 
       // emit the right limit of the event
@@ -885,10 +924,10 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
       flag = CVodeReInit(solverData.cvode.mem, time, solverData.cvode.y);
       if (flag < 0) return logError("SUNDIALS_ERROR: CVodeReInit() failed with flag = " + std::to_string(flag));
 
-      continue;
+      return oms_status_ok;
     }
 
-    if (flag == CV_SUCCESS)
+    if (flag >= 0) // Some kind of success
     {
       logDebug("CVode completed successfully at t = " + std::to_string(time));
 
@@ -914,7 +953,7 @@ oms_status_enu_t oms::SystemSC::doStepCVODE()
     }
     else
       return logError("CVode failed with flag = " + std::to_string(flag));
-  }
+  //}
 
   return oms_status_ok;
 
@@ -930,14 +969,29 @@ oms_status_enu_t oms::SystemSC::stepUntil(double stopTime)
 
   // main simulation loop
   oms_status_enu_t status = oms_status_ok;
-  while (time < std::min(stopTime, getModel().getStopTime()) && oms_status_ok == status)
+  double endTime = std::min(stopTime, getModel().getStopTime());
+  double lastTime = time;
+  while (time < endTime && oms_status_ok == status)
   {
-    status = doStep();
+    if (solverMethod == oms_solver_sc_explicit_euler)
+      status = doStepEuler(endTime);
+    else if (solverMethod == oms_solver_sc_cvode)
+      status = doStepCVODE(endTime);
+    else
+      return logError_InternalError;
+
     if (status != oms_status_ok)
       logWarning("Bad return code at time " + std::to_string(time));
 
-    if (isTopLevelSystem() && Flags::ProgressBar())
+    // Check whether stopping time has changed due to a request from an FMU
+    if (getModel().getStopTime() < endTime)
+      endTime = getModel().getStopTime();
+
+    if (isTopLevelSystem() && Flags::ProgressBar() && time > lastTime + maximumStepSize)
+    {
       Log::ProgressBar(startTime, stopTime, time);
+      lastTime = time;
+    }
   }
 
   if (isTopLevelSystem() && Flags::ProgressBar())

src/OMSimulatorLib/SystemSC.h

@@ -71,8 +71,8 @@ namespace oms
     oms_status_enu_t setSolver(oms_solver_enu_t solver) {if (solver > oms_solver_sc_min && solver < oms_solver_sc_max) {solverMethod=solver; return oms_status_ok;} return oms_status_error;}
 
   private:
-    oms_status_enu_t doStepEuler();
-    oms_status_enu_t doStepCVODE();
+    oms_status_enu_t doStepEuler(double stopTime);
+    oms_status_enu_t doStepCVODE(double stopTime);
 
   protected:
     SystemSC(const ComRef& cref, Model* parentModel, System* parentSystem);

testsuite/reference-fmus/Dahlquist.lua

@@ -58,7 +58,7 @@ end
 -- info:    maximum step size for 'model.root': 0.001000
 -- info:    Result file: Dahlquist-me.mat (bufferSize=1)
 -- info:    Final Statistics for 'model.root':
---          NumSteps = 10001 NumRhsEvals  = 10002 NumLinSolvSetups = 501
+--          NumSteps = 10001 NumRhsEvals  = 10002 NumLinSolvSetups = 502
 --          NumNonlinSolvIters = 10001 NumNonlinSolvConvFails = 0 NumErrTestFails = 0
 -- signal x is equal
 -- endResult

testsuite/simulation/DualMassOscillator.lua

@@ -50,9 +50,9 @@ oms_delete("DualMassOscillator")
 -- info:      system1.x1: 0.0
 -- info:      system2.x2: 0.5
 -- info:    Simulation
--- info:      system1.x1: 0.051037644866066
--- info:      system2.x2: 0.031639500249976
+-- info:      system1.x1: 0.051037644963887
+-- info:      system2.x2: 0.031639500306724
 -- info:    Final Statistics for 'DualMassOscillator.root':
---          NumSteps = 10007 NumRhsEvals  = 10008 NumLinSolvSetups = 507
+--          NumSteps = 10007 NumRhsEvals  = 10008 NumLinSolvSetups = 508
 --          NumNonlinSolvIters = 10007 NumNonlinSolvConvFails = 0 NumErrTestFails = 0
 -- endResult

testsuite/simulation/EventTest.lua

@@ -79,16 +79,15 @@ readFile("EventTest_lua.csv")
 -- 0, 0.3, -1
 -- 0.30000001, -1.00000002723e-08, -1
 -- 0.30000001, 0.3, -1
--- 0.60000002, -1.00000154823e-08, -1
+-- 0.60000002, -1.00000004943e-08, -1
 -- 0.60000002, 0.3, -1
--- 0.90000003, -1.00000214776e-08, -1
+-- 0.90000003, -1.00000004943e-08, -1
 -- 0.90000003, 0.3, -1
--- 1, 0.20000003, -1
--- 1.20000004, -1.0000003936e-08, -1
+-- 1.20000004, -1.00000006054e-08, -1
 -- 1.20000004, 0.3, -1
--- 1.50000005, -1.00000143721e-08, -1
+-- 1.50000005, -1.00000290271e-08, -1
 -- 1.50000005, 0.3, -1
--- 1.80000006, -1.00000390191e-08, -1
+-- 1.80000006, -1.0000053674e-08, -1
 -- 1.80000006, 0.3, -1
 -- 2, 0.10000006, -1
 -- 2, 0.10000006, -1

testsuite/simulation/Makefile

@@ -50,6 +50,7 @@ importPartialSnapshot.lua \
 importStartValues.lua \
 importStartValues1.lua \
 importSuppressUnitConversion.lua \
+integratorTest.lua \
 listVariants1.lua \
 multipleConnections.lua \
 multipleInstance.lua \

testsuite/simulation/integratorTest.lua

@@ -0,0 +1,79 @@
+-- status: correct
+-- teardown_command: rm -rf integratorTest_lua/
+-- linux: yes
+-- ucrt64: yes
+-- win: yes
+-- mac: no
+
+function readFile(filename)
+  local f = assert(io.open(filename, "r"))
+  local content = f:read("*all")
+  print(content)
+  f:close()
+end
+
+oms_setCommandLineOption("--suppressPath=true")
+oms_setTempDirectory("./integratorTest_lua/")
+oms_setWorkingDirectory("./integratorTest_lua/")
+
+oms_newModel("IntegratorTest")
+oms_addSystem("IntegratorTest.root", oms_system_sc)
+oms_addSubModel("IntegratorTest.root.integrator", "../../resources/Modelica.Blocks.Continuous.Integrator.fmu")
+
+-- simulation settings
+oms_setSolver("IntegratorTest.root", oms_solver_sc_cvode)
+oms_setResultFile("IntegratorTest", "integratorTest_lua.csv")
+oms_setStopTime("IntegratorTest", 10.0)
+oms_setVariableStepSize("IntegratorTest.root", 1.0, 1e-8, 1.0)
+
+oms_instantiate("IntegratorTest")
+oms_initialize("IntegratorTest")
+
+oms_setReal("IntegratorTest.root.integrator.u", 0.0)
+print(string.format("t = 0.0: y = %g", oms_getReal("IntegratorTest.root.integrator.y")))
+oms_stepUntil("IntegratorTest", 2.5)
+oms_setReal("IntegratorTest.root.integrator.u", 10.0)
+
+print(string.format("t = 2.5: y = %g", oms_getReal("IntegratorTest.root.integrator.y")))
+oms_stepUntil("IntegratorTest", 5.0)
+print(string.format("t = 5.0: y = %g", oms_getReal("IntegratorTest.root.integrator.y")))
+oms_stepUntil("IntegratorTest", 10.0)
+print(string.format("t = 10.0: y = %g", oms_getReal("IntegratorTest.root.integrator.y")))
+
+oms_delete("IntegratorTest")
+
+readFile("integratorTest_lua.csv")
+
+-- Result:
+-- info:    maximum step size for 'IntegratorTest.root': 1.000000
+-- info:    Result file: integratorTest_lua.csv (bufferSize=1)
+-- t = 0.0: y = 0
+-- t = 2.5: y = 0
+-- t = 5.0: y = 25
+-- t = 10.0: y = 75
+-- info:    Final Statistics for 'IntegratorTest.root':
+--          NumSteps = 16 NumRhsEvals  = 26 NumLinSolvSetups = 13
+--          NumNonlinSolvIters = 24 NumNonlinSolvConvFails = 0 NumErrTestFails = 4
+-- time,IntegratorTest.root.integrator._D_outputAlias_y,IntegratorTest.root.integrator.der(_D_outputAlias_y),IntegratorTest.root.integrator.local_set,IntegratorTest.root.integrator.u,IntegratorTest.root.integrator.y,IntegratorTest.root.integrator.initType,IntegratorTest.root.integrator.local_reset,IntegratorTest.root.integrator.use_reset,IntegratorTest.root.integrator.use_set
+-- 0, 0, 0, 0, 0, 0, 3, 0, 0, 0
+-- 1, 0, 0, 0, 0, 0, 3, 0, 0, 0
+-- 2, 0, 0, 0, 0, 0, 3, 0, 0, 0
+-- 2.5, 0, 0, 0, 0, 0, 3, 0, 0, 0
+-- 2.5, 0, 0, 0, 0, 0, 3, 0, 0, 0
+-- 2.5001, 0.001, 10, 0, 10, 0.001, 3, 0, 0, 0
+-- 2.5002, 0.002, 10, 0, 10, 0.002, 3, 0, 0, 0
+-- 2.5012, 0.012, 10, 0, 10, 0.012, 3, 0, 0, 0
+-- 2.5112, 0.112, 10, 0, 10, 0.112, 3, 0, 0, 0
+-- 2.6112, 1.112, 10, 0, 10, 1.112, 3, 0, 0, 0
+-- 3.6112, 11.112, 10, 0, 10, 11.112, 3, 0, 0, 0
+-- 4.6112, 21.112, 10, 0, 10, 21.112, 3, 0, 0, 0
+-- 5, 25, 10, 0, 10, 25, 3, 0, 0, 0
+-- 5, 25, 10, 0, 10, 25, 3, 0, 0, 0
+-- 6, 35, 10, 0, 10, 35, 3, 0, 0, 0
+-- 7, 45, 10, 0, 10, 45, 3, 0, 0, 0
+-- 8, 55, 10, 0, 10, 55, 3, 0, 0, 0
+-- 9, 65, 10, 0, 10, 65, 3, 0, 0, 0
+-- 10, 75, 10, 0, 10, 75, 3, 0, 0, 0
+-- 10, 75, 10, 0, 10, 75, 3, 0, 0, 0
+--
+-- endResult