From b10bf074b0179ac51ab0c20e175b867bfdf45aae Mon Sep 17 00:00:00 2001 From: Dominic Reber <71256590+domire8@users.noreply.github.com> Date: Wed, 8 Feb 2023 16:17:10 +0100 Subject: [PATCH] Ignore data fields when copying an empty state (#71) * Add resize helper to JointState * Resizing in initialize is not necessary * Avoid using virtual functions in JointState constructor * Improve JointState copy constructor Only set the state variables if the state is not empty * Update copy constructors in derived joint states * Reduce Clion warnings and add FIXME * Improve JointState copy constructor And only set state variables if state is not empty * Update copy constructor of derived Cartesian states * Test copy construction of parameters * Fix copy construction of ParameterContainer * Fix ParameterContainer translation helpers * Test copy construction of Python parameters * Add FIXMEs * 6.3.42 -> 6.3.43 * Update CHANGELOG * Update FIXMEs --- CHANGELOG.md | 1 + VERSION | 2 +- demos/CMakeLists.txt | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/include/parameter_container.h | 4 +- python/setup.py | 2 +- python/source/common/parameter_container.cpp | 21 ++++++++-- .../state_representation/test_parameters.py | 8 ++++ source/CMakeLists.txt | 2 +- .../space/cartesian/CartesianState.hpp | 2 +- .../space/joint/JointState.hpp | 6 +++ .../src/space/Jacobian.cpp | 1 + .../space/cartesian/CartesianAcceleration.cpp | 8 ++-- .../src/space/cartesian/CartesianPose.cpp | 8 ++-- .../src/space/cartesian/CartesianState.cpp | 14 ++----- .../src/space/cartesian/CartesianTwist.cpp | 8 ++-- .../src/space/cartesian/CartesianWrench.cpp | 8 ++-- .../src/space/joint/JointAccelerations.cpp | 8 ++-- .../src/space/joint/JointPositions.cpp | 8 ++-- .../src/space/joint/JointState.cpp | 41 +++++++++---------- .../src/space/joint/JointTorques.cpp | 8 ++-- .../src/space/joint/JointVelocities.cpp | 8 ++-- .../test/tests/test_parameter.cpp | 9 ++++ 24 files changed, 107 insertions(+), 76 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 59e24fd5c..1e3d7ddd1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -37,6 +37,7 @@ Release Versions: - Bind operators of joint states (#65) - Add initialize method to Parameter class (#68) - Allow Parameter construction with empty name (#67) +- Ignore data fields when copying an empty state (#71) ## 6.3.1 diff --git a/VERSION b/VERSION index 24f65e189..7326c5a9d 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -6.3.42 +6.3.43 diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 7ff981668..bf7c6465f 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -find_package(control_libraries 6.3.42 CONFIG REQUIRED) +find_package(control_libraries 6.3.43 CONFIG REQUIRED) set(DEMOS_SCRIPTS task_space_control_loop diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 226cfdd77..0b6d1ec68 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 = 6.3.42 +PROJECT_NUMBER = 6.3.43 # 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 e9dc3abc4..e53702e31 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/protocol/clproto_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(clproto VERSION 6.3.42) +project(clproto VERSION 6.3.43) # Default to C99 if(NOT CMAKE_C_STANDARD) diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h index be4ae5fd9..7a6123d6b 100644 --- a/python/include/parameter_container.h +++ b/python/include/parameter_container.h @@ -33,9 +33,9 @@ class ParameterContainer : public ParameterInterface { ); ParameterContainer(const ParameterContainer& parameter); - void set_value(const py::object& value); + void set_value(py::object value); - py::object get_value(); + py::object get_value() const; void initialize(); diff --git a/python/setup.py b/python/setup.py index 50cb22df3..9796ae4dc 100644 --- a/python/setup.py +++ b/python/setup.py @@ -11,7 +11,7 @@ # names of the environment variables that define osqp and openrobots include directories osqp_path_var = 'OSQP_INCLUDE_DIR' -__version__ = "6.3.42" +__version__ = "6.3.43" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp index 08b39f0a8..79ed70df7 100644 --- a/python/source/common/parameter_container.cpp +++ b/python/source/common/parameter_container.cpp @@ -8,6 +8,8 @@ namespace py_parameter { +// FIXME: Improve exceptions in switch cases and try catch casts + ParameterContainer::ParameterContainer( const std::string& name, const ParameterType& type, const StateType& parameter_state_type ) : ParameterInterface(name, type, parameter_state_type) { @@ -42,10 +44,13 @@ ParameterContainer::ParameterContainer( } ParameterContainer::ParameterContainer(const ParameterContainer& parameter) : - ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()), - values(parameter.values) {} + ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()) { + if (parameter) { + set_value(parameter.get_value()); + } +} -void ParameterContainer::set_value(const py::object& value) { +void ParameterContainer::set_value(py::object value) { switch (this->get_parameter_type()) { case ParameterType::INT: values.int_value = value.cast(); @@ -105,7 +110,7 @@ void ParameterContainer::set_value(const py::object& value) { this->set_empty(false); } -py::object ParameterContainer::get_value() { +py::object ParameterContainer::get_value() const { switch (this->get_parameter_type()) { case ParameterType::INT: return py::cast(values.int_value); @@ -160,6 +165,10 @@ void ParameterContainer::initialize() { } ParameterContainer interface_ptr_to_container(const std::shared_ptr& parameter) { + if (parameter->is_empty()) { + return ParameterContainer( + parameter->get_name(), parameter->get_parameter_type(), parameter->get_parameter_state_type()); + } switch (parameter->get_parameter_type()) { case ParameterType::INT: return ParameterContainer( @@ -233,6 +242,10 @@ ParameterContainer interface_ptr_to_container(const std::shared_ptr container_to_interface_ptr(const ParameterContainer& parameter) { + if (parameter.is_empty()) { + return make_shared_parameter_interface( + parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()); + } switch (parameter.get_parameter_type()) { case ParameterType::INT: return make_shared_parameter(parameter.get_name(), parameter.values.int_value); diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py index 604caf4a6..244f0ed03 100755 --- a/python/test/state_representation/test_parameters.py +++ b/python/test/state_representation/test_parameters.py @@ -44,6 +44,10 @@ def test_param_int(self): self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) self.assertEqual(param.get_value(), 0) + + new_param = sr.Parameter(param) + self.assertTrue(new_param.is_empty()) + param.set_value(1) self.assertTrue(param) self.assertFalse(param.is_empty()) @@ -52,6 +56,10 @@ def test_param_int(self): self.assertFalse(param1.is_empty()) self.assertEqual(param1.get_value(), 1) + new_param = sr.Parameter(param1) + self.assertFalse(new_param.is_empty()) + self.assertEqual(param1.get_value(), new_param.get_value()) + # FIXME (#50): Use parametrized pytests for these tests param1.initialize() self.assertTrue(param1.is_empty()) diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 0840ec715..fc4e9f7ef 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 6.3.42) +project(control_libraries VERSION 6.3.43) # Build options option(BUILD_TESTING "Build all tests." OFF) 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 b91001bbb..5e8eed7de 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -68,7 +68,7 @@ class CartesianState : public SpatialState { static CartesianState Identity(const std::string& name, const std::string& reference = "world"); /** - * @brief Constructor for a random Cartsian state + * @brief Constructor for a random Cartesian state */ static CartesianState Random(const std::string& name, const std::string& reference = "world"); 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 ca5d5cb4a..477691ec8 100644 --- a/source/state_representation/include/state_representation/space/joint/JointState.hpp +++ b/source/state_representation/include/state_representation/space/joint/JointState.hpp @@ -565,6 +565,12 @@ class JointState : public State { std::string to_string() const override; private: + /** + * @brief Resize the data vectors to a new size + * @param size The desired size + */ + void resize(unsigned int size); + std::vector names_;///< names of the joints Eigen::VectorXd positions_; ///< joints positions Eigen::VectorXd velocities_; ///< joints velocities diff --git a/source/state_representation/src/space/Jacobian.cpp b/source/state_representation/src/space/Jacobian.cpp index 209d9fcd1..12a32f22a 100644 --- a/source/state_representation/src/space/Jacobian.cpp +++ b/source/state_representation/src/space/Jacobian.cpp @@ -58,6 +58,7 @@ Jacobian::Jacobian(const std::string& robot_name, this->set_data(data); } +// FIXME: improve copy constructor after Jacobian refactor Jacobian::Jacobian(const Jacobian& jacobian) : State(jacobian), joint_names_(jacobian.joint_names_), diff --git a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp index 0a188f6c8..b48f3bc19 100644 --- a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp +++ b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp @@ -38,11 +38,11 @@ CartesianAcceleration::CartesianAcceleration( } CartesianAcceleration::CartesianAcceleration(const CartesianState& state) : CartesianState(state) { - // set all the state variables to 0 except linear and angular velocities this->set_type(StateType::CARTESIAN_ACCELERATION); - this->set_zero(); - this->set_acceleration(state.get_acceleration()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_acceleration(state.get_acceleration()); + } } CartesianAcceleration::CartesianAcceleration(const CartesianAcceleration& acceleration) : diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp index 7bc9f8fbc..3639052f9 100644 --- a/source/state_representation/src/space/cartesian/CartesianPose.cpp +++ b/source/state_representation/src/space/cartesian/CartesianPose.cpp @@ -44,11 +44,11 @@ CartesianPose::CartesianPose( } CartesianPose::CartesianPose(const CartesianState& state) : CartesianState(state) { - // set all the state variables to 0 except position and orientation this->set_type(StateType::CARTESIAN_POSE); - this->set_zero(); - this->set_pose(state.get_pose()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_pose(state.get_pose()); + } } CartesianPose::CartesianPose(const CartesianPose& pose) : CartesianPose(static_cast(pose)) {} diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index 0ee2c550c..cd879153b 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -51,16 +51,10 @@ CartesianState::CartesianState(const std::string& name, const std::string& refer } CartesianState::CartesianState(const CartesianState& state) : - SpatialState(state), - position_(state.position_), - orientation_(state.orientation_), - linear_velocity_(state.linear_velocity_), - angular_velocity_(state.angular_velocity_), - linear_acceleration_(state.linear_acceleration_), - angular_acceleration_(state.angular_acceleration_), - force_(state.force_), - torque_(state.torque_) { - this->set_type(StateType::CARTESIAN_STATE); + CartesianState(state.get_name(), state.get_reference_frame()) { + if (state) { + this->set_state_variable(state.get_state_variable(CartesianStateVariable::ALL), CartesianStateVariable::ALL); + } } CartesianState CartesianState::Identity(const std::string& name, const std::string& reference) { diff --git a/source/state_representation/src/space/cartesian/CartesianTwist.cpp b/source/state_representation/src/space/cartesian/CartesianTwist.cpp index edaffdedb..a397e35b7 100644 --- a/source/state_representation/src/space/cartesian/CartesianTwist.cpp +++ b/source/state_representation/src/space/cartesian/CartesianTwist.cpp @@ -38,11 +38,11 @@ CartesianTwist::CartesianTwist( } CartesianTwist::CartesianTwist(const CartesianState& state) : CartesianState(state) { - // set all the state variables to 0 except linear and angular velocities this->set_type(StateType::CARTESIAN_TWIST); - this->set_zero(); - this->set_twist(state.get_twist()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_twist(state.get_twist()); + } } CartesianTwist::CartesianTwist(const CartesianTwist& twist) : diff --git a/source/state_representation/src/space/cartesian/CartesianWrench.cpp b/source/state_representation/src/space/cartesian/CartesianWrench.cpp index 1453a3c80..e00a71e4c 100644 --- a/source/state_representation/src/space/cartesian/CartesianWrench.cpp +++ b/source/state_representation/src/space/cartesian/CartesianWrench.cpp @@ -36,11 +36,11 @@ CartesianWrench::CartesianWrench( } CartesianWrench::CartesianWrench(const CartesianState& state) : CartesianState(state) { - // set all the state variables to 0 except force and torque this->set_type(StateType::CARTESIAN_WRENCH); - this->set_zero(); - this->set_wrench(state.get_wrench()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_wrench(state.get_wrench()); + } } CartesianWrench::CartesianWrench(const CartesianWrench& wrench) : diff --git a/source/state_representation/src/space/joint/JointAccelerations.cpp b/source/state_representation/src/space/joint/JointAccelerations.cpp index d759979c0..e52f54641 100644 --- a/source/state_representation/src/space/joint/JointAccelerations.cpp +++ b/source/state_representation/src/space/joint/JointAccelerations.cpp @@ -33,11 +33,11 @@ JointAccelerations::JointAccelerations(const std::string& robot_name, } JointAccelerations::JointAccelerations(const JointState& state) : JointState(state) { - // set all the state variables to 0 except accelerations this->set_type(StateType::JOINT_ACCELERATIONS); - this->set_zero(); - this->set_accelerations(state.get_accelerations()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_accelerations(state.get_accelerations()); + } } JointAccelerations::JointAccelerations(const JointAccelerations& accelerations) : diff --git a/source/state_representation/src/space/joint/JointPositions.cpp b/source/state_representation/src/space/joint/JointPositions.cpp index f8d2e3e94..b57bd3690 100644 --- a/source/state_representation/src/space/joint/JointPositions.cpp +++ b/source/state_representation/src/space/joint/JointPositions.cpp @@ -34,11 +34,11 @@ JointPositions::JointPositions( } JointPositions::JointPositions(const JointState& state) : JointState(state) { - // set all the state variables to 0 except positions this->set_type(StateType::JOINT_POSITIONS); - this->set_zero(); - this->set_positions(state.get_positions()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_positions(state.get_positions()); + } } JointPositions::JointPositions(const JointPositions& positions) : diff --git a/source/state_representation/src/space/joint/JointState.cpp b/source/state_representation/src/space/joint/JointState.cpp index 297325b10..5aafcf4f1 100644 --- a/source/state_representation/src/space/joint/JointState.cpp +++ b/source/state_representation/src/space/joint/JointState.cpp @@ -32,29 +32,27 @@ static unsigned int get_state_variable_size_factor(const JointStateVariable& sta JointState::JointState() : State() { this->set_type(StateType::JOINT_STATE); - this->initialize(); } JointState::JointState(const std::string& robot_name, unsigned int nb_joints) : State(robot_name), names_(nb_joints) { this->set_type(StateType::JOINT_STATE); this->set_names(nb_joints); - this->initialize(); + this->resize(nb_joints); + this->set_zero(); } JointState::JointState(const std::string& robot_name, const std::vector& joint_names) : State(robot_name), names_(joint_names) { this->set_type(StateType::JOINT_STATE); - this->initialize(); + this->resize(joint_names.size()); + this->set_zero(); } JointState::JointState(const JointState& state) : - State(state), - names_(state.names_), - positions_(state.positions_), - velocities_(state.velocities_), - accelerations_(state.accelerations_), - torques_(state.torques_) { - this->set_type(StateType::JOINT_STATE); + JointState(state.get_name(), state.names_) { + if (state) { + this->set_state_variable(state.get_state_variable(JointStateVariable::ALL), JointStateVariable::ALL); + } } JointState JointState::Zero(const std::string& robot_name, unsigned int nb_joints) { @@ -91,6 +89,13 @@ JointState& JointState::operator=(const JointState& state) { return *this; } +void JointState::resize(unsigned int size) { + this->positions_.resize(size); + this->velocities_.resize(size); + this->accelerations_.resize(size); + this->torques_.resize(size); +} + Eigen::VectorXd JointState::get_state_variable(const JointStateVariable& state_variable_type) const { switch (state_variable_type) { case JointStateVariable::POSITIONS: @@ -351,7 +356,7 @@ void JointState::clamp_state_variable( const Eigen::ArrayXd& noise_ratio_array ) { Eigen::VectorXd state_variable = this->get_state_variable(state_variable_type); - int expected_size = state_variable.size(); + long expected_size = state_variable.size(); if (max_absolute_value_array.size() != expected_size) { throw IncompatibleSizeException( "Array of max values is of incorrect size: expected " + std::to_string(expected_size) + ", given " @@ -379,7 +384,7 @@ void JointState::clamp_state_variable( double max_absolute_value, const JointStateVariable& state_variable_type, double noise_ratio ) { Eigen::VectorXd state_variable = this->get_state_variable(state_variable_type); - int expected_size = state_variable.size(); + long expected_size = state_variable.size(); this->clamp_state_variable( max_absolute_value * Eigen::ArrayXd::Ones(expected_size), state_variable_type, noise_ratio * Eigen::ArrayXd::Ones(expected_size)); @@ -422,13 +427,6 @@ double dist(const JointState& s1, const JointState& s2, const JointStateVariable void JointState::initialize() { this->State::initialize(); - // resize - unsigned int size = this->names_.size(); - this->positions_.resize(size); - this->velocities_.resize(size); - this->accelerations_.resize(size); - this->torques_.resize(size); - // set to zeros this->set_zero(); } @@ -455,16 +453,17 @@ void JointState::set_zero() { this->velocities_.setZero(); this->accelerations_.setZero(); this->torques_.setZero(); + // FIXME: reset timestamp } std::vector JointState::to_std_vector() const { Eigen::VectorXd data = this->data(); - return std::vector(data.data(), data.data() + data.size()); + return {data.data(), data.data() + data.size()}; } void JointState::multiply_state_variable(const Eigen::MatrixXd& lambda, const JointStateVariable& state_variable_type) { Eigen::VectorXd state_variable = this->get_state_variable(state_variable_type); - int expected_size = state_variable.size(); + long expected_size = state_variable.size(); if (lambda.rows() != expected_size || lambda.cols() != expected_size) { throw IncompatibleSizeException( "Gain matrix is of incorrect size: expected " + std::to_string(expected_size) + "x" diff --git a/source/state_representation/src/space/joint/JointTorques.cpp b/source/state_representation/src/space/joint/JointTorques.cpp index 4901dfbad..9c457f9bf 100644 --- a/source/state_representation/src/space/joint/JointTorques.cpp +++ b/source/state_representation/src/space/joint/JointTorques.cpp @@ -30,11 +30,11 @@ JointTorques::JointTorques(const std::string& robot_name, const std::vectorset_type(StateType::JOINT_TORQUES); - this->set_zero(); - this->set_torques(state.get_torques()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_torques(state.get_torques()); + } } JointTorques::JointTorques(const JointTorques& torques) : JointTorques(static_cast(torques)) {} diff --git a/source/state_representation/src/space/joint/JointVelocities.cpp b/source/state_representation/src/space/joint/JointVelocities.cpp index 272942490..b864a9457 100644 --- a/source/state_representation/src/space/joint/JointVelocities.cpp +++ b/source/state_representation/src/space/joint/JointVelocities.cpp @@ -33,11 +33,11 @@ JointVelocities::JointVelocities( } JointVelocities::JointVelocities(const JointState& state) : JointState(state) { - // set all the state variables to 0 except velocities this->set_type(StateType::JOINT_VELOCITIES); - this->set_zero(); - this->set_velocities(state.get_velocities()); - this->set_empty(state.is_empty()); + if (state) { + this->set_zero(); + this->set_velocities(state.get_velocities()); + } } JointVelocities::JointVelocities(const JointVelocities& velocities) : diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp index aa7e64819..cc42138e4 100644 --- a/source/state_representation/test/tests/test_parameter.cpp +++ b/source/state_representation/test/tests/test_parameter.cpp @@ -138,16 +138,25 @@ TYPED_TEST_P(ParameterTest, Construction) { param.set_name("test"); EXPECT_EQ(param.get_name(), "test"); expect_values_equal(param.get_value(), TypeParam()); + + auto new_param(param); + EXPECT_TRUE(new_param.is_empty()); + ParameterInterface param_interface(param); EXPECT_EQ(param_interface.get_name(), param.get_name()); EXPECT_EQ(param_interface.get_type(), StateType::PARAMETER); EXPECT_EQ(param_interface.get_parameter_type(), param.get_parameter_type()); EXPECT_EQ(param_interface.get_parameter_state_type(), param.get_parameter_state_type()); + param.set_value(std::get<0>(test_case)); EXPECT_FALSE(param.is_empty()); EXPECT_TRUE(param); expect_values_equal(param.get_value(), std::get<0>(test_case)); + new_param = param; + EXPECT_FALSE(new_param.is_empty()); + expect_values_equal(param.get_value(), new_param.get_value()); + param.initialize(); EXPECT_TRUE(param.is_empty()); EXPECT_FALSE(param);