From 504922a87a43a680f0abc83facaf6f43d503f20d Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Tue, 2 Apr 2024 09:55:44 +0200 Subject: [PATCH] feat: add IO states to state representation (py) (#173) --- CHANGELOG.md | 1 + VERSION | 2 +- demos/CMakeLists.txt | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- .../include/state_representation_bindings.hpp | 1 + python/setup.py | 2 +- python/source/clproto/bind_clproto.cpp | 12 ++ .../state_representation/bind_io_state.cpp | 111 ++++++++++++++++++ .../state_representation/bind_state.cpp | 2 + .../state_representation_bindings.cpp | 1 + .../state_representation/test_io_state.py | 33 ++++++ python/test/test_clproto.py | 2 + source/CMakeLists.txt | 2 +- 14 files changed, 169 insertions(+), 6 deletions(-) create mode 100644 python/source/state_representation/bind_io_state.cpp create mode 100644 python/test/state_representation/test_io_state.py diff --git a/CHANGELOG.md b/CHANGELOG.md index 411d520fc..5ea46a918 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/VERSION b/VERSION index 8837f2cb1..7dc1035aa 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -7.3.8 +7.3.9 diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 963b9f409..a87fd1bcb 100644 --- a/demos/CMakeLists.txt +++ b/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 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 2400bbe2c..eb02462a5 100644 --- a/doxygen/doxygen.conf +++ b/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 diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index 27dee75fe..16ef7c472 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/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) diff --git a/python/include/state_representation_bindings.hpp b/python/include/state_representation_bindings.hpp index 616490323..c5f0c93e1 100644 --- a/python/include/state_representation_bindings.hpp +++ b/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); diff --git a/python/setup.py b/python/setup.py index 24074b80d..2f38acf7f 100644 --- a/python/setup.py +++ b/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'] diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp index 934f803c1..09cc96345 100644 --- a/python/source/clproto/bind_clproto.cpp +++ b/python/source/clproto/bind_clproto.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include 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(object.cast()); + case MessageType::DIGITAL_IO_STATE_MESSAGE: + return encode_bytes(object.cast()); + case MessageType::ANALOG_IO_STATE_MESSAGE: + return encode_bytes(object.cast()); case MessageType::SPATIAL_STATE_MESSAGE: return encode_bytes(object.cast()); 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(msg)); + case MessageType::DIGITAL_IO_STATE_MESSAGE: + return py::cast(decode(msg)); + case MessageType::ANALOG_IO_STATE_MESSAGE: + return py::cast(decode(msg)); case MessageType::SPATIAL_STATE_MESSAGE: return py::cast(decode(msg)); case MessageType::CARTESIAN_STATE_MESSAGE: diff --git a/python/source/state_representation/bind_io_state.cpp b/python/source/state_representation/bind_io_state.cpp new file mode 100644 index 000000000..8c83ea90d --- /dev/null +++ b/python/source/state_representation/bind_io_state.cpp @@ -0,0 +1,111 @@ +#include "state_representation_bindings.hpp" + +#include +#include + + +void digital_io_state(py::module_& m) { + py::class_, State> c(m, "DigitalIOState"); + + c.def(py::init(), "Empty constructor for an digital IO state"); + c.def(py::init(), "Constructor with name and number of digital IOs provided", "name"_a, "nb_ios"_a=0); + c.def(py::init&>(), "Constructor with name and list of digital IO names provided", "name"_a, "io_names"_a); + c.def(py::init(), "Copy constructor of an digital IO state", "state"_a); + + c.def_static("Zero", py::overload_cast(&DigitalIOState::Zero), "Constructor for a zero digital IO state", "name"_a, "nb_ios"_a); + c.def_static("Zero", py::overload_cast&>(&DigitalIOState::Zero), "Constructor for a zero digital IO state", "name"_a, "io_names"_a); + c.def_static("Random", py::overload_cast(&DigitalIOState::Random), "Constructor for a random digital IO state", "name"_a, "nb_ios"_a); + c.def_static("Random", py::overload_cast&>(&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(&DigitalIOState::set_names), "Setter of the names from the number of IOs", "nb_ios"_a); + c.def("set_names", py::overload_cast&>(&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(&DigitalIOState::set_value), "Set the value of a digital IO by its name", "value"_a, "name"_a); + c.def("set_value", py::overload_cast(&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(&DigitalIOState::set_true), "Set the a digital IO to true by its name", "name"_a); + c.def("set_true", py::overload_cast(&DigitalIOState::set_true), "Set the a digital IO to true by its index", "io_index"_a); + c.def("set_false", py::overload_cast(&DigitalIOState::set_false), "Set the a digital IO to false by its name", "name"_a); + c.def("set_false", py::overload_cast(&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&>(&DigitalIOState::set_data), "Set the values of the IO state from a single Eigen vector", "data"_a); + c.def("set_data", py::overload_cast&>(&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_, State> c(m, "AnalogIOState"); + + c.def(py::init(), "Empty constructor for an analog IO state"); + c.def(py::init(), "Constructor with name and number of analog IOs provided", "name"_a, "nb_ios"_a=0); + c.def(py::init&>(), "onstructor with name and list of analog IO names provided", "name"_a, "io_names"_a); + c.def(py::init(), "Copy constructor of an analog IO state", "state"_a); + + c.def_static("Zero", py::overload_cast(&AnalogIOState::Zero), "Constructor for a zero analog IO state", "name"_a, "nb_ios"_a); + c.def_static("Zero", py::overload_cast&>(&AnalogIOState::Zero), "Constructor for a zero analog IO state", "name"_a, "io_names"_a); + c.def_static("Random", py::overload_cast(&AnalogIOState::Random), "Constructor for a random analog IO state", "name"_a, "nb_ios"_a); + c.def_static("Random", py::overload_cast&>(&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(&AnalogIOState::set_names), "Setter of the names from the number of IOs", "nb_ios"_a); + c.def("set_names", py::overload_cast&>(&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(&AnalogIOState::set_value), "Set the value of an analog IO by its name", "value"_a, "name"_a); + c.def("set_value", py::overload_cast(&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(&AnalogIOState::set_data), "Set the values of the IO state from a single Eigen vector", "data"_a); + c.def("set_data", py::overload_cast&>(&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); +} \ No newline at end of file diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp index 4ae548449..904075c9f 100644 --- a/python/source/state_representation/bind_state.cpp +++ b/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(); } diff --git a/python/source/state_representation/state_representation_bindings.cpp b/python/source/state_representation/state_representation_bindings.cpp index 52bb6eb2f..f56d97c46 100644 --- a/python/source/state_representation/state_representation_bindings.cpp +++ b/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); } diff --git a/python/test/state_representation/test_io_state.py b/python/test/state_representation/test_io_state.py new file mode 100644 index 000000000..86d097649 --- /dev/null +++ b/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() diff --git a/python/test/test_clproto.py b/python/test/test_clproto.py index 21b413ce8..7bd9a0611 100755 --- a/python/test/test_clproto.py +++ b/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), diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 0dd5fd5ec..4385b4287 100644 --- a/source/CMakeLists.txt +++ b/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)