Merge pull request #194 from MaximilienNaveau/devel

add a sot_loader with the minimal functionalities.

CMakeLists.txt

@@ -36,11 +36,12 @@ CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
 
 # Project dependencies
 ADD_PROJECT_DEPENDENCY(dynamic-graph REQUIRED)
+ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED)
 ADD_PROJECT_DEPENDENCY(pinocchio REQUIRED)
-ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS regex)
+ADD_PROJECT_DEPENDENCY(Boost REQUIRED COMPONENTS regex program_options)
 IF(BUILD_TESTING)
   ADD_PROJECT_DEPENDENCY(example-robot-data 3.8.0)
-  FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework program_options)
+  FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
 ENDIF()
 
 IF(Boost_VERSION GREATER 107299 OR Boost_VERSION_MACRO GREATER 107299)
@@ -52,7 +53,6 @@ ENDIF()
 IF(BUILD_PYTHON_INTERFACE)
   FINDPYTHON()
   STRING(REGEX REPLACE "-" "_" PYTHON_DIR ${CUSTOM_HEADER_DIR})
-  ADD_PROJECT_DEPENDENCY(dynamic-graph-python 4.0.0 REQUIRED)
   ADD_PROJECT_DEPENDENCY(eigenpy REQUIRED)
   SEARCH_FOR_BOOST_PYTHON(REQUIRED)
 ENDIF(BUILD_PYTHON_INTERFACE)
@@ -131,6 +131,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/${CUSTOM_HEADER_DIR}/reader.hh
   include/${CUSTOM_HEADER_DIR}/robot-simu.hh
   include/${CUSTOM_HEADER_DIR}/robot-utils.hh
+  include/${CUSTOM_HEADER_DIR}/sot-loader.hh
   include/${CUSTOM_HEADER_DIR}/sot.hh
   include/${CUSTOM_HEADER_DIR}/stop-watch.hh
   include/${CUSTOM_HEADER_DIR}/switch.hh
@@ -170,6 +171,7 @@ SET(${PROJECT_NAME}_SOURCES
   src/tools/utils-windows.cpp
   src/tools/periodic-call.cpp
   src/tools/device.cpp
+  src/tools/sot-loader.cpp
   src/tools/trajectory.cpp
   src/tools/robot-utils.cpp
   src/matrix/matrix-svd.cpp
@@ -180,8 +182,8 @@ SET(${PROJECT_NAME}_SOURCES
 ADD_LIBRARY(${PROJECT_NAME} SHARED
   ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
 TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC Boost::regex
-  dynamic-graph::dynamic-graph pinocchio::pinocchio)
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC Boost::regex Boost::program_options
+  dynamic-graph::dynamic-graph dynamic-graph-python::dynamic-graph-python pinocchio::pinocchio)
 
 IF(SUFFIX_SO_VERSION)
   SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})

cmake @ 84f1ae12

-Subproject commit 7ad2464a98ba78159ab0f29cdcbc716dcc0b1394
+Subproject commit 84f1ae122856a2aaf43b265f4b0689c0296afdc6

include/sot/core/abstract-sot-external-interface.hh

@@ -65,4 +65,4 @@ createSotExternalInterface_t();
 typedef void destroySotExternalInterface_t(
     dynamicgraph::sot::AbstractSotExternalInterface *);
 
-#endif
+#endif // ABSTRACT_SOT_EXTERNAL_INTERFACE_HH

include/sot/core/feature-point6d-relative.hh

@@ -81,14 +81,14 @@ public:
   FeaturePoint6dRelative(const std::string &name);
   virtual ~FeaturePoint6dRelative(void) {}
 
-  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
+  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector & res,
                                              int time);
-  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
+  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector & res,
                                                 int time);
-  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
+  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix & res,
                                                 int time);
 
-  virtual void display(std::ostream &os) const;
+  virtual void display(std::ostream & os) const;
 
   void initCommands(void);
   void initSdes(const std::string &featureDesiredName);

include/sot/core/feature-point6d.hh

@@ -89,11 +89,11 @@ public:
 
   virtual unsigned int &getDimension(unsigned int &dim, int time);
 
-  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
+  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector & res,
                                              int time);
-  virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector &res,
+  virtual dynamicgraph::Vector &computeErrordot(dynamicgraph::Vector & res,
                                                 int time);
-  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
+  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix & res,
                                                 int time);
 
   /** Static Feature selection. */
@@ -107,7 +107,7 @@ public:
   inline static Flags selectTranslation(void) { return Flags("111000"); }
   inline static Flags selectRotation(void) { return Flags("000111"); }
 
-  virtual void display(std::ostream &os) const;
+  virtual void display(std::ostream & os) const;
 
 public:
   void servoCurrentPosition(void);

include/sot/core/fwd.hh

@@ -16,8 +16,8 @@ namespace sot {
 
 class Device;
 class AbstractSotExternalInterface;
-  
+
 } // namespace sot
-}// namespace dynamicgraph
+} // namespace dynamicgraph
 
 #endif // SOT_CORE_FWD_HH

include/sot/core/macros.hh

@@ -4,20 +4,24 @@
 // ref https://www.fluentcpp.com/2019/08/30/how-to-disable-a-warning-in-cpp/
 #if defined(_MSC_VER)
 
-#define SOT_CORE_DISABLE_WARNING_PUSH           __pragma(warning( push ))
-#define SOT_CORE_DISABLE_WARNING_POP            __pragma(warning( pop ))
-#define SOT_CORE_DISABLE_WARNING(warningNumber) __pragma(warning( disable : warningNumber ))
-#define SOT_CORE_DISABLE_WARNING_DEPRECATED     SOT_CORE_DISABLE_WARNING(4996)
+#define SOT_CORE_DISABLE_WARNING_PUSH __pragma(warning(push))
+#define SOT_CORE_DISABLE_WARNING_POP __pragma(warning(pop))
+#define SOT_CORE_DISABLE_WARNING(warningNumber)                                \
+  __pragma(warning(disable : warningNumber))
+#define SOT_CORE_DISABLE_WARNING_DEPRECATED SOT_CORE_DISABLE_WARNING(4996)
 #define SOT_CORE_DISABLE_WARNING_FALLTHROUGH
 
 #elif defined(__GNUC__) || defined(__clang__)
 
 #define SOT_CORE_DO_PRAGMA(X) _Pragma(#X)
-#define SOT_CORE_DISABLE_WARNING_PUSH           SOT_CORE_DO_PRAGMA(GCC diagnostic push)
-#define SOT_CORE_DISABLE_WARNING_POP            SOT_CORE_DO_PRAGMA(GCC diagnostic pop)
-#define SOT_CORE_DISABLE_WARNING(warningName)   SOT_CORE_DO_PRAGMA(GCC diagnostic ignored #warningName)
-#define SOT_CORE_DISABLE_WARNING_DEPRECATED     SOT_CORE_DISABLE_WARNING(-Wdeprecated-declarations)
-#define SOT_CORE_DISABLE_WARNING_FALLTHROUGH    SOT_CORE_DISABLE_WARNING(-Wimplicit-fallthrough)
+#define SOT_CORE_DISABLE_WARNING_PUSH SOT_CORE_DO_PRAGMA(GCC diagnostic push)
+#define SOT_CORE_DISABLE_WARNING_POP SOT_CORE_DO_PRAGMA(GCC diagnostic pop)
+#define SOT_CORE_DISABLE_WARNING(warningName)                                  \
+  SOT_CORE_DO_PRAGMA(GCC diagnostic ignored #warningName)
+#define SOT_CORE_DISABLE_WARNING_DEPRECATED                                    \
+  SOT_CORE_DISABLE_WARNING(-Wdeprecated - declarations)
+#define SOT_CORE_DISABLE_WARNING_FALLTHROUGH                                   \
+  SOT_CORE_DISABLE_WARNING(-Wimplicit - fallthrough)
 
 #else
 

include/sot/core/sot-loader.hh

+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+#ifndef _SOT_LOADER_HH_
+#define _SOT_LOADER_HH_
+
+// STL includes
+#include <map>
+
+// Dynamic Graph embeded python interpreter.
+#include <dynamic-graph/python/interpreter.hh>
+
+// Sot Framework includes
+#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
+#include <sot/core/device.hh>
+
+namespace dynamicgraph {
+namespace sot {
+
+/**
+ * @brief This class is loading the control part of the Stack-Of-Tasks.
+ * - 1/ It loads dynamically the graph interface.
+ * - 2/ It loads the python interpreter.
+ * - 3/ It loads the Device entity C++ pointer inside the python interpreter.
+ * - 4/ It provides the user interface to the graph:
+ *    - 4.1/ starts and stop the graph executtion.
+ *    - 4.2/ run a python command/script inside the embeded python interpreter.
+ *    - 4.3/ execute one iteration of the graph.
+ *
+ * In order to Use this class you need to provide a dynamic library containing
+ * an implementation of the AbstractSotExternalInterface class.
+ *
+ * Then you can either inherite from this class an initialize and use the
+ * sensors_in_ and control_values_ objects.
+ * Or you can create you own outside of this class.
+ * And then use the oneIteration to execute the graph.
+ */
+class SotLoader {
+protected:
+  /// \brief Check if the dynamic graph is running or not.
+  bool dynamic_graph_stopped_;
+
+  /// \brief The interface between the device and the robot driver.
+  AbstractSotExternalInterface *sot_external_interface_;
+
+  /// \brief Name of the dynamic library containing the
+  /// dgs::AbstractSotExternalInterface object.
+  std::string sot_dynamic_library_filename_;
+
+  /// \brief Handle on the SoT library.
+  void *sot_dynamic_library_;
+
+  /// \brief Embeded python interpreter.
+  python::Interpreter embeded_python_interpreter_;
+
+  /// \brief Map of sensor readings
+  std::map<std::string, SensorValues> sensors_in_;
+
+  /// \brief Map of control values
+  std::map<std::string, ControlValues> control_values_;
+
+  /// \brief Device entity created and loaded, so we deregister it as the Pool
+  /// is not responsible for it's life time.
+  std::string device_name_;
+
+public:
+  /// \brief Default constructor.
+  SotLoader();
+  /// \brief Default destructor.
+  ~SotLoader();
+
+  /// \brief Read user input to extract the path of the SoT dynamic library.
+  int parseOptions(int argc, char *argv[]);
+
+  /// \brief Prepare the SoT framework.
+  bool initialization();
+
+  /// \brief Unload the library which handles the robot device.
+  void cleanUp();
+
+  /// \brief Get Status of dg.
+  inline bool isDynamicGraphStopped() { return dynamic_graph_stopped_; }
+
+  /// \brief Get Status of dg.
+  inline void startDG() { dynamic_graph_stopped_ = false; }
+
+  /// \brief Get Status of dg.
+  inline void stopDG() { dynamic_graph_stopped_ = true; }
+
+  /// \brief Specify the name of the dynamic library.
+  inline void setDynamicLibraryName(std::string &afilename) {
+    sot_dynamic_library_filename_ = afilename;
+  }
+
+  /// \brief Run a python command inside the embeded python interpreter.
+  void runPythonCommand(const std::string &command, std::string &result,
+                        std::string &out, std::string &err);
+
+  /// \brief Run a python script inside the embeded python interpreter.
+  inline void runPythonFile(std::string ifilename, std::string &err) {
+    embeded_python_interpreter_.runPythonFile(ifilename, err);
+  }
+
+  /// \brief Run a python script inside the embeded python interpreter.
+  inline void runPythonFile(std::string ifilename) {
+    embeded_python_interpreter_.runPythonFile(ifilename);
+  }
+
+  /// \brief Compute one iteration of control.
+  /// Basically executes fillSensors, the SoT and the readControl.
+  void oneIteration(std::map<std::string, SensorValues> &sensors_in,
+                    std::map<std::string, ControlValues> &control_values);
+
+  /// \brief Load the Device entity in the python global scope.
+  void loadDeviceInPython(const std::string &device_name);
+};
+
+} /* namespace sot */
+} /* namespace dynamicgraph */
+
+#endif /* _SOT_LOADER_HH_ */

src/feature/feature-posture.cpp

@@ -117,8 +117,8 @@ void FeaturePosture::selectDof(unsigned dofId, bool control) {
   // Check that selected dof id is valid
   if (dofId >= dim) {
     std::ostringstream oss;
-    oss << "dof id should less than state dimension: "
-        << dim << ". Received " << dofId << ".";
+    oss << "dof id should less than state dimension: " << dim << ". Received "
+        << dofId << ".";
     throw ExceptionAbstract(ExceptionAbstract::TOOLS, oss.str());
   }
 

src/tools/device.cpp

@@ -15,8 +15,8 @@
 
 #include "sot/core/device.hh"
 #include <iostream>
-#include <sot/core/macros.hh>
 #include <sot/core/debug.hh>
+#include <sot/core/macros.hh>
 using namespace std;
 
 #include <Eigen/Geometry>
@@ -230,35 +230,33 @@ void Device::setVelocitySize(const unsigned int &size) {
 void Device::setState(const Vector &st) {
   if (sanityCheck_) {
     const Vector::Index &s = st.size();
-SOT_CORE_DISABLE_WARNING_PUSH
-SOT_CORE_DISABLE_WARNING_FALLTHROUGH
+    SOT_CORE_DISABLE_WARNING_PUSH
+    SOT_CORE_DISABLE_WARNING_FALLTHROUGH
     switch (controlInputType_) {
     case CONTROL_INPUT_TWO_INTEGRATION:
       dgRTLOG()
           << "Sanity check for this control is not well supported. "
              "In order to make it work, use pinocchio and the contact forces "
              "to estimate the joint torques for the given acceleration.\n";
-      if (s != lowerTorque_.size() || s != upperTorque_.size())
-      {
-	std::ostringstream os;
-	os << "dynamicgraph::sot::Device::setState: upper and/or lower torque"
-	  "bounds do not match state size. Input State size = " << st.size()
-	   << ", lowerTorque_.size() = " << lowerTorque_.size()
-	   << ", upperTorque_.size() = " << upperTorque_.size()
-	   << ". Set them first with setTorqueBounds.";
-	throw std::invalid_argument(os.str().c_str());
-      // fall through
+      if (s != lowerTorque_.size() || s != upperTorque_.size()) {
+        std::ostringstream os;
+        os << "dynamicgraph::sot::Device::setState: upper and/or lower torque"
+              "bounds do not match state size. Input State size = "
+           << st.size() << ", lowerTorque_.size() = " << lowerTorque_.size()
+           << ", upperTorque_.size() = " << upperTorque_.size()
+           << ". Set them first with setTorqueBounds.";
+        throw std::invalid_argument(os.str().c_str());
+        // fall through
       }
     case CONTROL_INPUT_ONE_INTEGRATION:
-      if (s != lowerVelocity_.size() || s != upperVelocity_.size())
-      {
-	std::ostringstream os;
-	os << "dynamicgraph::sot::Device::setState: upper and/or lower velocity"
-	  " bounds do not match state size. Input State size = " << st.size()
-	   << ", lowerVelocity_.size() = " << lowerVelocity_.size()
-	   << ", upperVelocity_.size() = " << upperVelocity_.size()
-	   << ". Set them first with setVelocityBounds.";
-	throw std::invalid_argument(os.str().c_str());
+      if (s != lowerVelocity_.size() || s != upperVelocity_.size()) {
+        std::ostringstream os;
+        os << "dynamicgraph::sot::Device::setState: upper and/or lower velocity"
+              " bounds do not match state size. Input State size = "
+           << st.size() << ", lowerVelocity_.size() = " << lowerVelocity_.size()
+           << ", upperVelocity_.size() = " << upperVelocity_.size()
+           << ". Set them first with setVelocityBounds.";
+        throw std::invalid_argument(os.str().c_str());
       }
       // fall through
     case CONTROL_INPUT_NO_INTEGRATION:
@@ -266,7 +264,7 @@ SOT_CORE_DISABLE_WARNING_FALLTHROUGH
     default:
       throw std::invalid_argument("Invalid control mode type.");
     }
-SOT_CORE_DISABLE_WARNING_POP
+    SOT_CORE_DISABLE_WARNING_POP
   }
   state_ = st;
   stateSOUT.setConstant(state_);
@@ -319,8 +317,8 @@ void Device::setControlInputType(const std::string &cit) {
 void Device::setSanityCheck(const bool &enableCheck) {
   if (enableCheck) {
     const Vector::Index &s = state_.size();
-SOT_CORE_DISABLE_WARNING_PUSH
-SOT_CORE_DISABLE_WARNING_FALLTHROUGH
+    SOT_CORE_DISABLE_WARNING_PUSH
+    SOT_CORE_DISABLE_WARNING_FALLTHROUGH
     switch (controlInputType_) {
     case CONTROL_INPUT_TWO_INTEGRATION:
       dgRTLOG()
@@ -347,7 +345,7 @@ SOT_CORE_DISABLE_WARNING_FALLTHROUGH
     default:
       throw std::invalid_argument("Invalid control mode type.");
     }
-SOT_CORE_DISABLE_WARNING_POP
+    SOT_CORE_DISABLE_WARNING_POP
   }
   sanityCheck_ = enableCheck;
 }
@@ -356,12 +354,12 @@ void Device::setPositionBounds(const Vector &lower, const Vector &upper) {
   std::ostringstream oss;
   if (lower.size() != state_.size()) {
     oss << "Lower bound size should be " << state_.size() << ", got "
-	<< lower.size();
+        << lower.size();
     throw std::invalid_argument(oss.str());
   }
   if (upper.size() != state_.size()) {
     oss << "Upper bound size should be " << state_.size() << ", got "
-	<< upper.size();
+        << upper.size();
     throw std::invalid_argument(oss.str());
   }
   lowerPosition_ = lower;
@@ -372,12 +370,12 @@ void Device::setVelocityBounds(const Vector &lower, const Vector &upper) {
   std::ostringstream oss;
   if (lower.size() != velocity_.size()) {
     oss << "Lower bound size should be " << velocity_.size() << ", got "
-	<< lower.size();
+        << lower.size();
     throw std::invalid_argument(oss.str());
   }
   if (upper.size() != velocity_.size()) {
     oss << "Upper bound size should be " << velocity_.size() << ", got "
-	<< upper.size();
+        << upper.size();
     throw std::invalid_argument(oss.str());
   }
   lowerVelocity_ = lower;
@@ -389,12 +387,12 @@ void Device::setTorqueBounds(const Vector &lower, const Vector &upper) {
   std::ostringstream oss;
   if (lower.size() != state_.size()) {
     oss << "Lower bound size should be " << state_.size() << ", got "
-	<< lower.size();
+        << lower.size();
     throw std::invalid_argument(oss.str());
   }
   if (upper.size() != state_.size()) {
     oss << "Lower bound size should be " << state_.size() << ", got "
-	<< upper.size();
+        << upper.size();
     throw std::invalid_argument(oss.str());
   }
   lowerTorque_ = lower;

src/tools/sot-loader.cpp

+/*
+ * Copyright 2016,
+ * Olivier Stasse,
+ *
+ * CNRS
+ *
+ */
+/* -------------------------------------------------------------------------- */
+/* --- INCLUDES ------------------------------------------------------------- */
+/* -------------------------------------------------------------------------- */
+
+// POSIX.1-2001
+#include <dlfcn.h>
+
+// C++ includes
+#include <iostream>
+#include <sstream>
+
+// Boost includes
+#include <boost/program_options.hpp>
+
+// Dynamic Graph includes
+#include <dynamic-graph/pool.h>
+
+// Local includes
+#include <sot/core/sot-loader.hh>
+
+namespace po = boost::program_options;
+
+namespace dynamicgraph {
+namespace sot {
+
+SotLoader::SotLoader() {
+  dynamic_graph_stopped_ = true;
+  sot_external_interface_ = nullptr;
+  sot_dynamic_library_filename_ = "";
+  sot_dynamic_library_ = nullptr;
+  device_name_ = "";
+}
+
+SotLoader::~SotLoader() { cleanUp(); }
+
+int SotLoader::parseOptions(int argc, char *argv[]) {
+  po::options_description desc("Allowed options");
+  desc.add_options()("help", "produce help message")(
+      "sot-dynamic-library", po::value<std::string>(), "Library to load");
+  desc.add_options()("help", "produce help message")(
+      "input-file", po::value<std::string>(), "Library to load");
+
+  // Input variable map from the command line (int argc, char* argv[]).
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+  po::notify(vm);
+
+  if (vm.count("help")) {
+    std::cout << desc << "\n";
+    return -1;
+  }
+  if (vm.count("sot-dynamic-library")) {
+    sot_dynamic_library_filename_ = vm["sot-dynamic-library"].as<std::string>();
+  } else if (vm.count("input-file")) {
+    sot_dynamic_library_filename_ = vm["input-file"].as<std::string>();
+  } else {
+    std::cout << "No filename specified\n";
+    return -1;
+  }
+  return 0;
+}
+
+bool SotLoader::initialization() {
+  // Load the library containing the AbstractSotExternalInterface.
+  sot_dynamic_library_ =
+      dlopen(sot_dynamic_library_filename_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+  if (!sot_dynamic_library_) {
+    std::cerr << "Cannot load library: " << dlerror() << '\n';
+    return false;
+  }
+
+  // reset errors
+  dlerror();
+
+  // Load the symbols.
+  createSotExternalInterface_t *createSotExternalInterface =
+      reinterpret_cast<createSotExternalInterface_t *>(reinterpret_cast<long>(
+          dlsym(sot_dynamic_library_, "createSotExternalInterface")));
+  const char *dlsym_error = dlerror();
+  if (dlsym_error) {
+    std::cerr << "Cannot load symbol create: " << dlsym_error << '\n';
+    return false;
+  }
+
+  // Create robot-controller
+  sot_external_interface_ = createSotExternalInterface();
+  assert(sot_external_interface_ && "Fail to create the sotExternalInterface");
+  std::cout << "SoT loaded at address [" << &sot_external_interface_
+            << "] from " << sot_dynamic_library_filename_ << "." << std::endl;
+
+  // Init the python interpreter.
+  std::string result, out, err;
+  // Debug print.
+  runPythonCommand("print(\"Executing python interpreter prologue...\")",
+                   result, out, err);
+  // make sure that the current environment variable are setup in the current
+  // python interpreter.
+  runPythonCommand("import sys, os", result, out, err);
+  runPythonCommand("print(\"python version:\", sys.version)", result, out, err);
+  runPythonCommand("pythonpath = os.environ['PYTHONPATH']", result, out, err);
+  runPythonCommand("path = []", result, out, err);
+  runPythonCommand("for p in pythonpath.split(':'):\n"
+                   "  if p not in sys.path:\n"
+                   "    path.append(p)",
+                   result, out, err);
+  runPythonCommand("path.extend(sys.path)", result, out, err);
+  runPythonCommand("sys.path = path", result, out, err);
+  // used to be able to invoke rospy
+  runPythonCommand("if not hasattr(sys, \'argv\'):\n"
+                   "    sys.argv  = ['sot']",
+                   result, out, err);
+  // help setting signals
+  runPythonCommand("import numpy as np", result, out, err);
+  // Debug print.
+  runPythonCommand("print(\"Executing python interpreter prologue... Done\")",
+                   result, out, err);
+
+  return true;
+}
+
+void SotLoader::cleanUp() {
+  // Unregister the device first if it exists to avoid a double destruction from
+  // the pool of entity and the class that handle the Device pointer.
+  if (device_name_ != "") {
+    PoolStorage::getInstance()->deregisterEntity(device_name_);
+  }
+
+  // We do not destroy the FactoryStorage singleton because the module will not
+  // be reloaded at next initialization (because Python C API cannot safely
+  // unload a module...).
+  // SignalCaster singleton could probably be destroyed.
+  dynamicgraph::PoolStorage::destroy();
+
+  // Load the symbols.
+  if (sot_dynamic_library_ != nullptr && sot_external_interface_ != nullptr) {
+    destroySotExternalInterface_t *destroySotExternalInterface =
+        reinterpret_cast<destroySotExternalInterface_t *>(
+            reinterpret_cast<long>(
+                dlsym(sot_dynamic_library_, "destroySotExternalInterface")));
+    const char *dlsym_error = dlerror();
+    if (dlsym_error) {
+      std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
+      return;
+    }
+
+    destroySotExternalInterface(sot_external_interface_);
+    sot_external_interface_ = nullptr;
+
+    /// Uncount the number of access to this library.
+    dlclose(sot_dynamic_library_);
+    sot_dynamic_library_ = nullptr;
+  }
+}
+
+void SotLoader::runPythonCommand(const std::string &command,
+                                 std::string &result, std::string &out,
+                                 std::string &err) {
+  embeded_python_interpreter_.python(command, result, out, err);
+}
+
+void SotLoader::oneIteration(
+    std::map<std::string, SensorValues> &sensors_in,
+    std::map<std::string, ControlValues> &control_values) {
+  if (!dynamic_graph_stopped_) {
+    try {
+      sot_external_interface_->nominalSetSensors(sensors_in);
+      sot_external_interface_->getControl(control_values);
+    } catch (std::exception &e) {
+      std::cout << "Exception while running the graph:\n"
+                << e.what() << std::endl;
+      throw e;
+    }
+  }
+}
+
+void SotLoader::loadDeviceInPython(const std::string &device_name) {
+  std::string result, out, err;