From a7649e7520a0ea0e8c859f9dfc0f44f08321809a Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 1 Apr 2022 11:07:34 +0200 Subject: [PATCH 1/6] Throw exception when setting state variable with wrong vector size --- .../space/cartesian/CartesianState.hpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 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 index ba64d3ec6..f91f59d26 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -557,8 +557,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 CartesianState::get_pose() const { @@ -679,6 +678,9 @@ inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, } inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, const std::vector& new_value) { + if (new_value.size() != 3) { + throw exceptions::IncompatibleSizeException("Provide a vector of length 3 to set the desired state variable."); + } this->set_state_variable(state_variable, Eigen::Vector3d::Map(new_value.data(), new_value.size())); } @@ -694,6 +696,9 @@ inline void CartesianState::set_state_variable( Eigen::Vector3d& linear_state_variable, Eigen::Vector3d& angular_state_variable, const std::vector& new_value ) { + if (new_value.size() != 6) { + throw exceptions::IncompatibleSizeException("Provide a vector of length 6 to set the desired state variable."); + } this->set_state_variable(linear_state_variable, std::vector(new_value.begin(), new_value.begin() + 3)); this->set_state_variable(angular_state_variable, std::vector(new_value.begin() + 3, new_value.end())); } From 93f3a314676246fdd6ac18afbbef178de5ce85b2 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 1 Apr 2022 11:14:13 +0200 Subject: [PATCH 2/6] Nicer exception message --- .../state_representation/space/cartesian/CartesianState.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 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 index f91f59d26..712670a53 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -679,7 +679,8 @@ inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, const std::vector& new_value) { if (new_value.size() != 3) { - throw exceptions::IncompatibleSizeException("Provide a vector of length 3 to set the desired state variable."); + 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())); } @@ -697,7 +698,8 @@ inline void CartesianState::set_state_variable( const std::vector& new_value ) { if (new_value.size() != 6) { - throw exceptions::IncompatibleSizeException("Provide a vector of length 6 to set the desired state variable."); + throw exceptions::IncompatibleSizeException( + "Input vector is of incorrect size: expected 6, given " + std::to_string(new_value.size())); } this->set_state_variable(linear_state_variable, std::vector(new_value.begin(), new_value.begin() + 3)); this->set_state_variable(angular_state_variable, std::vector(new_value.begin() + 3, new_value.end())); From e1e53bcea2114c80857948ff22c4fb8f410d62a1 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 1 Apr 2022 11:14:49 +0200 Subject: [PATCH 3/6] 5.1.0 -> 5.1.1 --- VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 2 +- source/CMakeLists.txt | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/VERSION b/VERSION index 831446cbd..ac14c3dfa 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.1.0 +5.1.1 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 0f4c7c439..f22c3a605 100644 --- a/doxygen/doxygen.conf +++ b/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 diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index baf440b61..2562eaf5b 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/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) diff --git a/python/setup.py b/python/setup.py index 86002d3cf..3c0df6f4d 100644 --- a/python/setup.py +++ b/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'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 547267e21..6c57c68bf 100644 --- a/source/CMakeLists.txt +++ b/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) From c8c84663f78f4c0df6639563b346492d509f6604 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 1 Apr 2022 11:21:30 +0200 Subject: [PATCH 4/6] Update CHANGELOG --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index e5a3f8ac6..024e77edf 100644 --- a/CHANGELOG.md +++ b/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. From f858209c2f0e4639fd55b9637391b827fd5eb59b Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 1 Apr 2022 11:52:27 +0200 Subject: [PATCH 5/6] Remove unused function --- .../space/cartesian/CartesianState.hpp | 23 ------------------- 1 file changed, 23 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 index 712670a53..9e2757988 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -86,17 +86,6 @@ class CartesianState : public SpatialState { const Eigen::Matrix& 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& new_value - ); - protected: /** * @brief Getter of the variable value corresponding to the input @@ -693,18 +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& new_value -) { - if (new_value.size() != 6) { - throw exceptions::IncompatibleSizeException( - "Input vector is of incorrect size: expected 6, given " + std::to_string(new_value.size())); - } - this->set_state_variable(linear_state_variable, std::vector(new_value.begin(), new_value.begin() + 3)); - this->set_state_variable(angular_state_variable, std::vector(new_value.begin() + 3, new_value.end())); -} - inline void CartesianState::set_position(const Eigen::Vector3d& position) { this->set_state_variable(this->position_, position); } From e1bfb1ad5f5d9b4d1429f9df11aef671e0089ffd Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 1 Apr 2022 11:54:36 +0200 Subject: [PATCH 6/6] Add unittests --- .../space/cartesian/test_cartesian_state.py | 2 ++ .../state_representation/space/cartesian/CartesianState.hpp | 2 +- .../test/tests/space/cartesian/test_cartesian_state.cpp | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) 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 289697682..262c55f84 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/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) 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 9e2757988..65aea975b 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -652,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)); 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 8e99fb564..2773610ad 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 @@ -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{1, 2, 3, 4}), exceptions::IncompatibleSizeException); // orientation Eigen::Vector4d orientation_vec = Eigen::Vector4d::Random().normalized();