Run fmi2SetXXX function to initialize FMU start values (#778)

* Swapped the order of setup and do_iteration in initialize function

* Added initialize start values function

* Cleaned up code

* Added a filtered list for modify_and_get.

* Error fix

* Filter function update

* Filter Update

* Typo fix

* review follow-up

* added FMU to README

* Fixed dangling reference.

* Revert to "Typo fix"

This reverts commit 790ba4c.

* Removed unused includes

* Ref for scalar_value (string)

* return string from the textual representations, since nullptr "short-circuits" the error message.

---------

Co-authored-by: Roger Eivind Stenbro <[email protected]>

Author: davidhjp01

include/cosim/algorithm/simulator.hpp

@@ -60,7 +60,8 @@ class simulator : public manipulable
     virtual void set_string(value_reference reference, std::string_view value) = 0;
 
     /**
-     *  Performs pre-simulation setup and enters initialisation mode.
+     *  Performs pre-simulation setup, instantiates the FMUs and enters 
+     *  initialisation mode. 
      *
      *  This function must be called exactly once, before initialisation and
      *  simulation can begin (i.e. before the first time either of

include/cosim/model_description.hpp

@@ -69,7 +69,7 @@ constexpr const char* to_text(variable_type v)
         case variable_type::boolean: return "boolean";
         case variable_type::string: return "string";
         case variable_type::enumeration: return "enumeration";
-        default: return nullptr;
+        default: return "NULL";
     }
 }
 
@@ -83,7 +83,7 @@ constexpr const char* to_text(variable_causality v)
         case variable_causality::input: return "input";
         case variable_causality::output: return "output";
         case variable_causality::local: return "local";
-        default: return nullptr;
+        default: return "NULL";
     }
 }
 
@@ -97,7 +97,7 @@ constexpr const char* to_text(variable_variability v)
         case variable_variability::tunable: return "tunable";
         case variable_variability::discrete: return "discrete";
         case variable_variability::continuous: return "continuous";
-        default: return nullptr;
+        default: return "NULL";
     }
 }
 

src/cosim/algorithm/fixed_step_algorithm.cpp

@@ -171,6 +171,7 @@ class fixed_step_algorithm::impl
             calculate_and_transfer();
         }
 
+
         for (auto& s : simulators_) {
             pool_.submit([&] {
                 s.second.sim->start_simulation();

src/cosim/slave_simulator.cpp

@@ -6,10 +6,10 @@
 #include "cosim/slave_simulator.hpp"
 
 #include "cosim/error.hpp"
+#include <cosim/utility/utility.hpp>
 
 #include <algorithm>
 #include <cassert>
-#include <sstream>
 #include <stdexcept>
 #include <unordered_map>
 
@@ -216,7 +216,9 @@ class set_variable_cache
         }
     }
 
-    std::pair<gsl::span<value_reference>, gsl::span<const T>> modify_and_get(duration deltaT)
+    std::pair<gsl::span<value_reference>, gsl::span<const T>> modify_and_get(
+        duration deltaT,
+        std::optional<std::function<bool(const value_reference, const T&)>> filter = std::nullopt)
     {
         if (!hasRunModifiers_) {
             for (const auto& entry : modifiers_) {
@@ -233,6 +235,24 @@ class set_variable_cache
             assert(references_.size() == values_.size());
             hasRunModifiers_ = true;
         }
+
+        if (filter) {
+            references_filtered_.clear();
+            values_filtered_.clear();
+
+            for (size_t i = 0; i < references_.size(); i++) {
+                auto& ref = references_.at(i);
+                auto& value = values_.at(i);
+
+                if ((*filter)(ref, value)) {
+                    references_filtered_.push_back(ref);
+                    values_filtered_.push_back(value);
+                }
+            }
+
+            return std::pair(gsl::make_span(references_filtered_), gsl::make_span(values_filtered_));
+        }
+
         return std::pair(gsl::make_span(references_), gsl::make_span(values_));
     }
 
@@ -243,6 +263,8 @@ class set_variable_cache
         }
         references_.clear();
         values_.clear();
+        references_filtered_.clear();
+        values_filtered_.clear();
         hasRunModifiers_ = false;
     }
 
@@ -268,6 +290,10 @@ class set_variable_cache
     // The references and values of the variables that will be set next.
     std::vector<value_reference> references_;
     boost::container::vector<T> values_;
+
+    // Filtered references and values of the values to be set next (if a filter is applied).
+    std::vector<value_reference> references_filtered_;
+    boost::container::vector<T> values_filtered_;
 };
 
 
@@ -490,6 +516,30 @@ class slave_simulator::impl
         std::optional<time_point> stopTime,
         std::optional<double> relativeTolerance)
     {
+        auto deltaT = duration::zero();
+        auto filter = [this](const variable_type vt) {
+            return [this, vt](const value_reference vr, const cosim::scalar_value&) {
+                const auto& vd = this->find_variable_description(vr, vt);
+                return vd.variability != variable_variability::constant &&
+                    vd.causality != variable_causality::input;
+            };
+        };
+
+        const auto [realRefs, realValues] = realSetCache_.modify_and_get(deltaT, filter(variable_type::real));
+        const auto [integerRefs, integerValues] = integerSetCache_.modify_and_get(deltaT, filter(variable_type::integer));
+        const auto [booleanRefs, booleanValues] = booleanSetCache_.modify_and_get(deltaT, filter(variable_type::boolean));
+        const auto [stringRefs, stringValues] = stringSetCache_.modify_and_get(deltaT, filter(variable_type::string));
+
+        slave_->set_variables(
+            gsl::make_span(realRefs),
+            gsl::make_span(realValues),
+            gsl::make_span(integerRefs),
+            gsl::make_span(integerValues),
+            gsl::make_span(booleanRefs),
+            gsl::make_span(booleanValues),
+            gsl::make_span(stringRefs),
+            gsl::make_span(stringValues));
+
         slave_->setup(startTime, stopTime, relativeTolerance);
         get_variables(duration::zero());
     }
@@ -802,5 +852,4 @@ step_result slave_simulator::do_step(
     return pimpl_->do_step(currentT, deltaT);
 }
 
-
 } // namespace cosim

tests/CMakeLists.txt

@@ -13,6 +13,7 @@ set(tests
     "scenario_manager_test"
     "synchronized_xy_series_test"
     "config_end_time_test"
+    "state_init_test"
 )
 
 set(unittests

tests/data/fmi2/README.md

@@ -5,6 +5,7 @@
 | `CraneController.fmu`  | OSP            | [MPL 2.0](../../../LICENSE)   |
 | `KnuckleBoomCrane.fmu` | OSP            | [MPL 2.0](../../../LICENSE)   |
 | `vector.fmu`           | [OSP cpp-fmus] | [MIT](./osp_cpp-fmus_LICENSE) |
+| `StateInitExample.fmu` | OSP            | [MPL 2.0](../../../LICENSE)   |
 
 
 [OSP cpp-fmus]: https://github.com/open-simulation-platform/cpp-fmus

tests/data/fmi2/StateInitExample.fmu

@@ -0,0 +1,17 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<OspSystemStructure xmlns="http://opensimulationplatform.com/MSMI/OSPSystemStructure" version="0.1">
+    <StartTime>0.0</StartTime>
+    <BaseStepSize>0.01</BaseStepSize>
+    <Algorithm>fixedStep</Algorithm>
+    <Simulators>
+        <Simulator name="example" source="../fmi2/StateInitExample.fmu">
+            <InitialValues>
+                <InitialValue variable="Parameters.Integrator1_x0">
+                    <Real value="10.0" />
+                </InitialValue>
+            </InitialValues>
+        </Simulator>
+    </Simulators>
+    <Connections>
+    </Connections>
+</OspSystemStructure>

tests/data/msmi/OspSystemStructure_StateInitExample.xml

@@ -0,0 +1,53 @@
+#include <cosim/algorithm/fixed_step_algorithm.hpp>
+#include <cosim/osp_config_parser.hpp>
+#include <cosim/observer/file_observer.hpp>
+#include <cosim/exception.hpp>
+#include <cosim/execution.hpp>
+#include <cosim/function/linear_transformation.hpp>
+#include <cosim/observer/last_value_observer.hpp>
+#include <cosim/system_structure.hpp>
+#include <algorithm>
+#include <iostream>
+
+#define REQUIRE(test) \
+    if (!(test)) throw std::runtime_error("Requirement not satisfied: " #test)
+
+int main()
+{
+    try {
+        const auto testDataDir = std::getenv("TEST_DATA_DIR");
+        REQUIRE(!!testDataDir);
+
+        cosim::filesystem::path configPath = testDataDir;
+
+        auto resolver = cosim::default_model_uri_resolver();
+        const auto config = cosim::load_osp_config(configPath / "msmi" / "OspSystemStructure_StateInitExample.xml", *resolver);
+
+        auto execution = cosim::execution(
+            config.start_time,
+            std::make_shared<cosim::fixed_step_algorithm>(config.step_size));
+
+        const auto entityMaps = cosim::inject_system_structure(
+            execution, config.system_structure, config.initial_values);
+        auto lvObserver = std::make_shared<cosim::last_value_observer>();
+
+        execution.add_observer(lvObserver);
+        execution.simulate_until(cosim::to_time_point(0.1));
+
+        auto sim = entityMaps.simulators.at("example"); 
+        const auto paramRef = config.system_structure.get_variable_description({"example", "Parameters.Integrator1_x0"}).reference;
+        const auto outRef = config.system_structure.get_variable_description({"example", "Integrator_out1"}).reference;
+
+        double initialValue = 0.0;
+        double outputValue = 0.0;
+        lvObserver->get_real(sim, gsl::make_span(&paramRef, 1), gsl::make_span(&initialValue, 1));
+        lvObserver->get_real(sim, gsl::make_span(&outRef, 1), gsl::make_span(&outputValue, 1));
+
+        REQUIRE(std::fabs(initialValue - 10.0) < 1.0e-9);
+        REQUIRE(std::fabs(outputValue - 10.1) < 1.0e-9);
+
+    } catch (const std::exception& e) {
+        std::cerr << "Error: " << e.what() << std::endl;
+        return 1;
+    }
+}