Get joint state by name or index of joint

CHANGELOG.md

@@ -13,6 +13,8 @@ Release Versions:
 - JSON conversion support for clproto (#205)
 - Support empty state objects in clproto (#207)
 - Add int and int array parameters (#208)
+- Add method to get a joint state by name or index of the joint,
+  and to get the index of the joint by its name (#210)
 
 ## 4.0.0
 

python/source/state_representation/bind_joint_space.cpp

@@ -17,45 +17,54 @@ void joint_state_variable(py::module_& m) {
 }
 
 void joint_state(py::module_& m) {
-  m.def("dist", py::overload_cast<const JointState&, const JointState&, const JointStateVariable&>(&state_representation::dist), "Compute the distance between two JointStates", "s1"_a, "s2"_a, "state_variable_type"_a=JointStateVariable::ALL);
+  m.def("dist", py::overload_cast<const JointState&, const JointState&, const JointStateVariable&>(&state_representation::dist), "Compute the distance between two JointState.", "s1"_a, "s2"_a, "state_variable_type"_a=JointStateVariable::ALL);
 
   py::class_<JointState, State> c(m, "JointState");
-  c.def(py::init(), "Empty constructor");
-  c.def(py::init<const std::string&, unsigned int>(), "Constructor with name and number of joints provided", "robot_name"_a, "nb_joints"_a=0);
-  c.def(py::init<const std::string&, const std::vector<std::string>&>(), "Constructor with name and list of joint names provided", "robot_name"_a, "joint_names"_a);
-  c.def(py::init<const JointState&>(), "Copy constructor of a JointState", "state"_a);
-
-  c.def_static("Zero", py::overload_cast<const std::string&, unsigned int>(&JointState::Zero), "Constructor for the zero JointState", "robot_name"_a, "nb_joints"_a);
-  c.def_static("Zero", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Zero), "Constructor for the zero JointState", "robot_name"_a, "joint_names"_a);
-  c.def_static("Random", py::overload_cast<const std::string&, unsigned int>(&JointState::Random), "Constructor for the random JointState", "robot_name"_a, "nb_joints"_a);
-  c.def_static("Random", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Random), "Constructor for the random JointState", "robot_name"_a, "joint_names"_a);
-
-  c.def("get_size", &JointState::get_size, "Getter of the size from the attributes");
-  c.def("get_names", &JointState::get_names, "Getter of the names attribute");
-  c.def("get_positions", &JointState::get_positions, "Getter of the positions attribute");
+  c.def(py::init(), "Empty constructor for a JointState.");
+  c.def(py::init<const std::string&, unsigned int>(), "Constructor with name and number of joints provided.", "robot_name"_a, "nb_joints"_a=0);
+  c.def(py::init<const std::string&, const std::vector<std::string>&>(), "Constructor with name and list of joint names provided.", "robot_name"_a, "joint_names"_a);
+  c.def(py::init<const JointState&>(), "Copy constructor of a JointState.", "state"_a);
+
+  c.def_static("Zero", py::overload_cast<const std::string&, unsigned int>(&JointState::Zero), "Constructor for a zero JointState.", "robot_name"_a, "nb_joints"_a);
+  c.def_static("Zero", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Zero), "Constructor for a zero JointState.", "robot_name"_a, "joint_names"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, unsigned int>(&JointState::Random), "Constructor for a random JointState.", "robot_name"_a, "nb_joints"_a);
+  c.def_static("Random", py::overload_cast<const std::string&, const std::vector<std::string>&>(&JointState::Random), "Constructor for a random JointState.", "robot_name"_a, "joint_names"_a);
+
+  c.def("get_size", &JointState::get_size, "Getter of the size from the attributes.");
+  c.def("get_names", &JointState::get_names, "Getter of the names attribute.");
+  c.def("get_joint_index", &JointState::get_joint_index, "Get joint index by the name of the joint, if it exists.", "joint_name"_a);
+  c.def("get_positions", &JointState::get_positions, "Getter of the positions attribute.");
+  c.def("get_position", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_position(joint_name); }, "Get the position of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_position", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_position(joint_index); }, "Get the position of a joint by its name, if it exists.", "joint_index"_a);
   c.def("get_velocities", &JointState::get_velocities, "Getter of the velocities attribute");
+  c.def("get_velocity", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_velocity(joint_name); }, "Get the velocity of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_velocity", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_velocity(joint_index); }, "Get the velocity of a joint by its name, if it exists.", "joint_index"_a);
   c.def("get_accelerations", &JointState::get_accelerations, "Getter of the accelerations attribute");
+  c.def("get_acceleration", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_acceleration(joint_name); }, "Get the acceleration of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_acceleration", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_acceleration(joint_index); }, "Get the acceleration of a joint by its name, if it exists.", "joint_index"_a);
   c.def("get_torques", &JointState::get_torques, "Getter of the torques attribute");
-
-  c.def("set_names", py::overload_cast<unsigned int>(&JointState::set_names), "Setter of the names attribute from the number of joints");
-  c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&JointState::set_names), "Setter of the names attribute");
-  c.def("set_positions", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_positions), "Setter of the positions attribute from a vector");
-  c.def("set_positions", py::overload_cast<const std::vector<double>&>(&JointState::set_positions), "Setter of the positions attribute from a list");
-  c.def("set_velocities", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_velocities), "Setter of the velocities attribute from a vector");
-  c.def("set_velocities", py::overload_cast<const std::vector<double>&>(&JointState::set_velocities), "Setter of the velocities attribute from a list");
-  c.def("set_accelerations", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a vector");
-  c.def("set_accelerations", py::overload_cast<const std::vector<double>&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a list");
-  c.def("set_torques", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_torques), "Setter of the torques attribute from a vector");
-  c.def("set_torques", py::overload_cast<const std::vector<double>&>(&JointState::set_torques), "Setter of the torques attribute from a list");
-
-  c.def("set_zero", &JointState::set_zero, "Set the JointState to a zero value");
-  c.def("clamp_state_variable", py::overload_cast<double, const JointStateVariable&, double>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable (velocities, accelerations or forces)", "value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0));
-  c.def("clamp_state_variable", py::overload_cast<const Eigen::ArrayXd&, const JointStateVariable&, const Eigen::ArrayXd&>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable (velocities, accelerations or forces)", "max_absolute_value_array"_a, "state_variable_type"_a, "noise_ratio_array"_a);
-  c.def("copy", &JointState::copy, "Return a copy of the JointState");
-  c.def("data", &JointState::data, "Returns the data as the concatenation of all the state variables in a single vector");
-  c.def("array", &JointState::array, "Returns the data vector as an array");
-  c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_data), "Set the data of the state from all the state variables in a single vector", "data"_a);
-  c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointState::set_data), "Set the data of the state from all the state variables in a single list", "data"_a);
+  c.def("get_torque", [](const JointState& joint_state, const std::string& joint_name) { return joint_state.get_torque(joint_name); }, "Get the torque of a joint by its name, if it exists.", "joint_name"_a);
+  c.def("get_torque", [](const JointState& joint_state, unsigned int joint_index) { return joint_state.get_torque(joint_index); }, "Get the torque of a joint by its name, if it exists.", "joint_index"_a);
+
+  c.def("set_names", py::overload_cast<unsigned int>(&JointState::set_names), "Setter of the names attribute from the number of joints.", "nb_joints"_a);
+  c.def("set_names", py::overload_cast<const std::vector<std::string>&>(&JointState::set_names), "Setter of the names attribute.", "names"_a);
+  c.def("set_positions", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_positions), "Setter of the positions attribute from a vector.", "positions"_a);
+  c.def("set_positions", py::overload_cast<const std::vector<double>&>(&JointState::set_positions), "Setter of the positions attribute from a list.", "positions"_a);
+  c.def("set_velocities", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_velocities), "Setter of the velocities attribute from a vector.", "velocities"_a);
+  c.def("set_velocities", py::overload_cast<const std::vector<double>&>(&JointState::set_velocities), "Setter of the velocities attribute from a list.", "velocities"_a);
+  c.def("set_accelerations", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a vector.", "accelerations"_a);
+  c.def("set_accelerations", py::overload_cast<const std::vector<double>&>(&JointState::set_accelerations), "Setter of the accelerations attribute from a list.", "accelerations"_a);
+  c.def("set_torques", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_torques), "Setter of the torques attribute from a vector.", "torques"_a);
+  c.def("set_torques", py::overload_cast<const std::vector<double>&>(&JointState::set_torques), "Setter of the torques attribute from a list.", "torques"_a);
+
+  c.def("set_zero", &JointState::set_zero, "Set the JointState to a zero value.");
+  c.def("clamp_state_variable", py::overload_cast<double, const JointStateVariable&, double>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable.", "value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0));
+  c.def("clamp_state_variable", py::overload_cast<const Eigen::ArrayXd&, const JointStateVariable&, const Eigen::ArrayXd&>(&JointState::clamp_state_variable), "Clamp inplace the magnitude of the a specific state variable.", "max_absolute_value_array"_a, "state_variable_type"_a, "noise_ratio_array"_a);
+  c.def("copy", &JointState::copy, "Return a copy of the JointState.");
+  c.def("data", &JointState::data, "Returns the data as the concatenation of all the state variables in a single vector.");
+  c.def("array", &JointState::array, "Returns the data vector as an array.");
+  c.def("set_data", py::overload_cast<const Eigen::VectorXd&>(&JointState::set_data), "Set the data of the state from all the state variables in a single vector.", "data"_a);
+  c.def("set_data", py::overload_cast<const std::vector<double>&>(&JointState::set_data), "Set the data of the state from all the state variables in a single list.", "data"_a);
 
   c.def(py::self += py::self);
   c.def(py::self + py::self);
@@ -73,9 +82,9 @@ void joint_state(py::module_& m) {
   c.def(Eigen::ArrayXd() * py::self);
   c.def(Eigen::MatrixXd() * py::self);
 
-  c.def("dist", &JointState::dist, "Compute the distance to another state as the sum of distances between each features", "state"_a, "state_variable_type"_a=JointStateVariable::ALL);
+  c.def("dist", &JointState::dist, "Compute the distance to another state as the sum of distances between each attribute.", "state"_a, "state_variable_type"_a=JointStateVariable::ALL);
 
-  c.def("to_list", &JointState::to_std_vector, "Return the state as a list");
+  c.def("to_list", &JointState::to_std_vector, "Return the state as a list.");
 
   c.def("__repr__", [](const JointState& state) {
     std::stringstream buffer;
@@ -103,8 +112,11 @@ void joint_positions(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "velocities",
+      "velocity",
       "accelerations",
+      "acceleration",
       "torques",
+      "torque",
   };
 
   for (const std::string& attr : deleted_attributes) {
@@ -161,8 +173,11 @@ void joint_velocities(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "positions",
+      "position",
       "accelerations",
+      "acceleration",
       "torques",
+      "torque",
   };
 
   for (const std::string& attr : deleted_attributes) {
@@ -224,8 +239,11 @@ void joint_accelerations(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "positions",
+      "position",
       "velocities",
+      "velocity",
       "torques",
+      "torque",
   };
 
   for (const std::string& attr : deleted_attributes) {
@@ -285,8 +303,11 @@ void joint_torques(py::module_& m) {
 
   std::vector<std::string> deleted_attributes = {
       "positions",
+      "position",
       "velocities",
+      "velocity",
       "accelerations",
+      "acceleration",
   };
 
   for (const std::string& attr : deleted_attributes) {

python/test/test_joint_state.py

@@ -11,14 +11,19 @@
     'set_data',
     'dist',
     'get_accelerations',
+    'get_acceleration',
+    'get_joint_index',
     'get_name',
     'get_names',
     'get_positions',
+    'get_position',
     'get_size',
     'get_timestamp',
     'get_torques',
+    'get_torque',
     'get_type',
     'get_velocities',
+    'get_velocity',
     'initialize',
     'is_compatible',
     'is_deprecated',

source/state_representation/include/state_representation/exceptions/JointNotFoundException.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include <exception>
+#include <iostream>
+
+namespace state_representation::exceptions {
+
+/**
+ * @class JointNotFoundException
+ * @brief Exception that is thrown when a joint name or index is out of range
+ */
+class JointNotFoundException : public std::logic_error {
+public:
+  explicit JointNotFoundException(const std::string& msg) : logic_error(msg) {};
+};
+}// namespace state_representation::exceptions

source/state_representation/include/state_representation/robot/JointAccelerations.hpp

@@ -16,12 +16,18 @@ class JointAccelerations : public JointState {
 
 public:
   const Eigen::VectorXd& get_positions() const = delete;
+  double get_position(unsigned int joint_index) const = delete;
+  double get_position(const std::string& joint_name) const = delete;
   void set_positions(const Eigen::VectorXd& positions) = delete;
   void set_positions(const std::vector<double>& positions) = delete;
   const Eigen::VectorXd& get_velocities() const = delete;
+  double get_velocity(unsigned int joint_index) const = delete;
+  double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& accelerations) = delete;
   void set_velocities(const std::vector<double>& accelerations) = delete;
   const Eigen::VectorXd& get_torques() const = delete;
+  double get_torque(unsigned int joint_index) const = delete;
+  double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;
 

source/state_representation/include/state_representation/robot/JointPositions.hpp

@@ -16,12 +16,18 @@ class JointPositions : public JointState {
 
 public:
   const Eigen::VectorXd& get_velocities() const = delete;
+  double get_velocity(unsigned int joint_index) const = delete;
+  double get_velocity(const std::string& joint_name) const = delete;
   void set_velocities(const Eigen::VectorXd& velocities) = delete;
   void set_velocities(const std::vector<double>& velocities) = delete;
   const Eigen::VectorXd& get_accelerations() const = delete;
+  double get_acceleration(unsigned int joint_index) const = delete;
+  double get_acceleration(const std::string& joint_name) const = delete;
   void set_accelerations(const Eigen::VectorXd& accelerations) = delete;
   void set_accelerations(const std::vector<double>& accelerations) = delete;
   const Eigen::VectorXd& get_torques() const = delete;
+  double get_torque(unsigned int joint_index) const = delete;
+  double get_torque(const std::string& joint_name) const = delete;
   void set_torques(const Eigen::VectorXd& torques) = delete;
   void set_torques(const std::vector<double>& torques) = delete;