feat: add C++ tests

aica-technology · Oct 18, 2024 · dcfb29a · dcfb29a
1 parent 88f9287
commit dcfb29a
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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<double>{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());
+}
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
@@ -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<double>{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());
 }