Throw exception if setting state variable from vector with wrong size

CHANGELOG.md

@@ -10,6 +10,10 @@ Release Versions:
 - [2.0.0](#200)
 - [1.0.0](#100)
 
+## Upcoming changes (in development)
+
+- Throw exception if setting state variable from vector with wrong size (#273)
+
 ## 5.1.0
 
 Version 5.1.0 contains a few new features and improvements to the behaviour and usage of the libraries.

VERSION

@@ -1 +1 @@
-5.1.0
+5.1.1

doxygen/doxygen.conf

@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 5.1.0
+PROJECT_NUMBER = 5.1.1
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

protocol/clproto_cpp/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.9)
-project(clproto VERSION 5.1.0)
+project(clproto VERSION 5.1.1)
 
 set(CMAKE_CXX_STANDARD 17)
 

python/setup.py

@@ -10,7 +10,7 @@
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 openrobots_path_var = 'OPENROBOTS_INCLUDE_DIR'
 
-__version__ = "5.1.0"
+__version__ = "5.1.1"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

python/test/state_representation/space/cartesian/test_cartesian_state.py

@@ -185,6 +185,8 @@ def test_get_set_fields(self):
         [self.assertAlmostEqual(cs.get_position()[i], position[i]) for i in range(3)]
         cs.set_position(1.1, 2.2, 3.3)
         assert_array_equal(np.array([1.1, 2.2, 3.3]), cs.get_position())
+        with self.assertRaises(RuntimeError):
+            cs.set_position([1., 2., 3., 4.])
 
         # orientation
         orientation_vec = np.random.rand(4)

source/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 5.1.0)
+project(control_libraries VERSION 5.1.1)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

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

@@ -86,17 +86,6 @@ class CartesianState : public SpatialState {
       const Eigen::Matrix<double, 6, 1>& new_value
   );
 
-  /**
-   * @brief Set new_value in the provided state_variable (twist, acceleration or wrench)
-   * @param linear_state_variable the linear part of the state variable to fill
-   * @param angular_state_variable the angular part of the state variable to fill
-   * @param new_value the new value of the state variable
-   */
-  void set_state_variable(
-      Eigen::Vector3d& linear_state_variable, Eigen::Vector3d& angular_state_variable,
-      const std::vector<double>& new_value
-  );
-
 protected:
   /**
    * @brief Getter of the variable value corresponding to the input
@@ -557,8 +546,7 @@ inline const Eigen::Quaterniond& CartesianState::get_orientation() const {
 inline Eigen::Vector4d CartesianState::get_orientation_coefficients() const {
   return Eigen::Vector4d(
       this->get_orientation().w(), this->get_orientation().x(), this->get_orientation().y(),
-      this->get_orientation().z()
-  );
+      this->get_orientation().z());
 }
 
 inline Eigen::Matrix<double, 7, 1> CartesianState::get_pose() const {
@@ -664,7 +652,7 @@ inline Eigen::VectorXd CartesianState::get_state_variable(const CartesianStateVa
 
 inline void CartesianState::set_all_state_variables(const Eigen::VectorXd& new_values) {
   if (new_values.size() != 25) {
-    throw state_representation::exceptions::IncompatibleSizeException(
+    throw exceptions::IncompatibleSizeException(
         "Input is of incorrect size: expected 25, given " + std::to_string(new_values.size()));
   }
   this->set_pose(new_values.segment(0, 7));
@@ -679,6 +667,10 @@ inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable,
 }
 
 inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, const std::vector<double>& new_value) {
+  if (new_value.size() != 3) {
+    throw exceptions::IncompatibleSizeException(
+        "Input vector is of incorrect size: expected 3, given " + std::to_string(new_value.size()));
+  }
   this->set_state_variable(state_variable, Eigen::Vector3d::Map(new_value.data(), new_value.size()));
 }
 
@@ -690,14 +682,6 @@ inline void CartesianState::set_state_variable(
   this->set_state_variable(angular_state_variable, new_value.tail(3));
 }
 
-inline void CartesianState::set_state_variable(
-    Eigen::Vector3d& linear_state_variable, Eigen::Vector3d& angular_state_variable,
-    const std::vector<double>& new_value
-) {
-  this->set_state_variable(linear_state_variable, std::vector<double>(new_value.begin(), new_value.begin() + 3));
-  this->set_state_variable(angular_state_variable, std::vector<double>(new_value.begin() + 3, new_value.end()));
-}
-
 inline void CartesianState::set_position(const Eigen::Vector3d& position) {
   this->set_state_variable(this->position_, position);
 }

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

@@ -119,6 +119,7 @@ TEST(CartesianStateTest, GetSetFields) {
   }
   cs.set_position(1.1, 2.2, 3.3);
   EXPECT_TRUE(Eigen::Vector3d(1.1, 2.2, 3.3).isApprox(cs.get_position()));
+  EXPECT_THROW(cs.set_position(std::vector<double>{1, 2, 3, 4}), exceptions::IncompatibleSizeException);
 
   // orientation
   Eigen::Vector4d orientation_vec = Eigen::Vector4d::Random().normalized();