feat(learned_model): create package (#6395)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Tomas Nagy <tomas@pmc.sk>

Author: atomyks

simulator/learning_based_vehicle_model/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.14.4)
+project(learning_based_vehicle_model)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Python3 COMPONENTS Interpreter Development)
+find_package(pybind11 CONFIG)
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  DIRECTORY src
+)
+target_link_libraries(${PROJECT_NAME} pybind11::embed ${Python3_LIBRARIES})
+target_include_directories(${PROJECT_NAME} PRIVATE ${Python3_INCLUDE_DIRS})
+
+target_compile_options(${PROJECT_NAME} PRIVATE -fvisibility=hidden)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+ament_auto_package()

simulator/learning_based_vehicle_model/README.md

@@ -0,0 +1,208 @@
+# Learned Model
+
+This is the design document for the Python learned model used in the `simple_planning_simulator` package.
+
+## Purpose / Use cases
+
+<!-- Required -->
+<!-- Things to consider:
+    - Why did we implement this feature? -->
+
+This library creates an interface between models in Python and PSIM (C++). It is used to quickly deploy learned Python models in PSIM without a need for complex C++ implementation.
+
+## Design
+
+<!-- Required -->
+<!-- Things to consider:
+    - How does it work? -->
+
+The idea behind this package is that the model we want to use for simulation consists of multiple sub-models (e.g., steering model, drive model, vehicle kinematics, etc.). These sub-models are implemented in Python and can be trainable. Each sub-model has string names for all of its inputs/outputs, which are used to create model interconnections automatically (see image below). This allows us to easily switch sub-models for better customization of the simulator.
+
+![py_model_interface](./image/python_model_interface.png "PyModel interface")
+
+## Assumptions / Known limits
+
+<!-- Required -->
+
+To use this package `python3` and `pybind11` need to be installed. The only assumption on Python sub-models is their interface.
+
+```python
+class PythonSubmodelInterface:
+
+    def forward(self, action, state):  # Required
+        """
+        Calculate forward pass through the model and returns next_state.
+        """
+        return list()
+
+    def get_state_names(self):  # Required
+        """
+        Return list of string names of the model states (outputs).
+        """
+        return list()
+
+    def get_action_names(self):  # Required
+        """
+        Return list of string names of the model actions (inputs).
+        """
+        return list()
+
+    def reset(self):  # Required
+        """
+        Reset model. This function is called after load_params().
+        """
+        pass
+
+    def load_params(self, path):  # Required
+        """
+        Load parameters of the model.
+        Inputs:
+            - path: Path to a parameter file to load by the model.
+        """
+        pass
+
+    def dtSet(self, dt):  # Required
+        """
+        Set dt of the model.
+        Inputs:
+            - dt: time step
+        """
+        pass
+```
+
+## API
+
+<!-- Required -->
+<!-- Things to consider:
+    - How do you use the package / API? -->
+
+To successfully create a vehicle model an InterconnectedModel class needs to be set up correctly.
+
+### InterconnectedModel class
+
+#### `Constructor`
+
+The constructor takes no arguments.
+
+#### `void addSubmodel(std::tuple<std::string, std::string, std::string> model_descriptor)`
+
+Add a new sub-model to the model.
+
+Inputs:
+
+- model_descriptor: Describes what model should be used. The model descriptor contains three strings:
+  - The first string is a path to a python module where the model is implemented.
+  - The second string is a path to the file where model parameters are stored.
+  - The third string is the name of the class that implements the model.
+
+Outputs:
+
+- None
+
+#### `void generateConnections(std::vector<char *> in_names, std::vector<char*> out_names)`
+
+Generate connections between sub-models and inputs/outputs of the model.
+
+Inputs:
+
+- in_names: String names for all of the model inputs in order.
+- out_names: String names for all of the model outputs in order.
+
+Outputs:
+
+- None
+
+#### `void initState(std::vector<double> new_state)`
+
+Set the initial state of the model.
+
+Inputs:
+
+- new_state: New state of the model.
+
+Outputs:
+
+- None
+
+#### `std::vector<double> updatePyModel(std::vector<double> psim_input)`
+
+Calculate the next state of the model by calculating the next state of all of the sub-models.
+
+Inputs:
+
+- psim_input: Input to the model.
+
+Outputs:
+
+- next_state: Next state of the model.
+
+#### `dtSet(double dt)`
+
+Set the time step of the model.
+
+Inputs:
+
+- dt: time step
+
+Outputs:
+
+- None
+
+### Example
+
+Firstly we need to set up the model.
+
+```C++
+InterconnectedModel vehicle;
+
+// Example of model descriptors
+std::tuple<char*, char*, char*> model_descriptor_1 = {
+    (char*)"path_to_python_module_with_model_class_1",
+    (char*)nullptr,  // If no param file is needed you can pass 'nullptr'
+    (char*)"ModelClass1"
+    };
+
+std::tuple<char*, char*, char*> model_descriptor_2 =   {
+    (char*)"path_to_python_module_with_model_class_2",
+    (char*)"/path_to/param_file",
+    (char*)"ModelClass2"  // Name of the python class. Needs to use the interface from 'Assumptions'
+    };
+
+// Create sub-models based on descriptors
+vehicle.addSubmodel(model_descriptor_1);
+vehicle.addSubmodel(model_descriptor_2);
+
+// Define STATE and INPUT names of the system
+std::vector<char*> state_names = {(char*)"STATE_NAME_1", (char*)"STATE_NAME_2"};
+std::vector<char*> input_names = {(char*)"INPUT_NAME_1", (char*)"INPUT_NAME_2"};
+
+// Automatically connect sub-systems with model input
+vehicle.generateConnections(input_names, state_names);
+
+// Set the time step of the model
+vehicle.dtSet(dt);
+```
+
+After the model is correctly set up, we can use it the following way.
+
+```C++
+// Example of an model input
+std::vector<double> vehicle_input = {0.0, 1.0}; // INPUT_NAME_1, INPUT_NAME_2
+
+// Example of an model state
+std::vector<double> current_state = {0.2, 0.5}; // STATE_NAME_1, STATE_NAME_2
+
+// Set model state
+vehicle.initState(current_state);
+
+// Calculate the next state of the model
+std::vector<double> next_state = vehicle.updatePyModel(vehicle_input);
+```
+
+## References / External links
+
+<!-- Optional -->
+
+## Related issues
+
+<!-- Required -->

simulator/learning_based_vehicle_model/image/python_model_interface.png

@@ -0,0 +1,130 @@
+// Copyright 2024 The Autoware Foundation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_
+#define LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_
+
+#include "learning_based_vehicle_model/model_connections_helpers.hpp"
+#include "learning_based_vehicle_model/simple_pymodel.hpp"
+#include "learning_based_vehicle_model/submodel_interface.hpp"
+
+#include <dlfcn.h>
+#include <pybind11/embed.h>
+#include <pybind11/stl.h>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+namespace py = pybind11;
+
+class __attribute__((visibility("default"))) InterconnectedModel
+{
+  // Vector of unique names of inputs and outputs of sub-models
+  std::vector<char *> signals_vec_names;
+  std::vector<double> model_signals_vec;
+  int num_signals;
+
+  std::vector<std::unique_ptr<SubModelInterface>> submodels;
+
+  // index in "map_in_to_sig_vec" is index in "py_inputs" and value in "map_in_to_sig_vec" is index
+  // in "all_variables_names"
+  std::vector<int> map_in_to_sig_vec;
+
+  // index in "map_sig_vec_to_out" is index in "py_model_outputs" and value in "map_sig_vec_to_out"
+  // is index in "all_variables_names"
+  std::vector<int> map_sig_vec_to_out;
+
+public:
+  py::scoped_interpreter guard{};  // start the interpreter and keep it alive
+
+  /**
+   * @brief constructor
+   */
+  InterconnectedModel()
+  {
+    // Initialize python library
+    // cspell:ignore libpython
+    // Manually load libpython3.10.so as we need it for python.h.
+    dlopen("libpython3.10.so", RTLD_GLOBAL | RTLD_NOW);
+    /*
+    More about the line above here:
+      https://stackoverflow.com/questions/60719987/embedding-python-which-uses-numpy-in-c-doesnt-work-in-library-dynamically-loa
+      https://mail.python.org/pipermail/new-bugs-announce/2008-November/003322.html
+      https://stackoverflow.com/questions/67891197/ctypes-cpython-39-x86-64-linux-gnu-so-undefined-symbol-pyfloat-type-in-embedd
+      https://man7.org/linux/man-pages/man3/dlopen.3.html
+    */
+  }
+
+private:
+  /**
+   * @brief create a mapping between vector of signal input names from PSIM to vector of signals
+   * @param [in] in_names vector of signal input names from PSIM
+   */
+  void mapInputs(std::vector<char *> in_names);
+
+  /**
+   * @brief create a mapping between vector of signal output names from PSIM to vector of signals
+   * @param [in] out_names vector of signal output names from PSIM
+   */
+  void mapOutputs(std::vector<char *> out_names);
+
+  /**
+   * @brief add unique names to the vector of signal names
+   * @param [in] names vector of signal names
+   */
+  void addNamesToSigVec(const std::vector<char *> & names);
+
+  /**
+   * @brief create of signal names from all sub-models and PSIM signal names
+   */
+  void getSignalNames(std::vector<char *> in_names, std::vector<char *> out_names);
+
+public:
+  /**
+   * @brief automatically create connections between PSIM and all of the sub-models
+   * @param [in] in_names string names of inputs available from PSIM
+   * @param [in] out_names string names of outputs required by PSIM
+   */
+  void generateConnections(std::vector<char *> in_names, std::vector<char *> out_names);
+
+  /**
+   * @brief add a sub-model consisting of base + error model
+   * @param [in] submodel_desc descriptor of the sub-model
+   */
+  void addSubmodel(std::tuple<std::string, std::string, std::string> submodel_desc);
+
+  /**
+   * @brief set a new model state if it was changed using PSIM interface (mainly position and
+   * orientation)
+   * @param [in] new_state new state set by PSIM
+   */
+  void initState(std::vector<double> new_state);
+
+  /**
+   * @brief set time step for all the models
+   * @param [in] dt time step
+   */
+  void dtSet(double dt);
+
+  /**
+   * @brief compute next step of the PSIM model using python sub-models
+   * @param [in] psim_input vector of input values provided by PSIM
+   */
+  std::vector<double> updatePyModel(std::vector<double> psim_input);
+};
+
+#endif  // LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_

simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp

@@ -0,0 +1,27 @@
+// Copyright 2024 The Autoware Foundation.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_
+#define LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_
+
+#include <cstring>
+#include <vector>
+
+std::vector<double> fillVectorUsingMap(
+  std::vector<double> vector1, std::vector<double> vector2, std::vector<int> map, bool inverse);
+
+std::vector<int> createConnectionsMap(
+  std::vector<char *> connection_names_1, std::vector<char *> connection_names_2);
+
+#endif  // LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_