Merge branch 'main' into feat/clamp_in_range

CHANGELOG.md

@@ -18,10 +18,10 @@ Release Versions
 
 ## Upcoming changes (in development)
 
-- feat(state-representation): add utilities for CartesianStateVariable (#195)
-- feat(state-representation): add utilities for JointStateVariable (#197)
+- feat(state-representation): add utilities for CartesianStateVariable (#195, #201)
+- feat(state-representation): add utilities for JointStateVariable (#197, #201)
 - feat(robot-model): add clamp_in_range function for individual JointStateVariable (#194)
-
+- 
 ## 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/test/state_representation/space/cartesian/test_cartesian_state.py

@@ -5,9 +5,10 @@
 from pyquaternion.quaternion import Quaternion
 from numpy.testing import assert_array_equal, assert_array_almost_equal
 from state_representation import State, CartesianState, StateType, CartesianStateVariable, CartesianPose, \
-    CartesianTwist, CartesianAcceleration, CartesianWrench
-from state_representation.exceptions import EmptyStateError, IncompatibleReferenceFramesError, IncompatibleSizeError, \
-    NotImplementedError
+    CartesianTwist, CartesianAcceleration, CartesianWrench, string_to_cartesian_state_variable, \
+    cartesian_state_variable_to_string
+from state_representation.exceptions import EmptyStateError, InvalidStateVariableError, IncompatibleReferenceFramesError, \
+    IncompatibleSizeError, NotImplementedError
 from datetime import timedelta
 
 from ..test_spatial_state import SPATIAL_STATE_METHOD_EXPECTS
@@ -63,7 +64,9 @@
     'set_twist',
     'set_wrench',
     'set_zero',
-    'to_list'
+    'to_list',
+    'get_state_variable',
+    'set_state_variable',
 ]
 
 
@@ -996,6 +999,25 @@ def test_multiplication_operators(self):
         with self.assertRaises(TypeError):
             wrench / timedelta(seconds=1)
 
+    def test_utilities(self):
+        state_variable_type = string_to_cartesian_state_variable("position")
+        self.assertIsInstance(state_variable_type, CartesianStateVariable)
+        self.assertEqual("position", cartesian_state_variable_to_string(state_variable_type))
+        with self.assertRaises(InvalidStateVariableError):
+            string_to_cartesian_state_variable("foo")
+
+        state = CartesianState()
+        with self.assertRaises(IncompatibleSizeError):
+            state.set_state_variable([1.0, 2.0, 3.0, 4.0], state_variable_type)
+        state.set_state_variable([1.0, 2.0, 3.0], state_variable_type)
+        self.assertTrue((state.get_state_variable(CartesianStateVariable.POSITION) == [1.0, 2.0, 3.0]).all())
+        self.assertTrue((state.get_state_variable(state_variable_type) == [1.0, 2.0, 3.0]).all())
+
+        state.set_state_variable([4.0, 5.0, 6.0], CartesianStateVariable.POSITION)
+        self.assertTrue((state.get_state_variable(CartesianStateVariable.POSITION) == [4.0, 5.0, 6.0]).all())
+
+        state.set_state_variable(np.array([7.0, 8.0, 9.0]), CartesianStateVariable.POSITION)
+        self.assertTrue((state.get_position() == [7.0, 8.0, 9.0]).all())
 
 if __name__ == '__main__':
     unittest.main()

python/test/state_representation/space/joint/test_joint_state.py

@@ -2,7 +2,9 @@
 import copy
 
 import numpy as np
-from state_representation import JointState, JointPositions, JointVelocities, JointAccelerations, JointTorques
+from state_representation import JointState, JointPositions, JointVelocities, JointAccelerations, JointTorques, \
+    JointStateVariable, string_to_joint_state_variable, joint_state_variable_to_string
+from state_representation.exceptions import InvalidStateVariableError, IncompatibleSizeError
 from datetime import timedelta
 
 JOINT_STATE_METHOD_EXPECTS = [
@@ -44,7 +46,10 @@
     'set_torques',
     'set_torque',
     'set_zero',
-    'to_list'
+    'to_list',
+    'multiply_state_variable',
+    'get_state_variable',
+    'set_state_variable'
 ]
 
 
@@ -465,6 +470,30 @@ def test_multiplication_operators(self):
         with self.assertRaises(TypeError):
             torques / timedelta(seconds=1)
 
+    def test_utilities(self):
+        state_variable_type = string_to_joint_state_variable("positions")
+        self.assertIsInstance(state_variable_type, JointStateVariable)
+        self.assertEqual("positions", joint_state_variable_to_string(state_variable_type))
+        with self.assertRaises(InvalidStateVariableError):
+            string_to_joint_state_variable("foo")
+
+        state = JointState("foo", 3)
+        with self.assertRaises(IncompatibleSizeError):
+            state.set_state_variable([1.0, 2.0, 3.0, 4.0], JointStateVariable.POSITIONS)
+        state.set_state_variable([1.0, 2.0, 3.0], JointStateVariable.POSITIONS)
+        self.assertTrue((state.get_state_variable(JointStateVariable.POSITIONS) == [1.0, 2.0, 3.0]).all())
+        self.assertTrue((state.get_state_variable(state_variable_type) == [1.0, 2.0, 3.0]).all())
+
+        matrix = np.random.rand(3, 3)
+        expected = matrix @ np.array([1.0, 2.0, 3.0])
+        state.multiply_state_variable(matrix, JointStateVariable.POSITIONS)
+        self.assertTrue((state.get_positions() == expected).all())
+
+        state.set_state_variable([4.0, 5.0, 6.0], JointStateVariable.POSITIONS)
+        self.assertTrue((state.get_state_variable(JointStateVariable.POSITIONS) == [4.0, 5.0, 6.0]).all())
+
+        state.set_state_variable(np.array([7.0, 8.0, 9.0]), JointStateVariable.POSITIONS)
+        self.assertTrue((state.get_positions() == [7.0, 8.0, 9.0]).all())
 
 if __name__ == '__main__':
     unittest.main()

source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp

@@ -642,6 +642,7 @@ inline state_representation::CartesianStateVariable string_to_cartesian_state_va
   } else {
     throw exceptions::InvalidStateVariableException("Invalid Cartesian state variable: " + variable);
   }
+  __builtin_unreachable();
 }
 
 /**
@@ -679,6 +680,7 @@ inline std::string cartesian_state_variable_to_string(const CartesianStateVariab
     case CartesianStateVariable::ALL:
       return "all";
   }
+  __builtin_unreachable();
 }
 
 }// namespace state_representation

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

@@ -606,6 +606,7 @@ inline state_representation::JointStateVariable string_to_joint_state_variable(c
   } else {
     throw exceptions::InvalidStateVariableException("Invalid joint state variable: " + variable);
   }
+  __builtin_unreachable();
 }
 
 /**
@@ -627,5 +628,6 @@ inline std::string joint_state_variable_to_string(const JointStateVariable& vari
     case JointStateVariable::ALL:
       return "all";
   }
+  __builtin_unreachable();
 }
 }// namespace state_representation

source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp

@@ -7,6 +7,7 @@
 #include "state_representation/space/cartesian/CartesianAcceleration.hpp"
 #include "state_representation/space/cartesian/CartesianWrench.hpp"
 #include "state_representation/exceptions/EmptyStateException.hpp"
+#include "state_representation/exceptions/InvalidStateVariableException.hpp"
 #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp"
 #include "state_representation/exceptions/NotImplementedException.hpp"
 
@@ -1083,3 +1084,24 @@ TEST(CartesianStateTest, TestSubtractionOperators) {
   //wrench -= twist;
   //wrench -= acc;
 }
+
+TEST(CartesianStateTest, TestUtilities) {
+  auto state_variable_type = string_to_cartesian_state_variable("position");
+  EXPECT_EQ(state_variable_type, CartesianStateVariable::POSITION);
+  EXPECT_EQ("position", cartesian_state_variable_to_string(CartesianStateVariable::POSITION));
+  EXPECT_THROW(string_to_cartesian_state_variable("foo"), exceptions::InvalidStateVariableException);
+
+  auto state = CartesianState();
+  auto new_values = Eigen::VectorXd(4);
+  new_values << 1.0, 2.0, 3.0, 4.0;
+  EXPECT_THROW(state.set_state_variable(new_values, state_variable_type), exceptions::IncompatibleSizeException);
+  new_values = Eigen::VectorXd(3);
+  new_values << 1.0, 2.0, 3.0;
+  state.set_state_variable(new_values, state_variable_type);
+  EXPECT_TRUE(state.get_state_variable(CartesianStateVariable::POSITION).cwiseEqual(new_values).all());
+  EXPECT_TRUE(state.get_state_variable(state_variable_type).cwiseEqual(new_values).all());
+
+  new_values << 4.0, 5.0, 6.0;
+  state.set_state_variable(new_values, CartesianStateVariable::POSITION);
+  EXPECT_TRUE(state.get_position().cwiseEqual(new_values).all());
+}

source/state_representation/test/tests/space/joint/test_joint_state.cpp

@@ -9,6 +9,7 @@
 #include "state_representation/exceptions/IncompatibleStatesException.hpp"
 #include "state_representation/exceptions/JointNotFoundException.hpp"
 #include "state_representation/exceptions/EmptyStateException.hpp"
+#include "state_representation/exceptions/InvalidStateVariableException.hpp"
 
 using namespace state_representation;
 
@@ -599,4 +600,30 @@ TEST(JointStateTest, TestSubtractionOperators) {
   //torques -= positions;
   //torques -= velocities;
   //torques -= accelerations;
-}
+}
+
+TEST(JointStateTest, TestUtilities) {
+  auto state_variable_type = string_to_joint_state_variable("positions");
+  EXPECT_EQ(state_variable_type, JointStateVariable::POSITIONS);
+  EXPECT_EQ("positions", joint_state_variable_to_string(JointStateVariable::POSITIONS));
+  EXPECT_THROW(string_to_joint_state_variable("foo"), exceptions::InvalidStateVariableException);
+
+  auto state = JointState("foo", 3);
+  auto new_values = Eigen::VectorXd(4);
+  EXPECT_THROW(
+      state.set_state_variable(new_values, JointStateVariable::POSITIONS), exceptions::IncompatibleSizeException);
+  new_values = Eigen::VectorXd(3);
+  new_values << 1.0, 2.0, 3.0;
+  state.set_state_variable(new_values, JointStateVariable::POSITIONS);
+  EXPECT_TRUE(state.get_state_variable(JointStateVariable::POSITIONS).cwiseEqual(new_values).all());
+  EXPECT_TRUE(state.get_state_variable(state_variable_type).cwiseEqual(new_values).all());
+
+  Eigen::MatrixXd matrix = Eigen::MatrixXd::Random(3, 3);
+  auto expected = matrix * new_values;
+  state.multiply_state_variable(matrix, JointStateVariable::POSITIONS);
+  EXPECT_TRUE((expected.array() == state.get_positions().array()).all());
+
+  new_values << 4.0, 5.0, 6.0;
+  state.set_state_variable(new_values, JointStateVariable::POSITIONS);
+  EXPECT_TRUE(state.get_positions().cwiseEqual(new_values).all());
+}