Add python bindings for dynamical_systems module (#238)

* Install DS module

* Add DS bindings

* Add DS unittests

* Fix CI

* Update CHANGELOG

* Allow to set attractor from JointPositions

* Add chrono operators for joint space

* Add tests for joint space point attractor DS

* Remove hashtag

.github/actions/build-test-python/entrypoint.sh

@@ -1,7 +1,7 @@
 #!/bin/sh -l
 
 echo ">>> Installing control libraries..."
-bash /github/workspace/source/install.sh --auto --no-controllers --no-dynamical-systems --no-robot-model || exit 1
+bash /github/workspace/source/install.sh --auto --no-controllers --no-robot-model || exit 1
 bash /github/workspace/protocol/install.sh --auto || exit 1
 
 echo ">>> Building Python bindings..."

CHANGELOG.md

@@ -25,6 +25,7 @@ Release Versions:
 - Add the `set_parameter_value` function in the DS base class (#236)
 - Remove the `set_base_frame` logic for `JointState` based DS
   and override `is_compatible` for PointAttractor DS (#236, #239)
+- Add python bindings for dynamical_systems module (#238)
 
 ### Pending TODOs for the next release
 

python/Dockerfile.python

@@ -4,7 +4,7 @@ ARG BRANCH=develop
 
 WORKDIR /source
 RUN git clone --single-branch --branch $BRANCH https://github.com/epfl-lasa/control-libraries
-RUN bash control-libraries/source/install.sh --auto --no-controllers --no-dynamical-systems --no-robot-model
+RUN bash control-libraries/source/install.sh --auto --no-controllers --no-robot-model
 RUN bash control-libraries/protocol/install.sh --auto
 
 COPY include control-libraries/python/include

python/include/dynamical_systems_bindings.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <dynamical_systems/IDynamicalSystem.hpp>
+
+namespace py = pybind11;
+using namespace pybind11::literals;
+using namespace dynamical_systems;
+
+void bind_type(py::module_& m);
+void bind_cartesian(py::module_& m);
+void bind_joint(py::module_& m);

python/include/py_dynamical_system.h

@@ -0,0 +1,25 @@
+#include <dynamical_systems/IDynamicalSystem.hpp>
+#include <state_representation/parameters/ParameterInterface.hpp>
+
+template<class S>
+class PyDynamicalSystem : public IDynamicalSystem<S>, public std::enable_shared_from_this<PyDynamicalSystem<S>> {
+public:
+  using IDynamicalSystem<S>::IDynamicalSystem;
+
+  [[nodiscard]] bool is_compatible(const S& state) const override {
+    PYBIND11_OVERRIDE(bool, IDynamicalSystem<S>, is_compatible, state);
+  }
+
+  void set_base_frame(const S& base_frame) override {
+    PYBIND11_OVERRIDE(void, IDynamicalSystem<S>, set_base_frame, base_frame);
+  }
+
+protected:
+  [[nodiscard]] S compute_dynamics(const S& state) const override {
+    PYBIND11_OVERRIDE_PURE(S, IDynamicalSystem<S>, compute_dynamics, state);
+  }
+
+  void validate_and_set_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override {
+    PYBIND11_OVERRIDE_PURE(void, IDynamicalSystem<S>, validate_and_set_parameter, parameter);
+  }
+};

python/setup.py

@@ -6,10 +6,11 @@
 import os
 
 __version__ = "4.1.0"
-__libraries__ = ['state_representation', 'clproto']
+__libraries__ = ['state_representation', 'clproto', 'dynamical_systems']
 __include_dirs__ = ['include']
 
 __install_clproto_module__ = True
+__install_dynamical_systems_module__ = True
 
 # check that eigen and state_representation libraries can be found
 try:
@@ -27,6 +28,9 @@
             if lib == 'clproto':
                 warnings.warn(f'{msg} The clproto module will not be installed.')
                 __install_clproto_module__ = False
+            if lib == 'dynamical_systems':
+                warnings.warn(f'{msg} The dynamical_systems module will not be installed.')
+                __install_dynamical_systems_module__ = False
             else:
                 raise Exception(msg)
 except Exception as e:
@@ -56,6 +60,17 @@
                           )
     )
 
+if __install_dynamical_systems_module__:
+    ext_modules.append(
+        Pybind11Extension("dynamical_systems",
+                          sorted(glob("source/dynamical_systems/*.cpp") + glob("source/common/*.cpp")),
+                          cxx_std=17,
+                          include_dirs=__include_dirs__,
+                          libraries=['state_representation', 'dynamical_systems'],
+                          define_macros=[('MODULE_VERSION_INFO', __version__)],
+                          )
+    )
+
 setup(
     name="control-libraries",
     version=__version__,

python/source/dynamical_systems/bind_cartesian.cpp

@@ -0,0 +1,87 @@
+#include "dynamical_systems_bindings.h"
+
+#include <dynamical_systems/Circular.hpp>
+#include <dynamical_systems/DefaultDynamicalSystem.hpp>
+#include <dynamical_systems/DynamicalSystemFactory.hpp>
+#include <dynamical_systems/IDynamicalSystem.hpp>
+#include <dynamical_systems/PointAttractor.hpp>
+#include <dynamical_systems/Ring.hpp>
+#include <state_representation/parameters/Parameter.hpp>
+#include <state_representation/space/cartesian/CartesianState.hpp>
+
+#include "parameter_container.h"
+#include "py_dynamical_system.h"
+
+using namespace state_representation;
+
+void cartesian(py::module_& m) {
+  py::class_<IDynamicalSystem<CartesianState>, PyDynamicalSystem<CartesianState>> c(m, "ICartesianDS");
+
+  c.def(py::init<>());
+
+  c.def("is_compatible", &IDynamicalSystem<CartesianState>::is_compatible);
+  c.def("evaluate", &IDynamicalSystem<CartesianState>::evaluate);
+  c.def("get_base_frame", &IDynamicalSystem<CartesianState>::get_base_frame);
+  c.def("set_base_frame", &IDynamicalSystem<CartesianState>::set_base_frame);
+
+  c.def("get_parameter", [](IDynamicalSystem<CartesianState>& self, const std::string& name) {
+    return parameter_interface_ptr_to_container(self.get_parameter(name));
+  });
+  c.def("get_parameters", [](IDynamicalSystem<CartesianState>& self) {
+    py::dict dict;
+    for (const auto& param_it : self.get_parameters()) {
+      dict[py::str(param_it.first)] = parameter_interface_ptr_to_container(param_it.second);
+    }
+    return dict;
+  });
+  c.def("get_parameter_value", [](IDynamicalSystem<CartesianState>& self, const std::string& name) {
+    return parameter_interface_ptr_to_container(self.get_parameter(name)).get_value();
+  });
+  c.def("get_parameter_list", [](IDynamicalSystem<CartesianState>& self) {
+    py::list list;
+    for (const auto& param_it : self.get_parameters()) {
+      list.append(parameter_interface_ptr_to_container(param_it.second));
+    }
+    return list;
+  });
+
+  c.def("set_parameter", [](IDynamicalSystem<CartesianState>& self, const ParameterContainer& parameter) {
+    self.set_parameter(container_to_parameter_interface_ptr(parameter));
+  });
+  c.def("set_parameters", [](IDynamicalSystem<CartesianState>& self, const std::vector<ParameterContainer>& parameters) {
+    for (auto param : parameters) {
+      self.set_parameter(container_to_parameter_interface_ptr(param));
+    }
+  });
+  c.def("set_parameters", [](IDynamicalSystem<CartesianState>& self, const std::map<std::string, ParameterContainer>& parameters) {
+    for (const auto& param_it: parameters) {
+      self.set_parameter(container_to_parameter_interface_ptr(param_it.second));
+    }
+  });
+}
+
+void bind_cartesian(py::module_& m) {
+  cartesian(m);
+  py::class_<Circular, IDynamicalSystem<CartesianState>>(m, "CartesianCircularDS").def(py::init<>());
+  py::class_<DefaultDynamicalSystem<CartesianState>, IDynamicalSystem<CartesianState>>(m, "CartesianDefaultDS").def(py::init<>());
+  py::class_<PointAttractor<CartesianState>, IDynamicalSystem<CartesianState>>(m, "CartesianPointAttractorDS").def(py::init<>());
+  py::class_<Ring, IDynamicalSystem<CartesianState>>(m, "CartesianRingDS").def(py::init<>());
+
+  m.def("create_cartesian_ds", [](DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM type) -> py::object {
+    switch (type) {
+      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::CIRCULAR:
+        return py::cast(Circular());
+        break;
+      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR:
+        return py::cast(PointAttractor<CartesianState>());
+        break;
+      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::RING:
+        return py::cast(Ring());
+        break;
+      default:
+      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE:
+        return py::cast(DefaultDynamicalSystem<CartesianState>());
+        break;
+    }
+  });
+}

python/source/dynamical_systems/bind_joint.cpp

@@ -0,0 +1,77 @@
+#include "dynamical_systems_bindings.h"
+
+#include <dynamical_systems/DefaultDynamicalSystem.hpp>
+#include <dynamical_systems/DynamicalSystemFactory.hpp>
+#include <dynamical_systems/IDynamicalSystem.hpp>
+#include <dynamical_systems/PointAttractor.hpp>
+#include <state_representation/parameters/Parameter.hpp>
+#include <state_representation/robot/JointState.hpp>
+
+#include "parameter_container.h"
+#include "py_dynamical_system.h"
+
+using namespace state_representation;
+
+void joint(py::module_& m) {
+  py::class_<IDynamicalSystem<JointState>, PyDynamicalSystem<JointState>> c(m, "IJointDS");
+
+  c.def(py::init<>());
+
+  c.def("is_compatible", &IDynamicalSystem<JointState>::is_compatible);
+  c.def("evaluate", &IDynamicalSystem<JointState>::evaluate);
+  c.def("get_base_frame", &IDynamicalSystem<JointState>::get_base_frame);
+  c.def("set_base_frame", &IDynamicalSystem<JointState>::set_base_frame);
+
+  c.def("get_parameter", [](IDynamicalSystem<JointState>& self, const std::string& name) {
+    return parameter_interface_ptr_to_container(self.get_parameter(name));
+  });
+  c.def("get_parameters", [](IDynamicalSystem<JointState>& self) {
+    py::dict dict;
+    for (const auto& param_it : self.get_parameters()) {
+      dict[py::str(param_it.first)] = parameter_interface_ptr_to_container(param_it.second);
+    }
+    return dict;
+  });
+  c.def("get_parameter_value", [](IDynamicalSystem<JointState>& self, const std::string& name) {
+    return parameter_interface_ptr_to_container(self.get_parameter(name)).get_value();
+  });
+  c.def("get_parameter_list", [](IDynamicalSystem<JointState>& self) {
+    py::list list;
+    for (const auto& param_it : self.get_parameters()) {
+      list.append(parameter_interface_ptr_to_container(param_it.second));
+    }
+    return list;
+  });
+
+  c.def("set_parameter", [](IDynamicalSystem<JointState>& self, const ParameterContainer& parameter) {
+    self.set_parameter(container_to_parameter_interface_ptr(parameter));
+  });
+  c.def("set_parameters", [](IDynamicalSystem<JointState>& self, const std::vector<ParameterContainer>& parameters) {
+    for (auto param : parameters) {
+      self.set_parameter(container_to_parameter_interface_ptr(param));
+    }
+  });
+  c.def("set_parameters", [](IDynamicalSystem<JointState>& self, const std::map<std::string, ParameterContainer>& parameters) {
+    for (const auto& param_it: parameters) {
+      self.set_parameter(container_to_parameter_interface_ptr(param_it.second));
+    }
+  });
+}
+
+void bind_joint(py::module_& m) {
+  joint(m);
+  py::class_<DefaultDynamicalSystem<JointState>, IDynamicalSystem<JointState>>(m, "JointDefaultDS").def(py::init<>());
+  py::class_<PointAttractor<JointState>, IDynamicalSystem<JointState>>(m, "JointPointAttractorDS").def(py::init<>());
+
+  m.def("create_joint_ds", [](DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM type) -> py::object {
+    switch (type) {
+      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR:
+        return py::cast(PointAttractor<JointState>());
+        break;
+      default:
+      case DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE:
+        return py::cast(DefaultDynamicalSystem<JointState>());
+        break;
+    }
+  });
+}

python/source/dynamical_systems/bind_type.cpp

@@ -0,0 +1,15 @@
+#include "dynamical_systems_bindings.h"
+
+#include <dynamical_systems/DynamicalSystemFactory.hpp>
+#include <state_representation/space/cartesian/CartesianState.hpp>
+
+using namespace state_representation;
+
+void bind_type(py::module_& m) {
+  py::enum_<DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM>(m, "DYNAMICAL_SYSTEM")
+      .value("NONE", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::NONE)
+      .value("CIRCULAR", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::CIRCULAR)
+      .value("POINT_ATTRACTOR", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::POINT_ATTRACTOR)
+      .value("RING", DynamicalSystemFactory<CartesianState>::DYNAMICAL_SYSTEM::RING)
+      .export_values();
+}

python/source/dynamical_systems/dynamical_systems_bindings.cpp

@@ -0,0 +1,18 @@
+#include "dynamical_systems_bindings.h"
+
+#define STRINGIFY(x) #x
+#define MACRO_STRINGIFY(x) STRINGIFY(x)
+
+PYBIND11_MODULE(dynamical_systems, m) {
+  m.doc() = "Python bindings for control libraries dynamical_systems";
+
+  #ifdef MODULE_VERSION_INFO
+  m.attr("__version__") = MACRO_STRINGIFY(MODULE_VERSION_INFO);
+  #else
+  m.attr("__version__") = "dev";
+  #endif
+
+  bind_type(m);
+  bind_cartesian(m);
+  bind_joint(m);
+}

python/source/state_representation/bind_joint_space.cpp

@@ -219,7 +219,9 @@ void joint_velocities(py::module_& m) {
   c.def(py::self / double());
   c.def(py::self / std::chrono::nanoseconds());
   c.def(py::self * std::chrono::nanoseconds());
+
   c.def(double() * py::self);
+  c.def(std::chrono::nanoseconds() * py::self);
   c.def(Eigen::ArrayXd() * py::self);
   c.def(Eigen::MatrixXd() * py::self);
 
@@ -290,7 +292,9 @@ void joint_accelerations(py::module_& m) {
   c.def(py::self /= double());
   c.def(py::self / double());
   c.def(py::self * std::chrono::nanoseconds());
+
   c.def(double() * py::self);
+  c.def(std::chrono::nanoseconds() * py::self);
   c.def(Eigen::ArrayXd() * py::self);
   c.def(Eigen::MatrixXd() * py::self);