diff --git a/CHANGELOG.md b/CHANGELOG.md index 8885cb121..1cca0ca59 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,8 +18,8 @@ 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) ## 9.0.0 diff --git a/python/test/state_representation/space/cartesian/test_cartesian_state.py b/python/test/state_representation/space/cartesian/test_cartesian_state.py index 89fb8bc43..ddf6f684c 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/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() diff --git a/python/test/state_representation/space/joint/test_joint_state.py b/python/test/state_representation/space/joint/test_joint_state.py index 4a4ca375e..f895b5819 100755 --- a/python/test/state_representation/space/joint/test_joint_state.py +++ b/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() 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 8148db007..871ca6131 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/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 diff --git a/source/state_representation/include/state_representation/space/joint/JointState.hpp b/source/state_representation/include/state_representation/space/joint/JointState.hpp index 717b75d7b..9177b1f3a 100644 --- a/source/state_representation/include/state_representation/space/joint/JointState.hpp +++ b/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 diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp index 4c0260b85..a46091684 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_state.cpp +++ b/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()); +} diff --git a/source/state_representation/test/tests/space/joint/test_joint_state.cpp b/source/state_representation/test/tests/space/joint/test_joint_state.cpp index 0f882ea67..a6b6ba062 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_state.cpp +++ b/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; -} \ No newline at end of file +} + +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()); +}