diff --git a/src/FmuInstance.cpp b/src/FmuInstance.cpp index ebb0b28..aa4f12b 100644 --- a/src/FmuInstance.cpp +++ b/src/FmuInstance.cpp @@ -551,9 +551,56 @@ FmuWrapper::FmuWrapper(const std::string &fmu_path, std::to_string(v)); } - // Instantiate FMI3 CoSimulation component - _instance = - fmi3_instantiateModelExchange(_fmu, false, false, nullptr, nullptr); + _model_description_xml = read_model_description_xml(_fmu_path); + bool cs = _model_description_xml.find("", cs_pos); + string cs_tag = _model_description_xml.substr(cs_pos, cs_end - cs_pos + 1); + + string var_step; + if(extract_xml_attribute(cs_tag, "canHandleVariableCommunicationStepSize", var_step)){ + _fixed_step = !(var_step == "true" || var_step == "1"); + + std::string step_string; + if (extract_xml_attribute(cs_tag, "fixedInternalStepSize", step_string)) { + _step_size = std::stod(step_string); + } + + } else{ + _fixed_step = false; + } + } + + _instance = fmi3_instantiateCoSimulation( + _fmu, + fmi3False, // visible + fmi3False, // loggingOn + fmi3False, // eventModeUsed + fmi3False, // earlyReturnAllowed + nullptr, // requiredIntermediateVariables + 0, // nRequiredIntermediateVariables + nullptr, // instanceEnvironment + nullptr, // logMessage + nullptr // intermediateUpdate + ); + + } else if(me){ + + _type = FmuType::ModelExchange; + _instance = fmi3_instantiateModelExchange(_fmu, false, false, nullptr, nullptr); + + } else{ + + throw std::runtime_error("FMU Type: Unknown"); + } if (!_instance) { fmi4c_freeFmu(_fmu); @@ -658,166 +705,203 @@ void FmuWrapper::reset() { } void FmuWrapper::do_step(double dt) { + if (!_instance || !_initialized) { throw std::runtime_error("FMU instance not initialized"); } - // Get the number of continuous states - size_t numStates = 0; - fmi3Status status = fmi3_getNumberOfContinuousStates(_instance, &numStates); - check_status(status, "getNumberOfContinuousStates"); - - if (numStates == 0) { - // No states to integrate, just advance time and handle events - _current_time += dt; - handle_events(); - return; - } + if (_type == FmuType::CoSimulation) { // CO-SIMULATION - // Adaptive RK45 integration parameters - const double &relTol = _solver_params._rel_tol; - const double &absTol = _solver_params._abs_tol; - const double &hmin = _solver_params._hmin; + fmi3Boolean eventHandlingNeeded = fmi3False; + fmi3Boolean terminateSimulation = fmi3False; + fmi3Boolean earlyReturn = fmi3False; + fmi3Float64 lastSuccessfulTime = _current_time; - double t = _current_time; - double tend = _current_time + dt; - double h = dt; // Initial step size + // exported solver within FMU + fmi3Status status = fmi3_doStep(_instance, + _current_time, + dt, + fmi3True, // noSetFMUStatePriorToCurrentPoint + &eventHandlingNeeded, + &terminateSimulation, + &earlyReturn, + &lastSuccessfulTime); + + check_status(status, "doStep"); - std::vector y(numStates); - std::vector ytemp(numStates); - std::vector k1(numStates), k2(numStates), k3(numStates); - std::vector k4(numStates), k5(numStates), k6(numStates); - std::vector yerr(numStates); + if (terminateSimulation) { + throw std::runtime_error("FMU requested termination during doStep"); + } - // Get initial state - status = fmi3_getContinuousStates(_instance, y.data(), numStates); - check_status(status, "getContinuousStates"); + _current_time += dt; - // Adaptive stepping loop - while (t < tend && h > hmin) { - // Limit step to not overshoot tend - if (t + h > tend) { - h = tend - t; + if (eventHandlingNeeded) { + fmi3_enterEventMode(_instance); + handle_events(); + fmi3_enterStepMode(_instance); } - // RK45 with Dormand-Prince coefficients - // Stage 1: k1 = f(t, y) - status = - fmi3_getContinuousStateDerivatives(_instance, k1.data(), numStates); - check_status(status, "getContinuousStateDerivatives at stage 1"); + } else if(_type == FmuType::ModelExchange){ // MODEL EXCHANGE - // Stage 2: k2 = f(t + (1/5)*h, y + (1/5)*h*k1) - for (size_t i = 0; i < numStates; ++i) { - ytemp[i] = y[i] + (h / 5.0) * k1[i]; - } - status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); - check_status(status, "setContinuousStates at stage 2"); - status = - fmi3_getContinuousStateDerivatives(_instance, k2.data(), numStates); - check_status(status, "getContinuousStateDerivatives at stage 2"); - - // Stage 3: k3 = f(t + (3/10)*h, y + (3/40)*h*k1 + (9/40)*h*k2) - for (size_t i = 0; i < numStates; ++i) { - ytemp[i] = y[i] + (3.0 / 40.0) * h * k1[i] + (9.0 / 40.0) * h * k2[i]; - } - status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); - check_status(status, "setContinuousStates at stage 3"); - status = - fmi3_getContinuousStateDerivatives(_instance, k3.data(), numStates); - check_status(status, "getContinuousStateDerivatives at stage 3"); - - // Stage 4: k4 = f(t + (4/5)*h, y + (44/45)*h*k1 - (56/15)*h*k2 + - // (32/9)*h*k3) - for (size_t i = 0; i < numStates; ++i) { - ytemp[i] = y[i] + (44.0 / 45.0) * h * k1[i] - (56.0 / 15.0) * h * k2[i] + - (32.0 / 9.0) * h * k3[i]; - } - status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); - check_status(status, "setContinuousStates at stage 4"); - status = - fmi3_getContinuousStateDerivatives(_instance, k4.data(), numStates); - check_status(status, "getContinuousStateDerivatives at stage 4"); - - // Stage 5: k5 = f(t + (8/9)*h, y + ...) - for (size_t i = 0; i < numStates; ++i) { - ytemp[i] = y[i] + (19372.0 / 6561.0) * h * k1[i] - - (25360.0 / 2187.0) * h * k2[i] + - (64448.0 / 6561.0) * h * k3[i] - (212.0 / 729.0) * h * k4[i]; - } - status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); - check_status(status, "setContinuousStates at stage 5"); - status = - fmi3_getContinuousStateDerivatives(_instance, k5.data(), numStates); - check_status(status, "getContinuousStateDerivatives at stage 5"); - - // Stage 6: k6 = f(t + h, y + ...) - for (size_t i = 0; i < numStates; ++i) { - ytemp[i] = y[i] + (9017.0 / 3168.0) * h * k1[i] - - (355.0 / 33.0) * h * k2[i] + (46732.0 / 5247.0) * h * k3[i] + - (49.0 / 176.0) * h * k4[i] - (5103.0 / 18656.0) * h * k5[i]; - } - status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); - check_status(status, "setContinuousStates at stage 6"); - status = - fmi3_getContinuousStateDerivatives(_instance, k6.data(), numStates); - check_status(status, "getContinuousStateDerivatives at stage 6"); - - // 5th order solution: y_new = y + h * (35/384*k1 + 500/1113*k3 + 125/192*k4 - // - 2187/6784*k5 + 11/84*k6) 4th order solution for error: y_hat = y + h * - // (5179/57600*k1 + 7571/16695*k3 + 393/640*k4 - 92097/339200*k5 + - // 187/2100*k6) - std::vector ynew(numStates); - for (size_t i = 0; i < numStates; ++i) { - ynew[i] = y[i] + h * (35.0 / 384.0 * k1[i] + 500.0 / 1113.0 * k3[i] + - 125.0 / 192.0 * k4[i] - 2187.0 / 6784.0 * k5[i] + - 11.0 / 84.0 * k6[i]); - - // Error estimate (difference between 5th and 4th order) - double y4th = - y[i] + h * (5179.0 / 57600.0 * k1[i] + 7571.0 / 16695.0 * k3[i] + - 393.0 / 640.0 * k4[i] - 92097.0 / 339200.0 * k5[i] + - 187.0 / 2100.0 * k6[i]); - yerr[i] = std::abs(ynew[i] - y4th); - } + // Get the number of continuous states + size_t numStates = 0; + fmi3Status status = fmi3_getNumberOfContinuousStates(_instance, &numStates); + check_status(status, "getNumberOfContinuousStates"); - // Compute maximum relative error - double maxError = 0.0; - for (size_t i = 0; i < numStates; ++i) { - double scale = absTol + relTol * std::abs(y[i]); - double error = yerr[i] / scale; - maxError = std::max(maxError, error); + if (numStates == 0) { + // No states to integrate, just advance time and handle events + _current_time += dt; + handle_events(); + return; } - // Adaptive step size control - if (maxError <= 1.0) { - // Step accepted: update y and t - y = ynew; - t += h; - _current_time = t; + // Adaptive RK45 integration parameters + const double &relTol = _solver_params._rel_tol; + const double &absTol = _solver_params._abs_tol; + const double &hmin = _solver_params._hmin; + + double t = _current_time; + double tend = _current_time + dt; + double h = dt; // Initial step size + + std::vector y(numStates); + std::vector ytemp(numStates); + std::vector k1(numStates), k2(numStates), k3(numStates); + std::vector k4(numStates), k5(numStates), k6(numStates); + std::vector yerr(numStates); + + // Get initial state + status = fmi3_getContinuousStates(_instance, y.data(), numStates); + check_status(status, "getContinuousStates"); + + // Adaptive stepping loop + while (t < tend && h > hmin) { + // Limit step to not overshoot tend + if (t + h > tend) { + h = tend - t; + } - // Set accepted state into FMU - status = fmi3_setContinuousStates(_instance, y.data(), numStates); - check_status(status, "setContinuousStates (accepted step)"); + // RK45 with Dormand-Prince coefficients + // Stage 1: k1 = f(t, y) + status = + fmi3_getContinuousStateDerivatives(_instance, k1.data(), numStates); + check_status(status, "getContinuousStateDerivatives at stage 1"); - // Increase step size for next iteration (but not too aggressively) - h *= 0.9 * std::pow(1.0 / maxError, 0.2); - h = std::min(h, 10.0 * (tend - t)); // Don't let h grow too much - } else { - // Step rejected: reduce step size - h *= 0.9 * std::pow(1.0 / maxError, 0.25); - } + // Stage 2: k2 = f(t + (1/5)*h, y + (1/5)*h*k1) + for (size_t i = 0; i < numStates; ++i) { + ytemp[i] = y[i] + (h / 5.0) * k1[i]; + } + status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); + check_status(status, "setContinuousStates at stage 2"); + status = + fmi3_getContinuousStateDerivatives(_instance, k2.data(), numStates); + check_status(status, "getContinuousStateDerivatives at stage 2"); + + // Stage 3: k3 = f(t + (3/10)*h, y + (3/40)*h*k1 + (9/40)*h*k2) + for (size_t i = 0; i < numStates; ++i) { + ytemp[i] = y[i] + (3.0 / 40.0) * h * k1[i] + (9.0 / 40.0) * h * k2[i]; + } + status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); + check_status(status, "setContinuousStates at stage 3"); + status = + fmi3_getContinuousStateDerivatives(_instance, k3.data(), numStates); + check_status(status, "getContinuousStateDerivatives at stage 3"); + + // Stage 4: k4 = f(t + (4/5)*h, y + (44/45)*h*k1 - (56/15)*h*k2 + + // (32/9)*h*k3) + for (size_t i = 0; i < numStates; ++i) { + ytemp[i] = y[i] + (44.0 / 45.0) * h * k1[i] - (56.0 / 15.0) * h * k2[i] + + (32.0 / 9.0) * h * k3[i]; + } + status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); + check_status(status, "setContinuousStates at stage 4"); + status = + fmi3_getContinuousStateDerivatives(_instance, k4.data(), numStates); + check_status(status, "getContinuousStateDerivatives at stage 4"); + + // Stage 5: k5 = f(t + (8/9)*h, y + ...) + for (size_t i = 0; i < numStates; ++i) { + ytemp[i] = y[i] + (19372.0 / 6561.0) * h * k1[i] - + (25360.0 / 2187.0) * h * k2[i] + + (64448.0 / 6561.0) * h * k3[i] - (212.0 / 729.0) * h * k4[i]; + } + status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); + check_status(status, "setContinuousStates at stage 5"); + status = + fmi3_getContinuousStateDerivatives(_instance, k5.data(), numStates); + check_status(status, "getContinuousStateDerivatives at stage 5"); + + // Stage 6: k6 = f(t + h, y + ...) + for (size_t i = 0; i < numStates; ++i) { + ytemp[i] = y[i] + (9017.0 / 3168.0) * h * k1[i] - + (355.0 / 33.0) * h * k2[i] + (46732.0 / 5247.0) * h * k3[i] + + (49.0 / 176.0) * h * k4[i] - (5103.0 / 18656.0) * h * k5[i]; + } + status = fmi3_setContinuousStates(_instance, ytemp.data(), numStates); + check_status(status, "setContinuousStates at stage 6"); + status = + fmi3_getContinuousStateDerivatives(_instance, k6.data(), numStates); + check_status(status, "getContinuousStateDerivatives at stage 6"); + + // 5th order solution: y_new = y + h * (35/384*k1 + 500/1113*k3 + 125/192*k4 + // - 2187/6784*k5 + 11/84*k6) 4th order solution for error: y_hat = y + h * + // (5179/57600*k1 + 7571/16695*k3 + 393/640*k4 - 92097/339200*k5 + + // 187/2100*k6) + std::vector ynew(numStates); + for (size_t i = 0; i < numStates; ++i) { + ynew[i] = y[i] + h * (35.0 / 384.0 * k1[i] + 500.0 / 1113.0 * k3[i] + + 125.0 / 192.0 * k4[i] - 2187.0 / 6784.0 * k5[i] + + 11.0 / 84.0 * k6[i]); + + // Error estimate (difference between 5th and 4th order) + double y4th = + y[i] + h * (5179.0 / 57600.0 * k1[i] + 7571.0 / 16695.0 * k3[i] + + 393.0 / 640.0 * k4[i] - 92097.0 / 339200.0 * k5[i] + + 187.0 / 2100.0 * k6[i]); + yerr[i] = std::abs(ynew[i] - y4th); + } + + // Compute maximum relative error + double maxError = 0.0; + for (size_t i = 0; i < numStates; ++i) { + double scale = absTol + relTol * std::abs(y[i]); + double error = yerr[i] / scale; + maxError = std::max(maxError, error); + } + + // Adaptive step size control + if (maxError <= 1.0) { + // Step accepted: update y and t + y = ynew; + t += h; + _current_time = t; + + // Set accepted state into FMU + status = fmi3_setContinuousStates(_instance, y.data(), numStates); + check_status(status, "setContinuousStates (accepted step)"); + + // Increase step size for next iteration (but not too aggressively) + h *= 0.9 * std::pow(1.0 / maxError, 0.2); + h = std::min(h, 10.0 * (tend - t)); // Don't let h grow too much + } else { + // Step rejected: reduce step size + h *= 0.9 * std::pow(1.0 / maxError, 0.25); + } - // Enforce minimum step size to prevent infinite loops - if (h < hmin) { - h = hmin; + // Enforce minimum step size to prevent infinite loops + if (h < hmin) { + h = hmin; + } } - } - // Ensure we're exactly at tend - _current_time = tend; + // Ensure we're exactly at tend + _current_time = tend; + + // Handle events at the end of the step + handle_events(); + } - // Handle events at the end of the step - handle_events(); + } void FmuWrapper::handle_events() { diff --git a/src/FmuInstance.hpp b/src/FmuInstance.hpp index b9dac13..9f8ed92 100644 --- a/src/FmuInstance.hpp +++ b/src/FmuInstance.hpp @@ -21,6 +21,12 @@ extern "C" { using json = nlohmann::json; +enum class FmuType{ + Unknown, + ModelExchange, + CoSimulation +}; + class FmuWrapper { public: FmuWrapper() {} @@ -90,6 +96,10 @@ class FmuWrapper { std::vector get_indep_names() const; std::vector get_binary_dependencies() const; + int get_type() const { return static_cast(_type); } + bool get_fixed_step() const { return _fixed_step; } + double get_step_size() const { return _step_size; } + struct SolverParams { double _rel_tol = 1e-6; double _abs_tol = 1e-8; @@ -119,6 +129,11 @@ class FmuWrapper { static const std::unordered_map _causality_map; static const std::unordered_map _data_type_map; + // FMU type variable + FmuType _type = FmuType::Unknown; + bool _fixed_step = false; + double _step_size = 0.0; + /// Resolve variable name to value reference fmi3ValueReference resolve_var_ref(const std::string& name) const; size_t resolve_real_array_length(const std::string& name) const; diff --git a/src/main/fmu_agent.cpp b/src/main/fmu_agent.cpp index c81df7d..a247f39 100644 --- a/src/main/fmu_agent.cpp +++ b/src/main/fmu_agent.cpp @@ -179,6 +179,7 @@ int main(int argc, char *const *argv) { } cout << " FMU file path: " << style::bold << fmu_path << style::reset << endl + << " FMU type: " << style::bold << ((plant.get_type() == 1) ? "Model Exchange" : "Co-Simulation") << style::reset << endl << " relative tol: " << style::bold << relative_tol << style::reset << endl << " absolutre tol: " << style::bold << absolute_tol << style::reset @@ -214,6 +215,8 @@ int main(int argc, char *const *argv) { auto last_timestep = chrono::steady_clock::now(); chrono::steady_clock::time_point now; double dt = 0, t = 0, t_in = 0, t_msg = 0; + int dtus = 0, step_s = 0; + static double t_buffer = 0.0; json status; array console_out; @@ -251,9 +254,32 @@ int main(int argc, char *const *argv) { agent.loop([&]() -> chrono::milliseconds { if (agent.receive(false) == message_type::json && agent.last_topic() != "control") { now = chrono::steady_clock::now(); + dt = chrono::duration_cast(now - last_timestep) - .count() / 1e6; + .count(); last_timestep = now; + + if(plant.get_fixed_step()){ + + t_buffer += dt; + step_s = plant.get_step_size() * 1e6; // us + + int num_steps = t_buffer / step_s; + if(num_steps > 0){ + dt = num_steps * step_s; // force dt + t_buffer -= dt; // t_buffer will contain the residual + } else{ + console_out[2] = "Skipped step: " + to_string(t_buffer) + "s"; + cout << goback(3) << fg::yellow << "Last message: " << console_out[0] + << fg::reset << endl + << "Received: " << console_out[1] << endl + << "Status update after: " << console_out[2] << endl + << "buffer: " << t_buffer << " dt: " << dt << endl; + return 0ms; + } + } + + dt = dt / 1e6; t += dt; console_out[2] = to_string(t) + " s"; plant.do_step(dt); @@ -266,19 +292,34 @@ int main(int argc, char *const *argv) { auto in = json::parse(get<1>(msg)); process_input(in); } - cout << goback(3) << fg::yellow << "Last message: " << console_out[0] - << fg::reset << endl - << "Received: " << console_out[1] << endl - << "Status update after: " << console_out[2] << endl; + + cout << goback(4) << fg::yellow << "Last message: " << console_out[0] + << fg::reset << endl + << "Received: " << console_out[1] << endl + << "Status update after: " << console_out[2] << endl + << "Simulation dt: " << dt << "s" << endl; + return 0ms; }); } else { agent.loop([&]() -> chrono::milliseconds { // timing now = chrono::steady_clock::now(); - dt = chrono::duration_cast(now - last_timestep) - .count() / 1e6; - last_timestep = now; + if(plant.get_fixed_step()){ + dt = static_cast(period.count()) / 1e3; + dtus = dt * 1e6; + step_s = plant.get_step_size() * 1e6; // us + if(dtus % step_s != 0){ // comparison in microseconds + + throw std::runtime_error("The agent period must be a multiple of the model's fixed step"); + } + + } else{ + dt = chrono::duration_cast(now - last_timestep) + .count() / 1e6; + + last_timestep = now; + } t += dt; // input if (agent.receive(true) == message_type::json &&