feat(state-representation): add utilities for JointStateVariable (#197)

CHANGELOG.md

@@ -18,7 +18,8 @@ Release Versions
 
 ## Upcoming changes (in development)
 
-- feat(state-representation): add utilities for CartesianStateVariable (#196)
+- feat(state-representation): add utilities for CartesianStateVariable (#195)
+- feat(state-representation): add utilities for JointStateVariable (#197)
 
 ## 9.0.0
 

python/source/state_representation/bind_joint_space.cpp

@@ -14,6 +14,9 @@ void joint_state_variable(py::module_& m) {
       .value("TORQUES", JointStateVariable::TORQUES)
       .value("ALL", JointStateVariable::ALL)
       .export_values();
+
+  m.def("string_to_joint_state_variable", &state_representation::string_to_joint_state_variable, "Convert a string to a JointStateVariable enum (case insensitive)", "variable"_a);
+  m.def("joint_state_variable_to_string", &state_representation::joint_state_variable_to_string, "Convert JointStateVariable to a string", "variable"_a);
 }
 
 void joint_state(py::module_& m) {
@@ -105,6 +108,12 @@ void joint_state(py::module_& m) {
     buffer << state;
     return buffer.str();
   });
+
+  c.def("multiply_state_variable", py::overload_cast<const Eigen::MatrixXd&, const JointStateVariable&>(&JointState::multiply_state_variable), "Proxy function that scale the specified state variable by a matrix", "matrix"_a, "state_variable_type"_a);
+  c.def("get_state_variable", &JointState::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 JointStateVariable&>(&JointState::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 JointStateVariable&>(&JointState::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<double, unsigned int, const JointStateVariable&>(&JointState::set_state_variable), "Setter of the variable value corresponding to the input", "new_value"_a, "joint_index"_a, "state_variable_type"_a);
 }
 
 void joint_positions(py::module_& m) {

source/state_representation/include/state_representation/space/joint/JointState.hpp

@@ -2,6 +2,7 @@
 
 #include "state_representation/State.hpp"
 #include "state_representation/exceptions/IncompatibleSizeException.hpp"
+#include "state_representation/exceptions/InvalidStateVariableException.hpp"
 
 namespace state_representation {
 
@@ -521,7 +522,6 @@ class JointState : public State {
    */
   friend std::ostream& operator<<(std::ostream& os, const JointState& state);
 
-protected:
   /**
    * @brief Proxy function that scale the specified state variable by a matrix
    * @param lambda The scaling matrix
@@ -559,6 +559,7 @@ class JointState : public State {
    */
   void set_state_variable(double new_value, unsigned int joint_index, const JointStateVariable& state_variable_type);
 
+protected:
   /**
    * @copydoc State::to_string
    */
@@ -580,4 +581,51 @@ inline void swap(JointState& state1, JointState& state2) {
   std::swap(state1.accelerations_, state2.accelerations_);
   std::swap(state1.torques_, state2.torques_);
 }
+
+/**
+ * @brief Convert a string to a JointStateVariable enum (case insensitive)
+ * @throws exceptions::InvalidStateVariableException
+ * @param variable The string to convert
+ * @return A JointStateVariable enum corresponding to the input string
+ */
+inline state_representation::JointStateVariable string_to_joint_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 == "positions") {
+    return JointStateVariable::POSITIONS;
+  } else if (case_insensitive_variable == "velocities") {
+    return JointStateVariable::VELOCITIES;
+  } else if (case_insensitive_variable == "accelerations") {
+    return JointStateVariable::ACCELERATIONS;
+  } else if (case_insensitive_variable == "torques") {
+    return JointStateVariable::TORQUES;
+  } else if (case_insensitive_variable == "all") {
+    return JointStateVariable::ALL;
+  } else {
+    throw exceptions::InvalidStateVariableException("Invalid joint state variable: " + variable);
+  }
+}
+
+/**
+ * @brief Convert JointStateVariable to a string
+ * @throws exceptions::InvalidStateVariableException
+ * @param variable The JointStateVariable enum to convert
+ * @return A string corresponding to the JointStateVariable enum
+ */
+inline std::string joint_state_variable_to_string(const JointStateVariable& variable) {
+  switch (variable) {
+    case JointStateVariable::POSITIONS:
+      return "positions";
+    case JointStateVariable::VELOCITIES:
+      return "velocities";
+    case JointStateVariable::ACCELERATIONS:
+      return "accelerations";
+    case JointStateVariable::TORQUES:
+      return "torques";
+    case JointStateVariable::ALL:
+      return "all";
+  }
+}
 }// namespace state_representation