feat: add IO states to state representation (py) (#173)

CHANGELOG.md

@@ -13,6 +13,7 @@ Release Versions:
 
 ## Upcoming changes (in development)
 
+- feat: add IO states to state representation (py) (#173)
 - ci: use caching from docker to run tests in CI (#429)
 - feat: add IO states to state representation (proto) (#172)
 - feat: add IO states to state representation (cpp) (#158)

VERSION

@@ -1 +1 @@
-7.3.8
+7.3.9

demos/CMakeLists.txt

@@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(control_libraries 7.3.8 CONFIG REQUIRED)
+find_package(control_libraries 7.3.9 CONFIG REQUIRED)
 
 set(DEMOS_SCRIPTS
   task_space_control_loop

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 7.3.8
+PROJECT_NUMBER = 7.3.9
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

protocol/clproto_cpp/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(clproto VERSION 7.3.8)
+project(clproto VERSION 7.3.9)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

python/include/state_representation_bindings.hpp

@@ -26,3 +26,4 @@ void bind_joint_space(py::module_& m);
 void bind_jacobian(py::module_& m);
 void bind_parameters(py::module_& m);
 void bind_geometry(py::module_& m);
+void bind_io_state(py::module_& m);

python/setup.py

@@ -11,7 +11,7 @@
 # names of the environment variables that define osqp and openrobots include directories
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
-__version__ = "7.3.8"
+__version__ = "7.3.9"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

python/source/clproto/bind_clproto.cpp

@@ -17,6 +17,8 @@
 #include <state_representation/space/joint/JointTorques.hpp>
 #include <state_representation/geometry/Shape.hpp>
 #include <state_representation/geometry/Ellipsoid.hpp>
+#include <state_representation/DigitalIOState.hpp>
+#include <state_representation/AnalogIOState.hpp>
 
 using namespace clproto;
 using namespace state_representation;
@@ -139,6 +141,8 @@ void message_type(py::module_& m) {
       .value("SHAPE_MESSAGE", MessageType::SHAPE_MESSAGE)
       .value("ELLIPSOID_MESSAGE", MessageType::ELLIPSOID_MESSAGE)
       .value("PARAMETER_MESSAGE", MessageType::PARAMETER_MESSAGE)
+      .value("DIGITAL_IO_STATE_MESSAGE", MessageType::DIGITAL_IO_STATE_MESSAGE)
+      .value("ANALOG_IO_STATE_MESSAGE", MessageType::ANALOG_IO_STATE_MESSAGE)
       .export_values();
 }
 
@@ -169,6 +173,10 @@ void methods(py::module_& m) {
       switch (type) {
         case MessageType::STATE_MESSAGE:
           return encode_bytes<State>(object.cast<State>());
+        case MessageType::DIGITAL_IO_STATE_MESSAGE:
+          return encode_bytes<DigitalIOState>(object.cast<DigitalIOState>());
+        case MessageType::ANALOG_IO_STATE_MESSAGE:
+          return encode_bytes<AnalogIOState>(object.cast<AnalogIOState>());
         case MessageType::SPATIAL_STATE_MESSAGE:
           return encode_bytes<SpatialState>(object.cast<SpatialState>());
         case MessageType::CARTESIAN_STATE_MESSAGE:
@@ -208,6 +216,10 @@ void methods(py::module_& m) {
       switch (check_message_type(msg)) {
         case MessageType::STATE_MESSAGE:
           return py::cast(decode<State>(msg));
+        case MessageType::DIGITAL_IO_STATE_MESSAGE:
+          return py::cast(decode<DigitalIOState>(msg));
+        case MessageType::ANALOG_IO_STATE_MESSAGE:
+          return py::cast(decode<AnalogIOState>(msg));
         case MessageType::SPATIAL_STATE_MESSAGE:
           return py::cast(decode<SpatialState>(msg));
         case MessageType::CARTESIAN_STATE_MESSAGE:

python/source/state_representation/bind_io_state.cpp

@@ -0,0 +1,111 @@
+#include "state_representation_bindings.hpp"
+
+#include <state_representation/DigitalIOState.hpp>
+#include <state_representation/AnalogIOState.hpp>
+
+
+void digital_io_state(py::module_& m) {
+  py::class_<DigitalIOState, std::shared_ptr<DigitalIOState>, State> c(m, "DigitalIOState");
+
+  c.def(py::init(), "Empty constructor for an digital IO state");
+  c.def(py::init<const std::string&, unsigned int>(), "Constructor with name and number of digital IOs provided", "name"_a, "nb_ios"_a=0);
+  c.def(py::init<const std::string&, const std::vector<std::string>&>(), "Constructor with name and list of digital IO names provided", "name"_a, "io_names"_a);
+  c.def(py::init<const DigitalIOState&>(), "Copy constructor of an digital IO state", "state"_a);
+
+  c.def_static("Zero", py::overload_cast<const std::string&, unsigned int>(&DigitalIOState::Zero), "Constructor for a zero digital IO state", "name"_a, "nb_ios"_a);
+  c.def_static("Zero", py::overload_cast<const std::string&, const std::vector<std::string>&>(&DigitalIOState::Zero), "Constructor for a zero digital IO state", "name"_a, "io_names"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, unsigned int>(&DigitalIOState::Random), "Constructor for a random digital IO state", "name"_a, "nb_ios"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, const std::vector<std::string>&>(&DigitalIOState::Random), "Constructor for a random digital IO state", "name"_a, "io_names"_a);
+
+  c.def("get_size", &DigitalIOState::get_size, "Getter of the size from the attributes.");
+  c.def("get_names", &DigitalIOState::get_names, "Getter of the names attribute.");
+  c.def("get_joint_index", &DigitalIOState::get_io_index, "Get IO index by the name of the IO, if it exists", "io_name"_a);
+  c.def("set_names", py::overload_cast<unsigned int>(&DigitalIOState::set_names), "Setter of the names from the number of IOs", "nb_ios"_a);
+  c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&DigitalIOState::set_names), "Setter of the names from a list of IO names", "names"_a);
+
+  c.def("get_value", [](const DigitalIOState& state, const std::string& name) { return state.get_value(name); }, "Get the value of a digital IO by its name, if it exists", "name"_a);
+  c.def("get_value", [](const DigitalIOState& state, unsigned int io_index) { return state.get_value(io_index); }, "Get the value of a digital IO by its index, if it exists", "io_index"_a);
+  c.def("set_value", py::overload_cast<bool, const std::string&>(&DigitalIOState::set_value), "Set the value of a digital IO by its name", "value"_a, "name"_a);
+  c.def("set_value", py::overload_cast<bool, unsigned int>(&DigitalIOState::set_value), "Set the value of a digital IO by its index", "value"_a, "io_index"_a);
+
+  c.def("is_true", [](const DigitalIOState& state, const std::string& name) { return state.is_true(name); }, "Check if a digital IO is true by its name, if it exists", "name"_a);
+  c.def("is_true", [](const DigitalIOState& state, unsigned int io_index) { return state.is_true(io_index); }, "Check if a digital IO is true by its index, if it exists", "io_index"_a);
+  c.def("is_false", [](const DigitalIOState& state, const std::string& name) { return state.is_false(name); }, "Check if a digital IO is false by its name, if it exists", "name"_a);
+  c.def("is_false", [](const DigitalIOState& state, unsigned int io_index) { return state.is_false(io_index); }, "Check if a digital IO is false by its index, if it exists", "io_index"_a);
+  c.def("set_true", py::overload_cast<const std::string&>(&DigitalIOState::set_true), "Set the a digital IO to true by its name", "name"_a);
+  c.def("set_true", py::overload_cast<unsigned int>(&DigitalIOState::set_true), "Set the a digital IO to true by its index", "io_index"_a);
+  c.def("set_false", py::overload_cast<const std::string&>(&DigitalIOState::set_false), "Set the a digital IO to false by its name", "name"_a);
+  c.def("set_false", py::overload_cast<unsigned int>(&DigitalIOState::set_false), "Set the a digital IO to false by its index", "io_index"_a);
+
+  c.def("copy", &DigitalIOState::copy, "Return a copy of the digital IO state");
+  c.def("set_false", py::overload_cast<>(&DigitalIOState::set_false), "Set all digital IOs false");
+  c.def("data", &DigitalIOState::data, "Returns the values of the IO state as an Eigen vector");
+  c.def("array", &DigitalIOState::array, "Returns the values of the IO state an Eigen array");
+  c.def("set_data", py::overload_cast<const Eigen::Vector<bool, Eigen::Dynamic>&>(&DigitalIOState::set_data), "Set the values of the IO state from a single Eigen vector", "data"_a);
+  c.def("set_data", py::overload_cast<const std::vector<bool>&>(&DigitalIOState::set_data), "Set the values of the IO state from a single list", "data"_a);
+
+  c.def("to_list", &DigitalIOState::to_std_vector, "Return the IO values as a list");
+
+  c.def("__copy__", [](const DigitalIOState &state) {
+    return DigitalIOState(state);
+  });
+  c.def("__deepcopy__", [](const DigitalIOState &state, py::dict) {
+    return DigitalIOState(state);
+  }, "memo"_a);
+  c.def("__repr__", [](const DigitalIOState& state) {
+    std::stringstream buffer;
+    buffer << state;
+    return buffer.str();
+  });
+}
+
+void analog_io_state(py::module_& m) {
+  py::class_<AnalogIOState, std::shared_ptr<AnalogIOState>, State> c(m, "AnalogIOState");
+
+  c.def(py::init(), "Empty constructor for an analog IO state");
+  c.def(py::init<const std::string&, unsigned int>(), "Constructor with name and number of analog IOs provided", "name"_a, "nb_ios"_a=0);
+  c.def(py::init<const std::string&, const std::vector<std::string>&>(), "onstructor with name and list of analog IO names provided", "name"_a, "io_names"_a);
+  c.def(py::init<const AnalogIOState&>(), "Copy constructor of an analog IO state", "state"_a);
+
+  c.def_static("Zero", py::overload_cast<const std::string&, unsigned int>(&AnalogIOState::Zero), "Constructor for a zero analog IO state", "name"_a, "nb_ios"_a);
+  c.def_static("Zero", py::overload_cast<const std::string&, const std::vector<std::string>&>(&AnalogIOState::Zero), "Constructor for a zero analog IO state", "name"_a, "io_names"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, unsigned int>(&AnalogIOState::Random), "Constructor for a random analog IO state", "name"_a, "nb_ios"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, const std::vector<std::string>&>(&AnalogIOState::Random), "Constructor for a random analog IO state", "name"_a, "io_names"_a);
+
+  c.def("get_size", &AnalogIOState::get_size, "Getter of the size from the attributes.");
+  c.def("get_names", &AnalogIOState::get_names, "Getter of the names attribute.");
+  c.def("get_joint_index", &AnalogIOState::get_io_index, "Get IO index by the name of the IO, if it exists", "io_name"_a);
+  c.def("set_names", py::overload_cast<unsigned int>(&AnalogIOState::set_names), "Setter of the names from the number of IOs", "nb_ios"_a);
+  c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&AnalogIOState::set_names), "Setter of the names from a list of IO names", "names"_a);
+
+  c.def("get_value", [](const AnalogIOState& state, const std::string& name) { return state.get_value(name); }, "Get the value of an analog IO by its name, if it exists", "name"_a);
+  c.def("get_value", [](const AnalogIOState& state, unsigned int io_index) { return state.get_value(io_index); }, "Get the value of an analog IO by its index, if it exists", "io_index"_a);
+  c.def("set_value", py::overload_cast<double, const std::string&>(&AnalogIOState::set_value), "Set the value of an analog IO by its name", "value"_a, "name"_a);
+  c.def("set_value", py::overload_cast<double, unsigned int>(&AnalogIOState::set_value), "Set the value of an analog IO by its index", "value"_a, "io_index"_a);
+
+  c.def("copy", &AnalogIOState::copy, "Return a copy of the analog IO state");
+  c.def("set_zero", &AnalogIOState::set_zero, "Set the analog IO state to zero data");
+  c.def("data", &AnalogIOState::data, "Returns the values of the IO state as an Eigen vector");
+  c.def("array", &AnalogIOState::array, "Returns the values of the IO state an Eigen array");
+  c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&AnalogIOState::set_data), "Set the values of the IO state from a single Eigen vector", "data"_a);
+  c.def("set_data", py::overload_cast<const std::vector<double>&>(&AnalogIOState::set_data), "Set the values of the IO state from a single list", "data"_a);
+
+  c.def("to_list", &AnalogIOState::to_std_vector, "Return the IO values as a list");
+
+  c.def("__copy__", [](const AnalogIOState &state) {
+    return AnalogIOState(state);
+  });
+  c.def("__deepcopy__", [](const AnalogIOState &state, py::dict) {
+    return AnalogIOState(state);
+  }, "memo"_a);
+  c.def("__repr__", [](const AnalogIOState& state) {
+    std::stringstream buffer;
+    buffer << state;
+    return buffer.str();
+  });
+}
+
+void bind_io_state(py::module_& m) {
+  digital_io_state(m);
+  analog_io_state(m);
+}

python/source/state_representation/bind_state.cpp

@@ -23,6 +23,8 @@ void state_type(py::module_& m) {
       .value("GEOMETRY_SHAPE", StateType::GEOMETRY_SHAPE)
       .value("GEOMETRY_ELLIPSOID", StateType::GEOMETRY_ELLIPSOID)
       .value("TRAJECTORY", StateType::TRAJECTORY)
+      .value("DIGITAL_IO_STATE", StateType::DIGITAL_IO_STATE)
+      .value("ANALOG_IO_STATE", StateType::ANALOG_IO_STATE)
       .export_values();
 }
 

python/source/state_representation/state_representation_bindings.cpp

@@ -20,4 +20,5 @@ PYBIND11_MODULE(state_representation, m) {
   bind_jacobian(m);
   bind_parameters(m);
   bind_geometry(m);
+  bind_io_state(m);
 }

python/test/state_representation/test_io_state.py

@@ -0,0 +1,33 @@
+import pytest
+
+import state_representation as sr
+
+
+def assert_list_equal(value, expected_value):
+    assert len(value) == len(expected_value)
+    assert all([a == b for a, b in zip(value, expected_value)])
+
+
+io_states = [(sr.DigitalIOState, "io", ["1", "2"], [True, False], sr.StateType.DIGITAL_IO_STATE),
+             (sr.AnalogIOState, "io", ["1", "2"], [0.5, 1.1], sr.StateType.ANALOG_IO_STATE)]
+
+
+@pytest.mark.parametrize("class_type,name,io_names,values,state_type", io_states)
+def test_construction(class_type, name, io_names, values, state_type):
+    state = class_type(name, io_names)
+    assert state.get_name() == name
+    assert state.get_type() == state_type
+    assert state.is_empty()
+    assert not state
+
+    new_state = class_type(state)
+    assert new_state.get_type() == state_type
+    assert new_state.is_empty()
+
+    state.set_data(values)
+    assert state
+    assert not state.is_empty()
+    assert_list_equal(state.to_list(), values)
+
+    state.reset()
+    assert state.is_empty()

python/test/test_clproto.py

@@ -5,6 +5,8 @@
 import state_representation as sr
 
 states = [(sr.State("test"), clproto.MessageType.STATE_MESSAGE),
+          (sr.DigitalIOState("test"), clproto.MessageType.DIGITAL_IO_STATE_MESSAGE),
+          (sr.AnalogIOState("test"), clproto.MessageType.ANALOG_IO_STATE_MESSAGE),
           (sr.SpatialState("test", "ref"), clproto.MessageType.SPATIAL_STATE_MESSAGE),
           (sr.CartesianState("test", "ref"), clproto.MessageType.CARTESIAN_STATE_MESSAGE),
           (sr.CartesianState().Random("test"), clproto.MessageType.CARTESIAN_STATE_MESSAGE),

source/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 7.3.8)
+project(control_libraries VERSION 7.3.9)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)