add support for fmi2GetDirectionalDerivative (#1109)

Author: arun3688

doc/UsersGuide/source/api/getDirectionalDerivative.rst

@@ -0,0 +1,31 @@
+#CAPTION#
+getDirectionalDerivative
+------------------------
+This function computes the directional derivatives of an FMU.
+#END#
+
+#LUA#
+.. code-block:: lua
+
+  value, status = oms_getDirectionalDerivative(cref)
+
+#END#
+
+#PYTHON#
+.. code-block:: python
+
+  value, status = oms.getDirectionalDerivative(cref)
+
+#END#
+
+#CAPI#
+.. code-block:: c
+
+  oms_status_enu_t oms_getDirectionalDerivative(const char* cref, double* value);
+
+#END#
+
+#END#
+
+#DESCRIPTION#
+#END#

src/OMSimulatorLib/Component.h

@@ -83,6 +83,7 @@ namespace oms
     virtual oms_status_enu_t getInteger(const ComRef& cref, int& value) { return logError_NotImplemented; }
     virtual oms_status_enu_t getReal(const ComRef& cref, double& value) { return logError_NotImplemented; }
     virtual oms_status_enu_t getRealOutputDerivative(const ComRef& cref, SignalDerivative& der) { return logError_NotImplemented; }
+    virtual oms_status_enu_t getDirectionalDerivative(const ComRef& cref, double& value) { return logError_NotImplemented; }
     virtual oms_status_enu_t restoreState() { return logError_NotImplemented; }
     virtual oms_status_enu_t saveState() { return logError_NotImplemented; }
     virtual oms_status_enu_t setBoolean(const ComRef& cref, bool value) { return logError_NotImplemented; }

src/OMSimulatorLib/ComponentFMUCS.cpp

@@ -1146,6 +1146,110 @@ oms_status_enu_t oms::ComponentFMUCS::getReal(const ComRef& cref, double& value)
   return getReal(vr, value);
 }
 
+
+oms_status_enu_t oms::ComponentFMUCS::getDirectionalDerivative(const ComRef& cref, double& value)
+{
+  if (!getModel().validState(oms_modelState_instantiated|oms_modelState_initialization|oms_modelState_simulation))
+    return logError_ModelInWrongState(getModel().getCref());
+
+  if (!getFMUInfo()->getProvidesDirectionalDerivative())
+    return logError("It is not possible to compute directionalDerivative for signal \"" + std::string(getFullCref() + cref) + "\" as providesDirectionalDerivative is false or not provieded in modelDescription.xml");
+
+  int j = -1;
+  for (size_t i = 0; i < allVariables.size(); i++)
+  {
+    if (allVariables[i].getCref() == cref && allVariables[i].isTypeReal())
+    {
+      j = i;
+      break;
+    }
+  }
+
+  if (!fmu || j < 0)
+    return logError_UnknownSignal(getFullCref() + cref);
+
+  if (oms_modelState_instantiated == getModel().getModelState())
+  {
+    if (getFMUInfo()->getGenerationTool().substr(0, 12) == "OpenModelica")
+      logWarning("It is not possible to get partial derivatives of OpenModelica generated fmus during initialization mode, getting directional derivative after intialization is possible");
+
+    // check index exist in ModelStructure inititalUnknowns
+    auto index = values.modelStructureInitialUnknowns.find(j+1);
+    if (index == values.modelStructureInitialUnknowns.end())
+      return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an <InitialUnknowns> index in <ModelStructure>");
+
+    //get dependencylist from <InitialUnknowns> in <ModelStructure>
+    std::vector<int> dependencyList = values.modelStructureInitialUnknowns[j+1];
+    getDirectionalDerivativeHeper(j, dependencyList, value);
+  }
+
+  if (oms_modelState_simulation == getModel().getModelState())
+  {
+    if (!allVariables[j].isOutput() && !allVariables[j].isState() && !allVariables[j].isDer())
+      return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an output or state or derivates after initalization");
+
+    // <Outputs>
+    if (allVariables[j].isOutput())
+    {
+      // check index exist in ModelStructure inititalUnknowns
+      auto index = values.modelStructureOutputs.find(j + 1);
+      if (index == values.modelStructureOutputs.end())
+        return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an <Outputs> index in <ModelStructure>");
+
+      // get dependencylist from <Outputs> in <ModelStructure>
+      std::vector<int> dependencyList = values.modelStructureOutputs[j + 1];
+      getDirectionalDerivativeHeper(j, dependencyList, value);
+    }
+
+    // <Derivatives>
+    if (allVariables[j].isState() || allVariables[j].isDer())
+    {
+      // check index exist in ModelStructure inititalUnknowns
+      auto index = values.modelStructureDerivatives.find(j + 1);
+      if (index == values.modelStructureDerivatives.end())
+        return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an <Derivatives> index in <ModelStructure>");
+
+      // get dependencylist from <Derivatives> in <ModelStructure>
+      std::vector<int> dependencyList = values.modelStructureDerivatives[j + 1];
+      getDirectionalDerivativeHeper(j, dependencyList, value);
+    }
+  }
+
+  // fmi2_value_reference_t vr_unknown[1] = {5};
+	// fmi2_value_reference_t vr_known[4] = {0, 2, 3, 4};
+	// fmi2_real_t dvknown[4] = {1.0, 1.0, 1.0, 1.0};
+	// fmi2_real_t val;
+  // std::cout << "Get directional derivative_static_1: " << val << std::endl;
+  // fmi2_import_get_directional_derivative(fmu, vr_unknown, 1, vr_known, 4, dvknown, &val);
+  // std::cout << "\nGet directional derivative_static_2: " << val << std::endl;
+
+  return oms_status_ok;
+}
+
+oms_status_enu_t oms::ComponentFMUCS::getDirectionalDerivativeHeper(const int &index, const std::vector<int> &dependencyList, double &value)
+{
+  fmi2_value_reference_t vr_unknown = allVariables[index].getValueReference();
+
+  fmi2_value_reference_t *vr_known = 0;
+  vr_known = (fmi2_value_reference_t *)calloc(dependencyList.size(), sizeof(fmi2_value_reference_t *));
+
+  fmi2_real_t *dvknown = 0;
+  dvknown = (fmi2_real_t *)calloc(dependencyList.size(), sizeof(fmi2_real_t *));
+
+  for (int i = 0; i < dependencyList.size(); i++)
+  {
+    vr_known[i] = allVariables[dependencyList[i] - 1].getValueReference();
+    dvknown[i] = 1.0;
+  }
+
+  fmi2_import_get_directional_derivative(fmu, &vr_unknown, 1, vr_known, dependencyList.size(), dvknown, &value);
+
+  free(vr_known);
+  free(dvknown);
+
+  return oms_status_ok;
+}
+
 oms_status_enu_t oms::ComponentFMUCS::getRealOutputDerivative(const ComRef& cref, SignalDerivative& der)
 {
   CallClock callClock(clock);

src/OMSimulatorLib/ComponentFMUCS.h

@@ -86,6 +86,9 @@ namespace oms
     oms_status_enu_t setInteger(const ComRef& cref, int value);
     oms_status_enu_t setReal(const ComRef& cref, double value);
 
+    oms_status_enu_t getDirectionalDerivative(const ComRef& cref, double& value);
+    oms_status_enu_t getDirectionalDerivativeHeper(const int& index, const std::vector<int>& dependencyList, double& value);
+
     oms_status_enu_t deleteStartValue(const ComRef& cref);
 
     oms_status_enu_t setFmuTime(double time) {this->time = time; return oms_status_ok;}

src/OMSimulatorLib/ComponentFMUME.cpp

@@ -1141,6 +1141,102 @@ oms_status_enu_t oms::ComponentFMUME::getReal(const ComRef& cref, double& value)
   return getReal(vr, value);
 }
 
+oms_status_enu_t oms::ComponentFMUME::getDirectionalDerivative(const ComRef &cref, double &value)
+{
+  // TODO implement the getDirectionalDerivative table
+  if (!getModel().validState(oms_modelState_instantiated | oms_modelState_initialization | oms_modelState_simulation))
+    return logError_ModelInWrongState(getModel().getCref());
+
+  if (!getFMUInfo()->getProvidesDirectionalDerivative())
+    return logError("It is not possible to compute directionalDerivative for signal \"" + std::string(getFullCref() + cref) + "\" as providesDirectionalDerivative is false or not provieded in modelDescription.xml");
+
+  int j = -1;
+  for (size_t i = 0; i < allVariables.size(); i++)
+  {
+    if (allVariables[i].getCref() == cref && allVariables[i].isTypeReal())
+    {
+      j = i;
+      break;
+    }
+  }
+
+  if (!fmu || j < 0)
+    return logError_UnknownSignal(getFullCref() + cref);
+
+  if (oms_modelState_instantiated == getModel().getModelState())
+  {
+    if (getFMUInfo()->getGenerationTool().substr(0, 12) == "OpenModelica")
+      logWarning("It is not possible to get partial derivatives of OpenModelica generated fmus during initialization mode, getting directional derivative after intialization is possible");
+
+    // check index exist in ModelStructure inititalUnknowns
+    auto index = values.modelStructureInitialUnknowns.find(j + 1);
+    if (index == values.modelStructureInitialUnknowns.end())
+      return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an <InitialUnknowns> index in <ModelStructure>");
+
+    // get dependencylist from <InitialUnknowns> in <ModelStructure>
+    std::vector<int> dependencyList = values.modelStructureInitialUnknowns[j + 1];
+    getDirectionalDerivativeHeper(j, dependencyList, value);
+  }
+
+  if (oms_modelState_simulation == getModel().getModelState())
+  {
+    if (!allVariables[j].isOutput() && !allVariables[j].isState() && !allVariables[j].isDer())
+      return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an output or state or derivates after initalization");
+
+    // <Outputs>
+    if (allVariables[j].isOutput())
+    {
+      // check index exist in ModelStructure inititalUnknowns
+      auto index = values.modelStructureOutputs.find(j + 1);
+      if (index == values.modelStructureOutputs.end())
+        return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an <Outputs> index in <ModelStructure>");
+
+      // get dependencylist from <Outputs> in <ModelStructure>
+      std::vector<int> dependencyList = values.modelStructureOutputs[j + 1];
+      getDirectionalDerivativeHeper(j, dependencyList, value);
+    }
+
+    // <Derivatives>
+    if (allVariables[j].isState() || allVariables[j].isDer())
+    {
+      // check index exist in ModelStructure inititalUnknowns
+      auto index = values.modelStructureDerivatives.find(j + 1);
+      if (index == values.modelStructureDerivatives.end())
+        return logError("Signal \"" + std::string(getFullCref() + cref) + "\" could not be resolved to an <Derivatives> index in <ModelStructure>");
+
+      // get dependencylist from <Derivatives> in <ModelStructure>
+      std::vector<int> dependencyList = values.modelStructureDerivatives[j + 1];
+      getDirectionalDerivativeHeper(j, dependencyList, value);
+    }
+  }
+
+  return oms_status_ok;
+}
+
+oms_status_enu_t oms::ComponentFMUME::getDirectionalDerivativeHeper(const int &index, const std::vector<int> &dependencyList, double &value)
+{
+  fmi2_value_reference_t vr_unknown = allVariables[index].getValueReference();
+
+  fmi2_value_reference_t *vr_known = 0;
+  vr_known = (fmi2_value_reference_t *)calloc(dependencyList.size(), sizeof(fmi2_value_reference_t *));
+
+  fmi2_real_t *dvknown = 0;
+  dvknown = (fmi2_real_t *)calloc(dependencyList.size(), sizeof(fmi2_real_t *));
+
+  for (int i = 0; i < dependencyList.size(); i++)
+  {
+    vr_known[i] = allVariables[dependencyList[i] - 1].getValueReference();
+    dvknown[i] = 1.0;
+  }
+
+  fmi2_import_get_directional_derivative(fmu, &vr_unknown, 1, vr_known, dependencyList.size(), dvknown, &value);
+
+  free(vr_known);
+  free(dvknown);
+
+  return oms_status_ok;
+}
+
 oms_status_enu_t oms::ComponentFMUME::setBoolean(const ComRef& cref, bool value)
 {
   CallClock callClock(clock);

src/OMSimulatorLib/ComponentFMUME.h

@@ -81,6 +81,9 @@ namespace oms
     oms_status_enu_t setInteger(const ComRef& cref, int value);
     oms_status_enu_t setReal(const ComRef& cref, double value);
 
+    oms_status_enu_t getDirectionalDerivative(const ComRef& cref, double& value);
+    oms_status_enu_t getDirectionalDerivativeHeper(const int& index, const std::vector<int>& dependencyList, double& value);
+
     oms_status_enu_t deleteStartValue(const ComRef& cref);
 
     oms_status_enu_t registerSignalsForResultFile(ResultWriter& resultFile);

src/OMSimulatorLib/FMUInfo.h

@@ -56,6 +56,8 @@ namespace oms
     bool getCanInterpolateInputs() const {return canInterpolateInputs;}
     bool getCanGetAndSetFMUstate() const {return canGetAndSetFMUstate;}
     unsigned int getMaxOutputDerivativeOrder() const {return maxOutputDerivativeOrder;}
+    bool getProvidesDirectionalDerivative() const {return providesDirectionalDerivative;}
+    std::string getGenerationTool() const {return std::string(generationTool);}
 
   private:
     // methods to copy the object

src/OMSimulatorLib/OMSimulator.cpp

@@ -1298,6 +1298,23 @@ oms_status_enu_t oms_getReal(const char* cref, double* value)
   return system->getReal(tail, *value);
 }
 
+oms_status_enu_t oms_getDirectionalDerivative(const char* cref, double* value)
+{
+  oms::ComRef tail(cref);
+  oms::ComRef front = tail.pop_front();
+
+  oms::Model* model = oms::Scope::GetInstance().getModel(front);
+  if (!model)
+    return logError_ModelNotInScope(front);
+
+  front = tail.pop_front();
+  oms::System* system = model->getSystem(front);
+  if (!system)
+    return logError_SystemNotInModel(model->getCref(), front);
+
+  return system->getDirectionalDerivative(tail, *value);
+}
+
 oms_status_enu_t oms_getResultFile(const char* cref_, char** filename, int* bufferSize)
 {
   oms::ComRef cref(cref_);

src/OMSimulatorLib/OMSimulator.h

@@ -95,6 +95,7 @@ OMSAPI oms_status_enu_t OMSCALL oms_getBus(const char* cref, oms_busconnector_t*
 OMSAPI oms_status_enu_t OMSCALL oms_getComponentType(const char* cref, oms_component_enu_t* type);
 OMSAPI oms_status_enu_t OMSCALL oms_getConnections(const char* cref, oms_connection_t*** connections);
 OMSAPI oms_status_enu_t OMSCALL oms_getConnector(const char* cref, oms_connector_t** connector);
+OMSAPI oms_status_enu_t OMSCALL oms_getDirectionalDerivative(const char* cref, double* value);
 OMSAPI oms_status_enu_t OMSCALL oms_getElement(const char* cref, oms_element_t** element);
 OMSAPI oms_status_enu_t OMSCALL oms_getElements(const char* cref, oms_element_t*** elements);
 OMSAPI oms_status_enu_t OMSCALL oms_getExternalModelInfo(const char* cref, const oms_external_tlm_model_info_t** externalModelInfo);

src/OMSimulatorLib/System.cpp

@@ -2134,6 +2134,24 @@ oms_status_enu_t oms::System::getReal(const ComRef& cref, double& value)
   return logError_UnknownSignal(getFullCref() + cref);
 }
 
+oms_status_enu_t oms::System::getDirectionalDerivative(const ComRef& cref, double& value)
+{
+  if (!getModel().validState(oms_modelState_virgin|oms_modelState_instantiated|oms_modelState_initialization|oms_modelState_simulation))
+    return logError_ModelInWrongState(getModel().getCref());
+
+  oms::ComRef tail(cref);
+  oms::ComRef head = tail.pop_front();
+
+  auto subsystem = subsystems.find(head);
+  if (subsystem != subsystems.end())
+    return logError("getDirectionalDerivative is computed only for fmu signals");
+
+  auto component = components.find(head);
+  if (component != components.end())
+    return component->second->getDirectionalDerivative(tail, value);
+
+  return logError_UnknownSignal(getFullCref() + cref);
+}
 
 oms_status_enu_t oms::System::setBoolean(const ComRef& cref, bool value)
 {