From 5b91f1da28e4f02e321372134bb02d57c5334a69 Mon Sep 17 00:00:00 2001
From: bpapaspyros <b.papaspyros@gmail.com>
Date: Thu, 17 Oct 2024 10:49:51 +0200
Subject: [PATCH] feat: add python bindings for existing and new utilities

---
 .../source/state_representation/bind_cartesian_space.cpp   | 7 +++++++
 python/source/state_representation/bind_exceptions.cpp     | 2 ++
 .../space/cartesian/CartesianState.hpp                     | 4 ++--
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp
index ae6c79840..8f7322d0e 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<const Eigen::VectorXd&, const CartesianStateVariable&>(&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 std::vector<double>&, 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..10a51afe9 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 <state_representation/exceptions/EmptyStateException.hpp>
+#include <state_representation/exceptions/InvalidStateVariableException.hpp>
 #include <state_representation/exceptions/IncompatibleReferenceFramesException.hpp>
 #include <state_representation/exceptions/IncompatibleSizeException.hpp>
 #include <state_representation/exceptions/IncompatibleStatesException.hpp>
@@ -11,6 +12,7 @@
 
 void bind_exceptions(py::module_& m) {
   py::register_exception<exceptions::EmptyStateException>(m, "EmptyStateError", PyExc_RuntimeError);
+  py::register_exception<exceptions::EmptyStateException>(m, "InvalidStateVariableException", PyExc_RuntimeError);
   py::register_exception<exceptions::IncompatibleReferenceFramesException>(m, "IncompatibleReferenceFramesError", PyExc_RuntimeError);
   py::register_exception<exceptions::IncompatibleSizeException>(m, "IncompatibleSizeError", PyExc_RuntimeError);
   py::register_exception<exceptions::IncompatibleStatesException>(m, "IncompatibleStatesError", PyExc_RuntimeError);
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 8fdee131c..8148db007 100644
--- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp
+++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp
@@ -564,7 +564,6 @@ class CartesianState : public SpatialState {
    */
   void set_state_variable(const Eigen::VectorXd& new_value, const CartesianStateVariable& state_variable_type);
 
-protected:
   /**
    * @brief Setter of the variable value corresponding to the input
    * @param new_value The new value of the variable as std vector
@@ -572,6 +571,7 @@ class CartesianState : public SpatialState {
    */
   void set_state_variable(const std::vector<double>& new_value, const CartesianStateVariable& state_variable_type);
 
+protected:
   /**
    * @copydoc SpatialState::to_string
    */
@@ -650,7 +650,7 @@ inline state_representation::CartesianStateVariable string_to_cartesian_state_va
  * @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) {
+inline std::string cartesian_state_variable_to_string(const CartesianStateVariable& variable) {
   switch (variable) {
     case CartesianStateVariable::POSITION:
       return "position";