From 88f928788942a81de0830d2e21b841852b5c029a Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 18 Oct 2024 14:11:32 +0200 Subject: [PATCH 1/6] feat: add Python tests --- .../space/cartesian/test_cartesian_state.py | 28 ++++++++++++++++--- .../space/joint/test_joint_state.py | 26 +++++++++++++++-- 2 files changed, 48 insertions(+), 6 deletions(-) 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..df1f244da 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,23 @@ 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): + result = string_to_cartesian_state_variable("foo") + + state = CartesianState() + state.set_position([1.0, 2.0, 3.0]) + 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_state_variable(CartesianStateVariable.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..f0181fd1f 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 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,23 @@ 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): + result = string_to_joint_state_variable("foo") + + state = JointState("foo", 3) + state.set_positions([1.0, 2.0, 3.0]) + 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()) + + 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_state_variable(JointStateVariable.POSITIONS) == [7.0, 8.0, 9.0]).all()) if __name__ == '__main__': unittest.main() From dcfb29ac0ab925d2e602865454e8659cfc466edc Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 18 Oct 2024 14:49:06 +0200 Subject: [PATCH 2/6] feat: add C++ tests --- .../space/cartesian/CartesianState.hpp | 2 ++ .../space/joint/JointState.hpp | 2 ++ .../space/cartesian/test_cartesian_state.cpp | 18 ++++++++++++++++++ .../tests/space/joint/test_joint_state.cpp | 18 ++++++++++++++++++ 4 files changed, 40 insertions(+) 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..86c57d98f 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,20 @@ 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(); + state.set_position(std::vector{1.0, 2.0, 3.0}); + EXPECT_TRUE(state.get_position().cwiseEqual(state.get_state_variable(CartesianStateVariable::POSITION)).all()); + EXPECT_TRUE(state.get_position().cwiseEqual(state.get_state_variable(state_variable_type)).all()); + + Eigen::VectorXd new_values(3); + 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()); +} \ No newline at end of file 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..55e2fcd52 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,21 @@ 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); + state.set_positions(std::vector{1.0, 2.0, 3.0}); + EXPECT_TRUE(state.get_positions().cwiseEqual(state.get_state_variable(JointStateVariable::POSITIONS)).all()); + EXPECT_TRUE(state.get_positions().cwiseEqual(state.get_state_variable(state_variable_type)).all()); + + Eigen::VectorXd new_values(3); + 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()); } \ No newline at end of file From c51b74f7a7cffbcb61645d8a9d2dfbdf2ca8950c Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 18 Oct 2024 14:49:18 +0200 Subject: [PATCH 3/6] docs: update CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8885cb121..885345714 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ Release Versions - feat(state-representation): add utilities for CartesianStateVariable (#195) - feat(state-representation): add utilities for JointStateVariable (#197) +- test(state-representation): add utility tests for Cartesian/JointState (#201) ## 9.0.0 From 9b7ae7e065ae265645949c5b89d7ddd7fcd8b8b4 Mon Sep 17 00:00:00 2001 From: Vaios Papaspyros Date: Fri, 18 Oct 2024 17:06:53 +0200 Subject: [PATCH 4/6] Apply suggestions from code review Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com> --- CHANGELOG.md | 5 ++--- .../space/cartesian/test_cartesian_state.py | 2 +- .../state_representation/space/joint/test_joint_state.py | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 885345714..1cca0ca59 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,9 +18,8 @@ Release Versions ## Upcoming changes (in development) -- feat(state-representation): add utilities for CartesianStateVariable (#195) -- feat(state-representation): add utilities for JointStateVariable (#197) -- test(state-representation): add utility tests for Cartesian/JointState (#201) +- 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 df1f244da..9de89695e 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_state.py @@ -1015,7 +1015,7 @@ def test_utilities(self): 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_state_variable(CartesianStateVariable.POSITION) == [7.0, 8.0, 9.0]).all()) + 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 f0181fd1f..6c5231496 100755 --- a/python/test/state_representation/space/joint/test_joint_state.py +++ b/python/test/state_representation/space/joint/test_joint_state.py @@ -486,7 +486,7 @@ def test_utilities(self): 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_state_variable(JointStateVariable.POSITIONS) == [7.0, 8.0, 9.0]).all()) + self.assertTrue((state.get_positions() == [7.0, 8.0, 9.0]).all()) if __name__ == '__main__': unittest.main() From 633600b70005b3e0908387d1fad47da780c4227b Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Fri, 18 Oct 2024 17:31:41 +0200 Subject: [PATCH 5/6] fix: update tests --- .../tests/space/cartesian/test_cartesian_state.cpp | 11 ++++++----- .../test/tests/space/joint/test_joint_state.cpp | 11 ++++++----- 2 files changed, 12 insertions(+), 10 deletions(-) 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 86c57d98f..41b313ed1 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 @@ -1092,12 +1092,13 @@ TEST(CartesianStateTest, TestUtilities) { EXPECT_THROW(string_to_cartesian_state_variable("foo"), exceptions::InvalidStateVariableException); auto state = CartesianState(); - state.set_position(std::vector{1.0, 2.0, 3.0}); - EXPECT_TRUE(state.get_position().cwiseEqual(state.get_state_variable(CartesianStateVariable::POSITION)).all()); - EXPECT_TRUE(state.get_position().cwiseEqual(state.get_state_variable(state_variable_type)).all()); + auto new_values = Eigen::VectorXd(3); + new_values << 1.0, 2.0, 3.0; + state.set_position(new_values); + 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()); - Eigen::VectorXd new_values(3); 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()); -} \ No newline at end of file +} 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 55e2fcd52..24ebf5d31 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 @@ -609,12 +609,13 @@ TEST(JointStateTest, TestUtilities) { EXPECT_THROW(string_to_joint_state_variable("foo"), exceptions::InvalidStateVariableException); auto state = JointState("foo", 3); - state.set_positions(std::vector{1.0, 2.0, 3.0}); - EXPECT_TRUE(state.get_positions().cwiseEqual(state.get_state_variable(JointStateVariable::POSITIONS)).all()); - EXPECT_TRUE(state.get_positions().cwiseEqual(state.get_state_variable(state_variable_type)).all()); + auto new_values = Eigen::VectorXd(3); + new_values << 1.0, 2.0, 3.0; + state.set_positions(new_values); + 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::VectorXd new_values(3); 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()); -} \ No newline at end of file +} From 165a31e4d9fb67f8fdeeb8b56bed9caad646f5dc Mon Sep 17 00:00:00 2001 From: bpapaspyros Date: Mon, 21 Oct 2024 18:32:39 +0200 Subject: [PATCH 6/6] test: add multply_state_variable and some incompatible size tests --- .../space/cartesian/test_cartesian_state.py | 6 ++++-- .../space/joint/test_joint_state.py | 13 ++++++++++--- .../tests/space/cartesian/test_cartesian_state.cpp | 7 +++++-- .../test/tests/space/joint/test_joint_state.cpp | 12 ++++++++++-- 4 files changed, 29 insertions(+), 9 deletions(-) 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 9de89695e..ddf6f684c 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_state.py @@ -1004,10 +1004,12 @@ def test_utilities(self): self.assertIsInstance(state_variable_type, CartesianStateVariable) self.assertEqual("position", cartesian_state_variable_to_string(state_variable_type)) with self.assertRaises(InvalidStateVariableError): - result = string_to_cartesian_state_variable("foo") + string_to_cartesian_state_variable("foo") state = CartesianState() - state.set_position([1.0, 2.0, 3.0]) + 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()) 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 6c5231496..f895b5819 100755 --- a/python/test/state_representation/space/joint/test_joint_state.py +++ b/python/test/state_representation/space/joint/test_joint_state.py @@ -4,7 +4,7 @@ import numpy as np 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 +from state_representation.exceptions import InvalidStateVariableError, IncompatibleSizeError from datetime import timedelta JOINT_STATE_METHOD_EXPECTS = [ @@ -475,13 +475,20 @@ def test_utilities(self): self.assertIsInstance(state_variable_type, JointStateVariable) self.assertEqual("positions", joint_state_variable_to_string(state_variable_type)) with self.assertRaises(InvalidStateVariableError): - result = string_to_joint_state_variable("foo") + string_to_joint_state_variable("foo") state = JointState("foo", 3) - state.set_positions([1.0, 2.0, 3.0]) + 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()) 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 41b313ed1..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 @@ -1092,9 +1092,12 @@ TEST(CartesianStateTest, TestUtilities) { EXPECT_THROW(string_to_cartesian_state_variable("foo"), exceptions::InvalidStateVariableException); auto state = CartesianState(); - auto new_values = Eigen::VectorXd(3); + 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_position(new_values); + 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()); 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 24ebf5d31..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 @@ -609,12 +609,20 @@ TEST(JointStateTest, TestUtilities) { EXPECT_THROW(string_to_joint_state_variable("foo"), exceptions::InvalidStateVariableException); auto state = JointState("foo", 3); - auto new_values = Eigen::VectorXd(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_positions(new_values); + 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());