test: add multply_state_variable and some incompatible size tests

aica-technology · Oct 21, 2024 · 165a31e · 165a31e
1 parent 633600b
commit 165a31e
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 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
@@ -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
@@ -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
@@ -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
@@ -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());