feat(state-representation): add utilities for CartesianStateVariable (#…

…196)

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

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) {

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::InvalidStateVariableException>(m, "InvalidStateVariableError", 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);

source/state_representation/include/state_representation/exceptions/InvalidStateVariableException.hpp

@@ -0,0 +1,11 @@
+#pragma once
+
+#include <exception>
+#include <iostream>
+
+namespace state_representation::exceptions {
+class InvalidStateVariableException : public std::runtime_error {
+public:
+  explicit InvalidStateVariableException(const std::string& msg) : runtime_error(msg) {};
+};
+}// namespace state_representation::exceptions

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<double>& 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