diff --git a/CHANGELOG.md b/CHANGELOG.md index b5a7a4da6..f75c698e6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,11 @@ Release Versions - [6.3.0](#630) - [6.2.0](#620) + +## Upcoming changes (in development) + +- feat(state-representation): add utilities for CartesianStateVariable (#196) + ## 9.0.0 Version 9.0.0 is a new major version of control libraries that is built on Ubuntu 24.04 with Python 3.12. It does not diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp index ae6c79840..3b5bdfcf7 100644 --- a/python/source/state_representation/bind_cartesian_space.cpp +++ b/python/source/state_representation/bind_cartesian_space.cpp @@ -48,6 +48,9 @@ void cartesian_state_variable(py::module_& m) { .value("WRENCH", CartesianStateVariable::WRENCH) .value("ALL", CartesianStateVariable::ALL) .export_values(); + + m.def("string_to_cartesian_state_variable", &state_representation::string_to_cartesian_state_variable, "Convert a string to a CartesianStateVariable enum (case insensitive)", "variable"_a); + m.def("cartesian_state_variable_to_string", &state_representation::cartesian_state_variable_to_string, "Convert CartesianStateVariable to a string", "variable"_a); } void cartesian_state(py::module_& m) { @@ -173,6 +176,10 @@ void cartesian_state(py::module_& m) { buffer << state; return buffer.str(); }); + + c.def("get_state_variable", &CartesianState::get_state_variable, "Getter of the variable value corresponding to the input", "state_variable_type"_a); + c.def("set_state_variable", py::overload_cast(&CartesianState::set_state_variable), "Setter of the variable value corresponding to the input", "new_value"_a, "state_variable_type"_a); + c.def("set_state_variable", py::overload_cast&, const CartesianStateVariable&>(&CartesianState::set_state_variable), "Setter of the variable value corresponding to the input", "new_value"_a, "state_variable_type"_a); } void cartesian_pose(py::module_& m) { diff --git a/python/source/state_representation/bind_exceptions.cpp b/python/source/state_representation/bind_exceptions.cpp index 5fd60c2ee..62d5e6e22 100644 --- a/python/source/state_representation/bind_exceptions.cpp +++ b/python/source/state_representation/bind_exceptions.cpp @@ -1,6 +1,7 @@ #include "state_representation_bindings.hpp" #include +#include #include #include #include @@ -11,6 +12,7 @@ void bind_exceptions(py::module_& m) { py::register_exception(m, "EmptyStateError", PyExc_RuntimeError); + py::register_exception(m, "InvalidStateVariableError", PyExc_RuntimeError); py::register_exception(m, "IncompatibleReferenceFramesError", PyExc_RuntimeError); py::register_exception(m, "IncompatibleSizeError", PyExc_RuntimeError); py::register_exception(m, "IncompatibleStatesError", PyExc_RuntimeError); diff --git a/source/state_representation/include/state_representation/exceptions/InvalidStateVariableException.hpp b/source/state_representation/include/state_representation/exceptions/InvalidStateVariableException.hpp new file mode 100644 index 000000000..299cfc690 --- /dev/null +++ b/source/state_representation/include/state_representation/exceptions/InvalidStateVariableException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace state_representation::exceptions { +class InvalidStateVariableException : public std::runtime_error { +public: + explicit InvalidStateVariableException(const std::string& msg) : runtime_error(msg) {}; +}; +}// namespace state_representation::exceptions diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp index b23738910..8148db007 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -2,6 +2,7 @@ #include "state_representation/space/SpatialState.hpp" #include "state_representation/exceptions/IncompatibleSizeException.hpp" +#include "state_representation/exceptions/InvalidStateVariableException.hpp" namespace state_representation { @@ -550,7 +551,6 @@ class CartesianState : public SpatialState { */ friend std::ostream& operator<<(std::ostream& os, const CartesianState& state); -protected: /** * @brief Getter of the variable value corresponding to the input * @param state_variable_type The type of variable to get @@ -571,6 +571,7 @@ class CartesianState : public SpatialState { */ void set_state_variable(const std::vector& new_value, const CartesianStateVariable& state_variable_type); +protected: /** * @copydoc SpatialState::to_string */ @@ -598,4 +599,86 @@ inline void swap(CartesianState& state1, CartesianState& state2) { std::swap(state1.force_, state2.force_); std::swap(state1.torque_, state2.torque_); } + +/** + * @brief Convert a string to a CartesianStateVariable enum (case insensitive) + * @throws exceptions::InvalidStateVariableException + * @param variable The string to convert + * @return A CartesianStateVariable enum corresponding to the input string + */ +inline state_representation::CartesianStateVariable string_to_cartesian_state_variable(const std::string& variable) { + std::string case_insensitive_variable; + std::transform(variable.begin(), variable.end(), std::back_inserter(case_insensitive_variable), [](unsigned char c) { + return std::tolower(c); + }); + if (case_insensitive_variable == "position") { + return CartesianStateVariable::POSITION; + } else if (case_insensitive_variable == "orientation") { + return CartesianStateVariable::ORIENTATION; + } else if (case_insensitive_variable == "pose") { + return CartesianStateVariable::POSE; + } else if (case_insensitive_variable == "linear_velocity") { + return CartesianStateVariable::LINEAR_VELOCITY; + } else if (case_insensitive_variable == "angular_velocity") { + return CartesianStateVariable::ANGULAR_VELOCITY; + } else if (case_insensitive_variable == "twist") { + return CartesianStateVariable::TWIST; + } else if (case_insensitive_variable == "linear_acceleration") { + return CartesianStateVariable::LINEAR_ACCELERATION; + } else if (case_insensitive_variable == "angular_acceleration") { + return CartesianStateVariable::ANGULAR_ACCELERATION; + } else if (case_insensitive_variable == "acceleration") { + return CartesianStateVariable::ACCELERATION; + } else if (case_insensitive_variable == "force") { + return CartesianStateVariable::FORCE; + } else if (case_insensitive_variable == "torque") { + return CartesianStateVariable::TORQUE; + } else if (case_insensitive_variable == "twist") { + return CartesianStateVariable::TWIST; + } else if (case_insensitive_variable == "wrench") { + return CartesianStateVariable::WRENCH; + } else if (case_insensitive_variable == "all") { + return CartesianStateVariable::ALL; + } else { + throw exceptions::InvalidStateVariableException("Invalid Cartesian state variable: " + variable); + } +} + +/** + * @brief Convert CartesianStateVariable to a string + * @throws exceptions::InvalidStateVariableException + * @param variable The CartesianStateVariable enum to convert + * @return A string corresponding to the CartesianStateVariable enum + */ +inline std::string cartesian_state_variable_to_string(const CartesianStateVariable& variable) { + switch (variable) { + case CartesianStateVariable::POSITION: + return "position"; + case CartesianStateVariable::ORIENTATION: + return "orientation"; + case CartesianStateVariable::POSE: + return "pose"; + case CartesianStateVariable::LINEAR_VELOCITY: + return "linear_velocity"; + case CartesianStateVariable::ANGULAR_VELOCITY: + return "angular_velocity"; + case CartesianStateVariable::TWIST: + return "twist"; + case CartesianStateVariable::LINEAR_ACCELERATION: + return "linear_acceleration"; + case CartesianStateVariable::ANGULAR_ACCELERATION: + return "angular_acceleration"; + case CartesianStateVariable::ACCELERATION: + return "acceleration"; + case CartesianStateVariable::FORCE: + return "force"; + case CartesianStateVariable::TORQUE: + return "torque"; + case CartesianStateVariable::WRENCH: + return "wrench"; + case CartesianStateVariable::ALL: + return "all"; + } +} + }// namespace state_representation