From f547ee5d85bbb5043d15b0955e9f05ef33d5067f Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 8 Apr 2022 18:42:19 +0200 Subject: [PATCH 01/17] Release 5.2.0 (#282) * v1.0.0-rc1 Add version and experimental features * Add project version 1.0.0 to top level CMakeLists * Use a build option EXPERIMENTAL_FEATURES which can be used to disable experimental features (off by default). * Mark DualQuaternion sources experimental using cmake if (EXPERIMENTAL_FEATURES) * Mark test_dual_quaternion as experimental using preprocessing directive #ifdef EXPERIMENTAL_FEATURES * Remove message lines from CMakeLists * Remove message("experimental") and subsequent second message line from state representation cmakelists, which was only included to test the EXPERIMENTAL_FEATURES flag was working * Update license to Gnu GPL v3 * v2.0.0-rc-02 * Update project version number * Update CHANGELOG * v3.0.0-rc01 * Increment version numbers in C++ and Python projects * Update CHANGELOG * v3.1.0-rc01 * Increment version numbers in C++ and Python projects * Update CHANGELOG * Set version 4.0.0 * Rename control-libraries * Update all references to control_libraries to the new repo name control-libraries * Update CHANGELOG * Update version number * Update CHANGELOG * Set version 5.0.0 * Update CHANGELOG * 5.1.0 -> 5.1.0 * Update CHANGELOG * Update python README * Move new bindings to features section * Set version 5.2.0 * Update CHANGELOG Co-authored-by: Enrico Eberhard Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- CHANGELOG.md | 14 ++++++++++++-- VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 2 +- source/CMakeLists.txt | 2 +- 6 files changed, 17 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d6c10d9fa..696412334 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,7 @@ # CHANGELOG Release Versions: +- [5.2.0](#520) - [5.1.0](#510) - [5.0.0](#500) - [4.1.0](#410) @@ -10,12 +11,21 @@ Release Versions: - [2.0.0](#200) - [1.0.0](#100) -## Upcoming changes (in development) +## 5.2.0 +Version 5.2.0 contains a few fixes and a new feature for the Impedance controller. + +### Features + +**controllers** +- Add force limit parameter to Impedance controller (#276) + +### Fixes + +**general** - Throw exception if setting state variable from vector with wrong size (#273) - Improve installation script and python installation guide (#274) - Fix path in protocol installation guide (#275) -- Add force limit parameter to Impedance controller (#276) - Fix python bindings import issues (#279) ## 5.1.0 diff --git a/VERSION b/VERSION index 220d8e0a4..91ff57278 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.1.5 +5.2.0 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 82fe0b145..16e0b0ef3 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.5 +PROJECT_NUMBER = 5.2.0 # 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 80856b732..81b9e7445 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.5) +project(clproto VERSION 5.2.0) set(CMAKE_CXX_STANDARD 17) diff --git a/python/setup.py b/python/setup.py index 27bd82706..49b1536e2 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.5" +__version__ = "5.2.0" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index db5d71c6a..e784fd7aa 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.5) +project(control_libraries VERSION 5.2.0) # Build options option(BUILD_TESTING "Build all tests." OFF) From 4a36a6d4c7f171a84ecb8d75df009abb684da1f5 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 12 Apr 2022 14:10:41 +0200 Subject: [PATCH 02/17] Fix if else conditions in setup.py to correctly install modules (#285) * Fix if else conditions in setup.py to correctly install modules * 5.2.0 -> 5.2.1 * Update CHANGELOG --- CHANGELOG.md | 4 ++++ VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 22 +++++++++++----------- source/CMakeLists.txt | 2 +- 6 files changed, 19 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 696412334..534a0ea32 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,10 @@ Release Versions: - [2.0.0](#200) - [1.0.0](#100) +## Upcoming changes (in development) + +- Fix if else conditions in setup.py to correctly install modules (#285) + ## 5.2.0 Version 5.2.0 contains a few fixes and a new feature for the Impedance controller. diff --git a/VERSION b/VERSION index 91ff57278..26d99a283 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.0 +5.2.1 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 16e0b0ef3..eb5ea7ce0 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.2.0 +PROJECT_NUMBER = 5.2.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 81b9e7445..78a644e68 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.2.0) +project(clproto VERSION 5.2.1) set(CMAKE_CXX_STANDARD 17) diff --git a/python/setup.py b/python/setup.py index 49b1536e2..b162efc39 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.2.0" +__version__ = "5.2.1" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] @@ -28,13 +28,6 @@ else: raise Exception('Could not find Eigen3 package!') - if __install_robot_model_module__: - osqp_path = os.environ[osqp_path_var] if osqp_path_var in os.environ.keys() else '/usr/local/include/osqp' - __include_dirs__.append(osqp_path) - openrobots_path = os.environ[ - openrobots_path_var] if openrobots_path_var in os.environ.keys() else '/opt/openrobots/include' - __include_dirs__.append('/opt/openrobots/include') - for lib in __libraries__: status = os.popen(f'ldconfig -p | grep {lib}').read().strip() if len(status) == 0: @@ -42,18 +35,25 @@ if lib == 'clproto': warnings.warn(f'{msg} The clproto module will not be installed.') __install_clproto_module__ = False - if lib == 'dynamical_systems': + elif lib == 'dynamical_systems': warnings.warn(f'{msg} The dynamical_systems module will not be installed.') __install_dynamical_systems_module__ = False - if lib == 'robot_model': + elif lib == 'robot_model': warnings.warn(f'{msg} The robot_model module will not be installed.') __install_robot_model_module__ = False - if lib == 'controllers': + elif lib == 'controllers': warnings.warn(f'{msg} The controllers module will not be installed.') __install_controllers_module__ = False else: raise Exception(msg) + if __install_robot_model_module__: + osqp_path = os.environ[osqp_path_var] if osqp_path_var in os.environ.keys() else '/usr/local/include/osqp' + __include_dirs__.append(osqp_path) + openrobots_path = os.environ[ + openrobots_path_var] if openrobots_path_var in os.environ.keys() else '/opt/openrobots/include' + __include_dirs__.append('/opt/openrobots/include') + if __install_controllers_module__ and not __install_robot_model_module__: warnings.warn( 'The robot model module is required to build the controllers module! ' diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index e784fd7aa..c05539bd2 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.0) +project(control_libraries VERSION 5.2.1) # Build options option(BUILD_TESTING "Build all tests." OFF) From f7067ee3e92d1e46084bb3ac0d39e7225159522c Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Fri, 8 Apr 2022 11:12:14 +0200 Subject: [PATCH 03/17] Refactor state type (#277) * Refactor StateType * Define StateType enum in a new header file * Refactor StateType fields: * * Complete coverage of spatial classes instead of just e.g. State and Pose * * Abstraction of Parameter type, to differentiate underlying parameter type from the State derived Parameter object * * More consistent naming with underscore separation * * Mark Dual Quaternion types as experimental * Add compile definition for experimental features if desired * Refactor State * Remove local StateType definition and include new header * Add protected set_type method to allow the StateType to be updated by derived class * Move implementations from header into the source file * Update tests * Update SpatialState * Add default constructor for SpatialState to set the SPATIAL_STATE state type * Update tests * Update CartesianState * Fix typos in docstring and argument name * Use new StateType * Update tests * Update CartesianState derivatives * Set new StateType in constructors * Update tests * Update JointState and derivatives * Set new StateType in constructors * Update tests * Update Jacobian * Set new StateType in constructors * Extend compatible state types in is_compatible * Update tests * Skip Ellipsoid fitting test * Skip test to reduce computational burden and time * Add ParameterType * Add ParameterType enum in header to list contained data types * StateType::NONE * Add default state type NONE to cover cases where the state type is not set or not applicable * Refactor ParameterInterface * Use one constructor that takes the parameter type and optional parameter_state_type state type instead * Add getters for parameter type and state type * Refactor default constructor to take the ParameterType, setting the internal StateType to PARAMETER by default * Add constructor explicitly for state parameters, which sets the parameter_type to STATE and the parameter_state_type accordingly * Add default virtual destructor for safety due to inheritance and enable_shared_from_this * Update Parameter * Set new ParameterType in constructor initializers * For state parameters, set the parameter state type * Declare default virtual destructor * Update parameter tests * Update controllers * Use new parameter_type accessor and ParameterType when validating parameters * Update dynamical systems * Use new parameter_type accessor and ParameterType when validating parameters, and parameter_state_type accessor when validating state parameters * CHANGELOG * 5.1.4 -> 5.1.5 --- CHANGELOG.md | 1 + source/CMakeLists.txt | 4 + .../controllers/impedance/Impedance.hpp | 8 +- .../src/impedance/CompliantTwist.cpp | 2 +- .../dynamical_systems/src/PointAttractor.cpp | 14 ++-- .../include/state_representation/State.hpp | 78 ++----------------- .../state_representation/StateType.hpp | 38 +++++++++ .../parameters/Parameter.hpp | 5 ++ .../parameters/ParameterInterface.hpp | 35 ++++++++- .../parameters/ParameterType.hpp | 22 ++++++ .../space/SpatialState.hpp | 10 ++- .../space/cartesian/CartesianState.hpp | 4 +- source/state_representation/src/State.cpp | 51 ++++++++++++ .../src/parameters/Parameter.cpp | 59 +++++++------- .../src/parameters/ParameterInterface.cpp | 13 +++- .../src/space/Jacobian.cpp | 20 +++-- .../src/space/SpatialState.cpp | 4 + .../space/cartesian/CartesianAcceleration.cpp | 8 +- .../src/space/cartesian/CartesianPose.cpp | 9 ++- .../src/space/cartesian/CartesianState.cpp | 6 +- .../src/space/cartesian/CartesianTwist.cpp | 8 +- .../src/space/cartesian/CartesianWrench.cpp | 8 +- .../src/space/joint/JointAccelerations.cpp | 10 ++- .../src/space/joint/JointPositions.cpp | 11 ++- .../src/space/joint/JointState.cpp | 6 +- .../src/space/joint/JointTorques.cpp | 11 ++- .../src/space/joint/JointVelocities.cpp | 11 ++- .../cartesian/test_cartesian_acceleration.cpp | 3 + .../space/cartesian/test_cartesian_pose.cpp | 9 +++ .../space/cartesian/test_cartesian_state.cpp | 2 +- .../space/cartesian/test_cartesian_twist.cpp | 6 ++ .../space/cartesian/test_cartesian_wrench.cpp | 3 + .../space/joint/test_joint_accelerations.cpp | 22 ++++++ .../space/joint/test_joint_positions.cpp | 20 +++++ .../tests/space/joint/test_joint_state.cpp | 29 ++++++- .../tests/space/joint/test_joint_torques.cpp | 14 ++++ .../space/joint/test_joint_velocities.cpp | 23 ++++++ .../test/tests/space/test_jacobian.cpp | 4 + .../test/tests/space/test_spatial_state.cpp | 28 ++++--- .../test/tests/test_ellipsoid.cpp | 1 + .../test/tests/test_parameter.cpp | 33 +++++--- .../test/tests/test_state.cpp | 14 ++-- 42 files changed, 494 insertions(+), 173 deletions(-) create mode 100644 source/state_representation/include/state_representation/StateType.hpp create mode 100644 source/state_representation/include/state_representation/parameters/ParameterType.hpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 534a0ea32..64a4f99ec 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ Release Versions: ## Upcoming changes (in development) - Fix if else conditions in setup.py to correctly install modules (#285) +- Refactor state type (#277) ## 5.2.0 diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index c05539bd2..ec7f1e7d8 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -39,6 +39,10 @@ else() find_package(GTest QUIET) endif() +if(EXPERIMENTAL_FEATURES) + add_compile_definitions(EXPERIMENTAL_FEATURES) +endif() + add_subdirectory(state_representation) list(APPEND INSTALL_INTERFACE_LINK_LIBRARIES state_representation) get_target_property(TARGET_INTERFACE_LINK_LIBRARIES state_representation INTERFACE_LINK_LIBRARIES) diff --git a/source/controllers/include/controllers/impedance/Impedance.hpp b/source/controllers/include/controllers/impedance/Impedance.hpp index 02bee783c..30a1287d0 100644 --- a/source/controllers/include/controllers/impedance/Impedance.hpp +++ b/source/controllers/include/controllers/impedance/Impedance.hpp @@ -126,10 +126,10 @@ Eigen::MatrixXd Impedance::gain_matrix_from_parameter( const std::shared_ptr& parameter ) { Eigen::MatrixXd matrix; - if (parameter->get_type() == state_representation::StateType::PARAMETER_DOUBLE) { + if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE) { auto gain = std::static_pointer_cast>(parameter); matrix = gain->get_value() * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_); - } else if (parameter->get_type() == state_representation::StateType::PARAMETER_DOUBLE_ARRAY) { + } else if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE_ARRAY) { auto gain = std::static_pointer_cast>>(parameter); if (gain->get_value().size() != this->dimensions_) { throw state_representation::exceptions::IncompatibleSizeException( @@ -138,7 +138,7 @@ Eigen::MatrixXd Impedance::gain_matrix_from_parameter( } Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain->get_value().data(), this->dimensions_); matrix = diagonal.asDiagonal(); - } else if (parameter->get_type() == state_representation::StateType::PARAMETER_VECTOR) { + } else if (parameter->get_parameter_type() == state_representation::ParameterType::VECTOR) { auto gain = std::static_pointer_cast>(parameter); if (gain->get_value().size() != this->dimensions_) { throw state_representation::exceptions::IncompatibleSizeException( @@ -147,7 +147,7 @@ Eigen::MatrixXd Impedance::gain_matrix_from_parameter( } Eigen::VectorXd diagonal = gain->get_value(); matrix = diagonal.asDiagonal(); - } else if (parameter->get_type() == state_representation::StateType::PARAMETER_MATRIX) { + } else if (parameter->get_parameter_type() == state_representation::ParameterType::MATRIX) { auto gain = std::static_pointer_cast>(parameter); if (gain->get_value().rows() != this->dimensions_ || gain->get_value().cols() != this->dimensions_) { auto dim = std::to_string(this->dimensions_); diff --git a/source/controllers/src/impedance/CompliantTwist.cpp b/source/controllers/src/impedance/CompliantTwist.cpp index 920aa1a39..9e45f0792 100644 --- a/source/controllers/src/impedance/CompliantTwist.cpp +++ b/source/controllers/src/impedance/CompliantTwist.cpp @@ -79,7 +79,7 @@ CartesianState CompliantTwist::compute_command( void CompliantTwist::validate_and_set_parameter( const std::shared_ptr& parameter ) { - if (parameter->get_type() != StateType::PARAMETER_DOUBLE) { + if (parameter->get_parameter_type() != ParameterType::DOUBLE) { throw state_representation::exceptions::InvalidParameterException( "Parameter " + parameter->get_name() + " must be a double"); } diff --git a/source/dynamical_systems/src/PointAttractor.cpp b/source/dynamical_systems/src/PointAttractor.cpp index 72cf834b6..8c7e7cda9 100644 --- a/source/dynamical_systems/src/PointAttractor.cpp +++ b/source/dynamical_systems/src/PointAttractor.cpp @@ -56,10 +56,10 @@ bool PointAttractor::is_compatible(const JointState& state) const { template void PointAttractor::set_gain(const std::shared_ptr& parameter, unsigned int expected_size) { - if (parameter->get_type() == StateType::PARAMETER_DOUBLE) { + if (parameter->get_parameter_type() == ParameterType::DOUBLE) { auto gain = parameter->get_parameter_value(); this->gain_->set_value(gain * Eigen::MatrixXd::Identity(expected_size, expected_size)); - } else if (parameter->get_type() == StateType::PARAMETER_DOUBLE_ARRAY) { + } else if (parameter->get_parameter_type() == ParameterType::DOUBLE_ARRAY) { auto gain = parameter->get_parameter_value>(); if (gain.size() != expected_size) { throw exceptions::IncompatibleSizeException( @@ -67,7 +67,7 @@ void PointAttractor::set_gain(const std::shared_ptr& para } Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain.data(), expected_size); this->gain_->set_value(diagonal.asDiagonal()); - } else if (parameter->get_type() == StateType::PARAMETER_MATRIX) { + } else if (parameter->get_parameter_type() == ParameterType::MATRIX) { auto gain = parameter->get_parameter_value(); if (gain.rows() != expected_size && gain.cols() != expected_size) { throw exceptions::IncompatibleSizeException( @@ -143,9 +143,9 @@ void PointAttractor::set_base_frame(const CartesianState& base_f template<> void PointAttractor::validate_and_set_parameter(const std::shared_ptr& parameter) { if (parameter->get_name() == "attractor") { - if (parameter->get_type() == StateType::PARAMETER_CARTESIANSTATE) { + if (parameter->get_parameter_state_type() == StateType::CARTESIAN_STATE) { this->set_attractor(parameter->get_parameter_value()); - } else if (parameter->get_type() == StateType::PARAMETER_CARTESIANPOSE) { + } else if (parameter->get_parameter_state_type() == StateType::CARTESIAN_POSE) { this->set_attractor(parameter->get_parameter_value()); } } else if (parameter->get_name() == "gain") { @@ -159,9 +159,9 @@ void PointAttractor::validate_and_set_parameter(const std::share template<> void PointAttractor::validate_and_set_parameter(const std::shared_ptr& parameter) { if (parameter->get_name() == "attractor") { - if (parameter->get_type() == StateType::PARAMETER_JOINTSTATE) { + if (parameter->get_parameter_state_type() == StateType::JOINT_STATE) { this->set_attractor(parameter->get_parameter_value()); - } else if (parameter->get_type() == StateType::PARAMETER_JOINTPOSITIONS) { + } else if (parameter->get_parameter_state_type() == StateType::JOINT_POSITIONS) { this->set_attractor(parameter->get_parameter_value()); } } else if (parameter->get_name() == "gain") { diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp index 5d72f4ced..912bff341 100644 --- a/source/state_representation/include/state_representation/State.hpp +++ b/source/state_representation/include/state_representation/State.hpp @@ -9,6 +9,7 @@ #include #include +#include "state_representation/StateType.hpp" #include "state_representation/MathTools.hpp" /** @@ -16,31 +17,6 @@ * @brief Core state variables and objects */ namespace state_representation { -enum class StateType { - STATE, - CARTESIANSTATE, - DUALQUATERNIONSTATE, - JOINTSTATE, - JACOBIANMATRIX, - TRAJECTORY, - GEOMETRY_SHAPE, - GEOMETRY_ELLIPSOID, - PARAMETER_DOUBLE, - PARAMETER_DOUBLE_ARRAY, - PARAMETER_BOOL, - PARAMETER_BOOL_ARRAY, - PARAMETER_STRING, - PARAMETER_STRING_ARRAY, - PARAMETER_CARTESIANSTATE, - PARAMETER_CARTESIANPOSE, - PARAMETER_JOINTSTATE, - PARAMETER_JOINTPOSITIONS, - PARAMETER_ELLIPSOID, - PARAMETER_MATRIX, - PARAMETER_VECTOR, - PARAMETER_INT, - PARAMETER_INT_ARRAY -}; /** * @class State @@ -178,6 +154,13 @@ class State : public std::enable_shared_from_this { */ friend std::ostream& operator<<(std::ostream& os, const State& state); +protected: + /** + * @brief Override the state type + * @param type the type of the state to set + */ + void set_type(const StateType& type); + private: StateType type_; ///< type of the State std::string name_; ///< name of the state @@ -198,54 +181,9 @@ inline State& State::operator=(const State& state) { return *this; } -inline const StateType& State::get_type() const { - return this->type_; -} - -inline bool State::is_empty() const { - return this->empty_; -} - -inline void State::set_empty(bool empty) { - this->empty_ = empty; -} - -inline void State::set_filled() { - this->empty_ = false; - this->reset_timestamp(); -} - -inline const std::chrono::time_point& State::get_timestamp() const { - return this->timestamp_; -} - -inline void State::set_timestamp(const std::chrono::time_point& timepoint) { - this->timestamp_ = timepoint; -} - -inline void State::reset_timestamp() { - this->timestamp_ = std::chrono::steady_clock::now(); -} - template inline bool State::is_deprecated(const std::chrono::duration& time_delay) { return ((std::chrono::steady_clock::now() - this->timestamp_) > time_delay); } -inline const std::string& State::get_name() const { - return this->name_; -} - -inline void State::set_name(const std::string& name) { - this->name_ = name; -} - -inline bool State::is_compatible(const State& state) const { - bool compatible = (this->name_ == state.name_); - return compatible; -} - -inline void State::initialize() { - this->empty_ = true; -} }// namespace state_representation diff --git a/source/state_representation/include/state_representation/StateType.hpp b/source/state_representation/include/state_representation/StateType.hpp new file mode 100644 index 000000000..0f8ed9b47 --- /dev/null +++ b/source/state_representation/include/state_representation/StateType.hpp @@ -0,0 +1,38 @@ +#pragma once + +/** + * @namespace state_representation + * @brief Core state variables and objects + */ +namespace state_representation { + +/** + * @enum StateType + * @brief The class types inheriting from State. + */ +enum class StateType { + NONE, + STATE, + SPATIAL_STATE, + CARTESIAN_STATE, + CARTESIAN_POSE, + CARTESIAN_TWIST, + CARTESIAN_ACCELERATION, + CARTESIAN_WRENCH, + JOINT_STATE, + JOINT_POSITIONS, + JOINT_VELOCITIES, + JOINT_ACCELERATIONS, + JOINT_TORQUES, + JACOBIAN, + PARAMETER, + GEOMETRY_SHAPE, + GEOMETRY_ELLIPSOID, + TRAJECTORY, +#ifdef EXPERIMENTAL_FEATURES + DUAL_QUATERNION_STATE, + DUAL_QUATERNION_POSE, + DUAL_QUATERNION_TWIST +#endif +}; +} \ No newline at end of file diff --git a/source/state_representation/include/state_representation/parameters/Parameter.hpp b/source/state_representation/include/state_representation/parameters/Parameter.hpp index 561823e41..2a419ac2d 100644 --- a/source/state_representation/include/state_representation/parameters/Parameter.hpp +++ b/source/state_representation/include/state_representation/parameters/Parameter.hpp @@ -25,6 +25,11 @@ class Parameter : public ParameterInterface { */ explicit Parameter(const std::string& name, const T& value); + /** + * @brief Default virtual destructor + */ + virtual ~Parameter() = default; + /** * @brief Copy constructor * @param parameter The parameter to copy diff --git a/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp b/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp index 8b6459967..72d0dc844 100644 --- a/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp +++ b/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp @@ -2,6 +2,7 @@ #include +#include "state_representation/parameters/ParameterType.hpp" #include "state_representation/exceptions/InvalidParameterCastException.hpp" #include "state_representation/exceptions/InvalidPointerException.hpp" #include "state_representation/State.hpp" @@ -15,11 +16,14 @@ class Parameter; class ParameterInterface : public State { public: /** - * @brief Constructor with parameter name and type of the parameter. - * @param type The type of the parameter + * @brief Constructor with parameter name and type. * @param name The name of the parameter + * @param type The type of the parameter + * @param parameter_state_type The state type of the parameter, if applicable */ - explicit ParameterInterface(const StateType& type, const std::string& name); + ParameterInterface( + const std::string& name, const ParameterType& type, const StateType& parameter_state_type = StateType::NONE + ); /** * @brief Copy constructor @@ -27,6 +31,11 @@ class ParameterInterface : public State { */ ParameterInterface(const ParameterInterface& parameter); + /** + * @brief Default virtual destructor + */ + virtual ~ParameterInterface() = default; + /** * @brief Copy assignment operator that has to be defined * to the custom assignment operator. @@ -64,7 +73,7 @@ class ParameterInterface : public State { T get_parameter_value(); /** - * @brief Set the parameter value of a derived Parameter instance through the ParameterInterface pointer. + * @brief Set the parameter value of a derived Parameter instance through the ParameterInterface pointer. * @details This throws an InvalidParameterCastException if the ParameterInterface does not point to * a valid Parameter instance or if the specified type does not match the type of the Parameter instance. * @see ParameterInterface::get_parameter() @@ -73,6 +82,24 @@ class ParameterInterface : public State { */ template void set_parameter_value(const T& value); + + /** + * @brief Get the parameter type. + * @return The type of the underlying parameter + */ + ParameterType get_parameter_type() const; + + /** + * @brief Get the state type of the parameter. + * @details If the parameter type from get_parameter_type() is not ParameterType::STATE, + * this will return StateType::NONE. + * @return The state type of the underlying parameter + */ + StateType get_parameter_state_type() const; + +private: + ParameterType parameter_type_; + StateType parameter_state_type_; }; template diff --git a/source/state_representation/include/state_representation/parameters/ParameterType.hpp b/source/state_representation/include/state_representation/parameters/ParameterType.hpp new file mode 100644 index 000000000..97b374399 --- /dev/null +++ b/source/state_representation/include/state_representation/parameters/ParameterType.hpp @@ -0,0 +1,22 @@ +#pragma once + +namespace state_representation { + +/** + * @enum ParameterType + * @brief The parameter value types. + */ +enum class ParameterType { + BOOL, + BOOL_ARRAY, + INT, + INT_ARRAY, + DOUBLE, + DOUBLE_ARRAY, + STRING, + STRING_ARRAY, + STATE, + VECTOR, + MATRIX +}; +} \ No newline at end of file diff --git a/source/state_representation/include/state_representation/space/SpatialState.hpp b/source/state_representation/include/state_representation/space/SpatialState.hpp index e1f06a572..60c1fd1d1 100644 --- a/source/state_representation/include/state_representation/space/SpatialState.hpp +++ b/source/state_representation/include/state_representation/space/SpatialState.hpp @@ -9,13 +9,19 @@ class SpatialState : public State { public: /** - * @brief Empty constructor only specifying the type. + * @brief Empty constructor. + */ + SpatialState(); + + /** + * @brief Empty constructor with a specific state type. + * @param type The type of the State */ explicit SpatialState(const StateType& type); /** * @brief Constructor with name and reference frame specification. - * @param type The type of SpatialState (Cartesian or DualQuaternion) + * @param type The type of the State * @param name The name of the State * @param reference_frame The reference frame in which the state is expressed, by default world * @param empty Specify if the state is initialized as empty, default true 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 65aea975b..31abc95ab 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -109,8 +109,8 @@ class CartesianState : public SpatialState { /** * @brief Constructor with name and reference frame provided - * @brief name the name of the state - * @brief reference the name of the reference frame + * @param name the name of the state + * @param reference the name of the reference frame */ explicit CartesianState(const std::string& name, const std::string& reference = "world"); diff --git a/source/state_representation/src/State.cpp b/source/state_representation/src/State.cpp index 1e50c9cc5..242c5ee34 100644 --- a/source/state_representation/src/State.cpp +++ b/source/state_representation/src/State.cpp @@ -16,6 +16,52 @@ State::State(const State& state) : empty_(state.empty_), timestamp_(std::chrono::steady_clock::now()) {} +const StateType& State::get_type() const { + return this->type_; +} + +bool State::is_empty() const { + return this->empty_; +} + +void State::set_empty(bool empty) { + this->empty_ = empty; +} + +void State::set_filled() { + this->empty_ = false; + this->reset_timestamp(); +} + +const std::chrono::time_point& State::get_timestamp() const { + return this->timestamp_; +} + +void State::set_timestamp(const std::chrono::time_point& timepoint) { + this->timestamp_ = timepoint; +} + +void State::reset_timestamp() { + this->timestamp_ = std::chrono::steady_clock::now(); +} + +const std::string& State::get_name() const { + return this->name_; +} + +void State::set_name(const std::string& name) { + this->name_ = name; +} + +bool State::is_compatible(const State& state) const { + bool compatible = (this->name_ == state.name_); + return compatible; +} + +void State::initialize() { + this->empty_ = true; +} + void State::set_data(const Eigen::VectorXd&) { throw exceptions::NotImplementedException("set_data() is not implemented for the base State class"); } @@ -35,4 +81,9 @@ std::ostream& operator<<(std::ostream& os, const State& state) { os << " State: " << state.get_name(); return os; } + +void State::set_type(const StateType& type) { + this->type_ = type; +} + }// namespace state_representation diff --git a/source/state_representation/src/parameters/Parameter.cpp b/source/state_representation/src/parameters/Parameter.cpp index f6e3bc05d..b0c13f0bb 100644 --- a/source/state_representation/src/parameters/Parameter.cpp +++ b/source/state_representation/src/parameters/Parameter.cpp @@ -8,147 +8,148 @@ namespace state_representation { template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_INT, name) {} + ParameterInterface(name, ParameterType::INT) {} template<> Parameter::Parameter(const std::string& name, const int& value) : - ParameterInterface(StateType::PARAMETER_INT, name), value_(value) { + ParameterInterface(name, ParameterType::INT), value_(value) { this->set_filled(); } template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_INT_ARRAY, name) {} + ParameterInterface(name, ParameterType::INT_ARRAY) {} template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : - ParameterInterface(StateType::PARAMETER_INT_ARRAY, name), value_(value) { + ParameterInterface(name, ParameterType::INT_ARRAY), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_DOUBLE, name) {} + ParameterInterface(name, ParameterType::DOUBLE) {} template<> Parameter::Parameter(const std::string& name, const double& value) : - ParameterInterface(StateType::PARAMETER_DOUBLE, name), value_(value) { + ParameterInterface(name, ParameterType::DOUBLE), value_(value) { this->set_filled(); } template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_DOUBLE_ARRAY, name) {} + ParameterInterface(name, ParameterType::DOUBLE_ARRAY) {} template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : - ParameterInterface(StateType::PARAMETER_DOUBLE_ARRAY, name), value_(value) { + ParameterInterface(name, ParameterType::DOUBLE_ARRAY), value_(value) { this->set_filled(); } template<> -Parameter::Parameter(const std::string& name) : ParameterInterface(StateType::PARAMETER_BOOL, name) {} +Parameter::Parameter(const std::string& name) : + ParameterInterface(name, ParameterType::BOOL) {} template<> Parameter::Parameter(const std::string& name, const bool& value) : - ParameterInterface(StateType::PARAMETER_BOOL, name), value_(value) { + ParameterInterface(name, ParameterType::BOOL), value_(value) { this->set_filled(); } template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_BOOL_ARRAY, name) { + ParameterInterface(name, ParameterType::BOOL_ARRAY) { } template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : - ParameterInterface(StateType::PARAMETER_BOOL_ARRAY, name), value_(value) { + ParameterInterface(name, ParameterType::BOOL_ARRAY), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_STRING, name) {} + ParameterInterface(name, ParameterType::STRING) {} template<> Parameter::Parameter(const std::string& name, const std::string& value) : - ParameterInterface(StateType::PARAMETER_STRING, name), value_(value) { + ParameterInterface(name, ParameterType::STRING), value_(value) { this->set_filled(); } template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_STRING_ARRAY, name) {} + ParameterInterface(name, ParameterType::STRING_ARRAY) {} template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : - ParameterInterface(StateType::PARAMETER_STRING_ARRAY, name), value_(value) { + ParameterInterface(name, ParameterType::STRING_ARRAY), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_CARTESIANSTATE, name) {} + ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_STATE) {} template<> Parameter::Parameter(const std::string& name, const CartesianState& value) : - ParameterInterface(StateType::PARAMETER_CARTESIANSTATE, name), value_(value) { + ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_STATE), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_CARTESIANPOSE, name) {} + ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_POSE) {} template<> Parameter::Parameter(const std::string& name, const CartesianPose& value) : - ParameterInterface(StateType::PARAMETER_CARTESIANPOSE, name), value_(value) { + ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_POSE), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_JOINTSTATE, name) {} + ParameterInterface(name, ParameterType::STATE, StateType::JOINT_STATE) {} template<> Parameter::Parameter(const std::string& name, const JointState& value) : - ParameterInterface(StateType::PARAMETER_JOINTSTATE, name), value_(value) { + ParameterInterface(name, ParameterType::STATE, StateType::JOINT_STATE), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_JOINTPOSITIONS, name) {} + ParameterInterface(name, ParameterType::STATE, StateType::JOINT_POSITIONS) {} template<> Parameter::Parameter(const std::string& name, const JointPositions& value) : - ParameterInterface(StateType::PARAMETER_JOINTPOSITIONS, name), value_(value) { + ParameterInterface(name, ParameterType::STATE, StateType::JOINT_POSITIONS), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name, const Ellipsoid& value) : - ParameterInterface(StateType::PARAMETER_ELLIPSOID, name), value_(value) { + ParameterInterface(name, ParameterType::STATE, StateType::GEOMETRY_ELLIPSOID), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_MATRIX, name) {} + ParameterInterface(name, ParameterType::MATRIX) {} template<> Parameter::Parameter(const std::string& name, const Eigen::MatrixXd& value) : - ParameterInterface(StateType::PARAMETER_MATRIX, name), value_(value) { + ParameterInterface(name, ParameterType::MATRIX), value_(value) { this->set_filled(); } template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(StateType::PARAMETER_VECTOR, name) {} + ParameterInterface(name, ParameterType::VECTOR) {} template<> Parameter::Parameter(const std::string& name, const Eigen::VectorXd& value) : - ParameterInterface(StateType::PARAMETER_VECTOR, name), value_(value) { + ParameterInterface(name, ParameterType::VECTOR), value_(value) { this->set_filled(); } diff --git a/source/state_representation/src/parameters/ParameterInterface.cpp b/source/state_representation/src/parameters/ParameterInterface.cpp index ce22e7e3c..442aef4fb 100644 --- a/source/state_representation/src/parameters/ParameterInterface.cpp +++ b/source/state_representation/src/parameters/ParameterInterface.cpp @@ -2,7 +2,9 @@ namespace state_representation { -ParameterInterface::ParameterInterface(const StateType& type, const std::string& name) : State(type, name) {} +ParameterInterface::ParameterInterface( + const std::string& name, const ParameterType& type, const StateType& parameter_state_type +) : State(StateType::PARAMETER, name), parameter_type_(type), parameter_state_type_(parameter_state_type) {} ParameterInterface::ParameterInterface(const ParameterInterface& parameter) : State(parameter) {} @@ -10,4 +12,13 @@ ParameterInterface& ParameterInterface::operator=(const ParameterInterface& stat State::operator=(state); return (*this); } + +ParameterType ParameterInterface::get_parameter_type() const { + return parameter_type_; +} + +StateType ParameterInterface::get_parameter_state_type() const { + return parameter_state_type_; +} + }// namespace state_representation diff --git a/source/state_representation/src/space/Jacobian.cpp b/source/state_representation/src/space/Jacobian.cpp index 90653920b..dab175f62 100644 --- a/source/state_representation/src/space/Jacobian.cpp +++ b/source/state_representation/src/space/Jacobian.cpp @@ -4,7 +4,7 @@ #include "state_representation/exceptions/IncompatibleStatesException.hpp" namespace state_representation { -Jacobian::Jacobian() : State(StateType::JACOBIANMATRIX) { +Jacobian::Jacobian() : State(StateType::JACOBIAN) { this->State::initialize(); } @@ -12,7 +12,7 @@ Jacobian::Jacobian(const std::string& robot_name, unsigned int nb_joints, const std::string& frame, const std::string& reference_frame) : - State(StateType::JACOBIANMATRIX, robot_name), + State(StateType::JACOBIAN, robot_name), joint_names_(nb_joints), frame_(frame), reference_frame_(reference_frame), @@ -26,7 +26,7 @@ Jacobian::Jacobian(const std::string& robot_name, const std::vector& joint_names, const std::string& frame, const std::string& reference_frame) : - State(StateType::JACOBIANMATRIX, robot_name), + State(StateType::JACOBIAN, robot_name), joint_names_(joint_names), frame_(frame), reference_frame_(reference_frame), @@ -88,7 +88,7 @@ Jacobian Jacobian::Random(const std::string& robot_name, bool Jacobian::is_compatible(const State& state) const { bool compatible = false; switch (state.get_type()) { - case StateType::JACOBIANMATRIX: + case StateType::JACOBIAN: // compatibility is assured through the vector of joint names compatible = (this->get_name() == state.get_name()) && (this->cols_ == dynamic_cast(state).get_joint_names().size()); @@ -101,7 +101,11 @@ bool Jacobian::is_compatible(const State& state) const { && (this->frame_ == dynamic_cast(state).get_frame()))); } break; - case StateType::JOINTSTATE: + case StateType::JOINT_STATE: + case StateType::JOINT_POSITIONS: + case StateType::JOINT_VELOCITIES: + case StateType::JOINT_ACCELERATIONS: + case StateType::JOINT_TORQUES: // compatibility is assured through the vector of joint names compatible = (this->get_name() == state.get_name()) && (this->cols_ == dynamic_cast(state).get_size()); @@ -111,7 +115,11 @@ bool Jacobian::is_compatible(const State& state) const { } } break; - case StateType::CARTESIANSTATE: + case StateType::CARTESIAN_STATE: + case StateType::CARTESIAN_POSE: + case StateType::CARTESIAN_TWIST: + case StateType::CARTESIAN_ACCELERATION: + case StateType::CARTESIAN_WRENCH: // compatibility is assured through the reference frame and the name of the frame compatible = (this->reference_frame_ == dynamic_cast(state).get_reference_frame()) && (this->frame_ == dynamic_cast(state).get_name()); diff --git a/source/state_representation/src/space/SpatialState.cpp b/source/state_representation/src/space/SpatialState.cpp index f75ef1da0..185babced 100644 --- a/source/state_representation/src/space/SpatialState.cpp +++ b/source/state_representation/src/space/SpatialState.cpp @@ -1,6 +1,10 @@ #include "state_representation/space/SpatialState.hpp" namespace state_representation { + +SpatialState::SpatialState() : + State(StateType::SPATIAL_STATE), reference_frame_("world") {} + SpatialState::SpatialState(const StateType& type) : State(type), reference_frame_("world") {} diff --git a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp index 55074f1cd..db0961240 100644 --- a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp +++ b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp @@ -5,11 +5,14 @@ using namespace state_representation::exceptions; namespace state_representation { CartesianAcceleration::CartesianAcceleration(const std::string& name, const std::string& reference) : - CartesianState(name, reference) {} + CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_ACCELERATION); +} CartesianAcceleration::CartesianAcceleration( const std::string& name, const Eigen::Vector3d& linear_acceleration, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_ACCELERATION); this->set_linear_acceleration(linear_acceleration); } @@ -17,6 +20,7 @@ CartesianAcceleration::CartesianAcceleration( const std::string& name, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_acceleration, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_ACCELERATION); this->set_linear_acceleration(linear_acceleration); this->set_angular_acceleration(angular_acceleration); } @@ -24,11 +28,13 @@ CartesianAcceleration::CartesianAcceleration( CartesianAcceleration::CartesianAcceleration( const std::string& name, const Eigen::Matrix& acceleration, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_ACCELERATION); this->set_acceleration(acceleration); } 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()); diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp index 2259c3c52..5cabd1a91 100644 --- a/source/state_representation/src/space/cartesian/CartesianPose.cpp +++ b/source/state_representation/src/space/cartesian/CartesianPose.cpp @@ -5,22 +5,27 @@ using namespace state_representation::exceptions; namespace state_representation { -CartesianPose::CartesianPose(const std::string& name, const std::string& reference) : CartesianState(name, reference) {} +CartesianPose::CartesianPose(const std::string& name, const std::string& reference) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_POSE); +} CartesianPose::CartesianPose(const std::string& name, const Eigen::Vector3d& position, const std::string& reference) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_POSE); this->set_position(position); } CartesianPose::CartesianPose( const std::string& name, double x, double y, double z, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_POSE); this->set_position(x, y, z); } CartesianPose::CartesianPose( const std::string& name, const Eigen::Quaterniond& orientation, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_POSE); this->set_orientation(orientation); } @@ -28,12 +33,14 @@ CartesianPose::CartesianPose( const std::string& name, const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_POSE); this->set_position(position); this->set_orientation(orientation); } 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()); diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index a593c4659..b4319c3b9 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -6,12 +6,12 @@ using namespace state_representation::exceptions; namespace state_representation { -CartesianState::CartesianState() : SpatialState(StateType::CARTESIANSTATE) { +CartesianState::CartesianState() : SpatialState(StateType::CARTESIAN_STATE) { this->initialize(); } -CartesianState::CartesianState(const std::string& robot_name, const std::string& reference) : - SpatialState(StateType::CARTESIANSTATE, robot_name, reference) { +CartesianState::CartesianState(const std::string& name, const std::string& reference) : + SpatialState(StateType::CARTESIAN_STATE, name, reference) { this->initialize(); } diff --git a/source/state_representation/src/space/cartesian/CartesianTwist.cpp b/source/state_representation/src/space/cartesian/CartesianTwist.cpp index f63745de6..20e64a31f 100644 --- a/source/state_representation/src/space/cartesian/CartesianTwist.cpp +++ b/source/state_representation/src/space/cartesian/CartesianTwist.cpp @@ -5,11 +5,14 @@ using namespace state_representation::exceptions; namespace state_representation { CartesianTwist::CartesianTwist(const std::string& name, const std::string& reference) : - CartesianState(name, reference) {} + CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_TWIST); +} CartesianTwist::CartesianTwist( const std::string& name, const Eigen::Vector3d& linear_velocity, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_TWIST); this->set_linear_velocity(linear_velocity); } @@ -17,6 +20,7 @@ CartesianTwist::CartesianTwist( const std::string& name, const Eigen::Vector3d& linear_velocity, const Eigen::Vector3d& angular_velocity, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_TWIST); this->set_linear_velocity(linear_velocity); this->set_angular_velocity(angular_velocity); } @@ -24,11 +28,13 @@ CartesianTwist::CartesianTwist( CartesianTwist::CartesianTwist( const std::string& name, const Eigen::Matrix& twist, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_TWIST); this->set_twist(twist); } 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()); diff --git a/source/state_representation/src/space/cartesian/CartesianWrench.cpp b/source/state_representation/src/space/cartesian/CartesianWrench.cpp index 4e5dc889a..31221618e 100644 --- a/source/state_representation/src/space/cartesian/CartesianWrench.cpp +++ b/source/state_representation/src/space/cartesian/CartesianWrench.cpp @@ -4,16 +4,20 @@ using namespace state_representation::exceptions; namespace state_representation { CartesianWrench::CartesianWrench(const std::string& name, const std::string& reference) : - CartesianState(name, reference) {} + CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_WRENCH); +} CartesianWrench::CartesianWrench(const std::string& name, const Eigen::Vector3d& force, const std::string& reference) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_WRENCH); this->set_force(force); } CartesianWrench::CartesianWrench( const std::string& name, const Eigen::Vector3d& force, const Eigen::Vector3d& torque, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_WRENCH); this->set_force(force); this->set_torque(torque); } @@ -21,11 +25,13 @@ CartesianWrench::CartesianWrench( CartesianWrench::CartesianWrench( const std::string& name, const Eigen::Matrix& wrench, const std::string& reference ) : CartesianState(name, reference) { + this->set_type(StateType::CARTESIAN_WRENCH); this->set_wrench(wrench); } 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()); diff --git a/source/state_representation/src/space/joint/JointAccelerations.cpp b/source/state_representation/src/space/joint/JointAccelerations.cpp index 52b99ffe8..e109262e9 100644 --- a/source/state_representation/src/space/joint/JointAccelerations.cpp +++ b/source/state_representation/src/space/joint/JointAccelerations.cpp @@ -5,24 +5,30 @@ using namespace state_representation::exceptions; namespace state_representation { JointAccelerations::JointAccelerations(const std::string& robot_name, unsigned int nb_joints) : - JointState(robot_name, nb_joints) {} + JointState(robot_name, nb_joints) { + this->set_type(StateType::JOINT_ACCELERATIONS); +} JointAccelerations::JointAccelerations(const std::string& robot_name, const Eigen::VectorXd& accelerations) : JointState(robot_name, accelerations.size()) { + this->set_type(StateType::JOINT_ACCELERATIONS); this->set_accelerations(accelerations); } JointAccelerations::JointAccelerations(const std::string& robot_name, const std::vector& joint_names) : - JointState(robot_name, joint_names) {} + JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_ACCELERATIONS);} JointAccelerations::JointAccelerations(const std::string& robot_name, const std::vector& joint_names, const Eigen::VectorXd& accelerations) : JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_ACCELERATIONS); this->set_accelerations(accelerations); } 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()); diff --git a/source/state_representation/src/space/joint/JointPositions.cpp b/source/state_representation/src/space/joint/JointPositions.cpp index 767222cf3..ae9f45334 100644 --- a/source/state_representation/src/space/joint/JointPositions.cpp +++ b/source/state_representation/src/space/joint/JointPositions.cpp @@ -6,24 +6,31 @@ using namespace state_representation::exceptions; namespace state_representation { JointPositions::JointPositions(const std::string& robot_name, unsigned int nb_joints) : - JointState(robot_name, nb_joints) {} + JointState(robot_name, nb_joints) { + this->set_type(StateType::JOINT_POSITIONS); +} JointPositions::JointPositions(const std::string& robot_name, const Eigen::VectorXd& positions) : JointState(robot_name, positions.size()) { + this->set_type(StateType::JOINT_POSITIONS); this->set_positions(positions); } JointPositions::JointPositions(const std::string& robot_name, const std::vector& joint_names) : - JointState(robot_name, joint_names) {} + JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_POSITIONS); +} JointPositions::JointPositions( const std::string& robot_name, const std::vector& joint_names, const Eigen::VectorXd& positions ) : JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_POSITIONS); this->set_positions(positions); } 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()); diff --git a/source/state_representation/src/space/joint/JointState.cpp b/source/state_representation/src/space/joint/JointState.cpp index fbf14d639..136a6ac06 100644 --- a/source/state_representation/src/space/joint/JointState.cpp +++ b/source/state_representation/src/space/joint/JointState.cpp @@ -13,18 +13,18 @@ static void assert_index_in_range(unsigned int joint_index, unsigned int size) { } } -JointState::JointState() : State(StateType::JOINTSTATE) { +JointState::JointState() : State(StateType::JOINT_STATE) { this->initialize(); } JointState::JointState(const std::string& robot_name, unsigned int nb_joints) : - State(StateType::JOINTSTATE, robot_name), names_(nb_joints) { + State(StateType::JOINT_STATE, robot_name), names_(nb_joints) { this->set_names(nb_joints); this->initialize(); } JointState::JointState(const std::string& robot_name, const std::vector& joint_names) : - State(StateType::JOINTSTATE, robot_name), names_(joint_names) { + State(StateType::JOINT_STATE, robot_name), names_(joint_names) { this->initialize(); } diff --git a/source/state_representation/src/space/joint/JointTorques.cpp b/source/state_representation/src/space/joint/JointTorques.cpp index bf6f8a1b8..a59a5872e 100644 --- a/source/state_representation/src/space/joint/JointTorques.cpp +++ b/source/state_representation/src/space/joint/JointTorques.cpp @@ -3,23 +3,30 @@ using namespace state_representation::exceptions; namespace state_representation { -JointTorques::JointTorques(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) {} +JointTorques::JointTorques(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) { + this->set_type(StateType::JOINT_TORQUES); +} JointTorques::JointTorques(const std::string& robot_name, const Eigen::VectorXd& torques) : JointState(robot_name, torques.size()) { + this->set_type(StateType::JOINT_TORQUES); this->set_torques(torques); } JointTorques::JointTorques(const std::string& robot_name, const std::vector& joint_names) : - JointState(robot_name, joint_names) {} + JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_TORQUES); +} JointTorques::JointTorques(const std::string& robot_name, const std::vector& joint_names, const Eigen::VectorXd& torques) : JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_TORQUES); this->set_torques(torques); } JointTorques::JointTorques(const JointState& state) : JointState(state) { // set all the state variables to 0 except torques + this->set_type(StateType::JOINT_TORQUES); this->set_zero(); this->set_torques(state.get_torques()); this->set_empty(state.is_empty()); diff --git a/source/state_representation/src/space/joint/JointVelocities.cpp b/source/state_representation/src/space/joint/JointVelocities.cpp index 33fdd9759..cfb597e60 100644 --- a/source/state_representation/src/space/joint/JointVelocities.cpp +++ b/source/state_representation/src/space/joint/JointVelocities.cpp @@ -5,24 +5,31 @@ using namespace state_representation::exceptions; namespace state_representation { JointVelocities::JointVelocities(const std::string& robot_name, unsigned int nb_joints) : - JointState(robot_name, nb_joints) {} + JointState(robot_name, nb_joints) { + this->set_type(StateType::JOINT_VELOCITIES); +} JointVelocities::JointVelocities(const std::string& robot_name, const Eigen::VectorXd& velocities) : JointState(robot_name, velocities.size()) { + this->set_type(StateType::JOINT_VELOCITIES); this->set_velocities(velocities); } JointVelocities::JointVelocities(const std::string& robot_name, const std::vector& joint_names) : - JointState(robot_name, joint_names) {} + JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_VELOCITIES); +} JointVelocities::JointVelocities( const std::string& robot_name, const std::vector& joint_names, const Eigen::VectorXd& velocities ) : JointState(robot_name, joint_names) { + this->set_type(StateType::JOINT_VELOCITIES); this->set_velocities(velocities); } 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()); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp index 667a166bf..f569217c3 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp @@ -14,6 +14,7 @@ static void expect_only_acceleration(CartesianAcceleration& acceleration) { TEST(CartesianAccelerationTest, RandomAccelerationInitialization) { CartesianAcceleration random = CartesianAcceleration::Random("test"); + EXPECT_EQ(random.get_type(), StateType::CARTESIAN_ACCELERATION); EXPECT_GT(random.get_acceleration().norm(), 0); expect_only_acceleration(random); } @@ -21,12 +22,14 @@ TEST(CartesianAccelerationTest, RandomAccelerationInitialization) { TEST(CartesianAccelerationTest, CopyAcceleration) { CartesianAcceleration acc1 = CartesianAcceleration::Random("test"); CartesianAcceleration acc2(acc1); + EXPECT_EQ(acc1.get_type(), acc2.get_type()); EXPECT_EQ(acc1.get_name(), acc2.get_name()); EXPECT_EQ(acc1.get_reference_frame(), acc2.get_reference_frame()); EXPECT_EQ(acc1.data(), acc2.data()); expect_only_acceleration(acc2); CartesianAcceleration acc3 = acc1; + EXPECT_EQ(acc1.get_type(), acc3.get_type()); EXPECT_EQ(acc1.get_name(), acc3.get_name()); EXPECT_EQ(acc1.get_reference_frame(), acc3.get_reference_frame()); EXPECT_EQ(acc1.data(), acc3.data()); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp index 80a1ea4dd..18398d37e 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp @@ -36,6 +36,7 @@ class CartesianPoseTestClass : public testing::Test { TEST(CartesianPoseTest, RandomPoseInitialization) { CartesianPose random = CartesianPose::Random("test"); + EXPECT_EQ(random.get_type(), StateType::CARTESIAN_POSE); EXPECT_NE(random.get_position().norm(), 0); EXPECT_NE(random.get_orientation().w(), 0); EXPECT_NE(random.get_orientation().x(), 0); @@ -47,12 +48,14 @@ TEST(CartesianPoseTest, RandomPoseInitialization) { TEST(CartesianPoseTest, CopyPose) { CartesianPose pose1 = CartesianPose::Random("test"); CartesianPose pose2(pose1); + EXPECT_EQ(pose1.get_type(), pose2.get_type()); EXPECT_EQ(pose1.get_name(), pose2.get_name()); EXPECT_EQ(pose1.get_reference_frame(), pose2.get_reference_frame()); EXPECT_EQ(pose1.data(), pose2.data()); expect_only_pose(pose2); CartesianPose pose3 = pose1; + EXPECT_EQ(pose1.get_type(), pose3.get_type()); EXPECT_EQ(pose1.get_name(), pose3.get_name()); EXPECT_EQ(pose1.get_reference_frame(), pose3.get_reference_frame()); EXPECT_EQ(pose1.data(), pose3.data()); @@ -152,6 +155,7 @@ TEST(CartesianPoseTest, MultiplyPoseAndState) { CartesianState s = CartesianPose::Random("test2", "test"); CartesianState res = p * s; CartesianPose res2 = p * static_cast(s); + EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_POSE); expect_near(res.get_position(), res2.get_position()); EXPECT_GT(abs(res.get_orientation().dot(res2.get_orientation())), 1 - 1e-5); } @@ -162,6 +166,7 @@ TEST_F(CartesianPoseTestClass, TestAddTwoPoses) { Eigen::Quaterniond rot2(0, 1, 0, 0); tf2 = CartesianPose("t1", pos2, rot2); auto tf3 = tf1 + tf2; + EXPECT_EQ(tf3.get_type(), StateType::CARTESIAN_POSE); Eigen::Vector3d pos_truth(1, 0, 0); Eigen::Quaterniond rot_truth(0, -1, 0, 0); expect_near(tf3.get_position(), pos_truth); @@ -173,18 +178,22 @@ TEST_F(CartesianPoseTestClass, TestPoseToVelocity) { std::chrono::seconds dt1(1); std::chrono::milliseconds dt2(100); auto res1 = tf1 / dt1; + EXPECT_EQ(res1.get_type(), StateType::CARTESIAN_TWIST); EXPECT_EQ(res1.get_linear_velocity(), tf1.get_position()); EXPECT_EQ(res1.get_angular_velocity(), Eigen::Vector3d(M_PI, 0, 0)); auto res2 = tf1 / dt2; + EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_TWIST); EXPECT_EQ(res2.get_linear_velocity(), 10 * tf1.get_position()); EXPECT_EQ(res2.get_angular_velocity(), 10 * Eigen::Vector3d(M_PI, 0, 0)); } TEST_F(CartesianPoseTestClass, TestImplicitConversion) { CartesianTwist vel("t1"); + EXPECT_EQ(vel.get_type(), StateType::CARTESIAN_TWIST); vel.set_linear_velocity(Eigen::Vector3d(0.1, 0.1, 0.1)); vel.set_angular_velocity(Eigen::Vector3d(M_PI, 0, 0)); tf1 += vel; + EXPECT_EQ(tf1.get_type(), StateType::CARTESIAN_POSE); Eigen::Vector3d pos_truth(1.1, 2.1, 3.1); Eigen::Quaterniond rot_truth(0, 1, 0, 0); EXPECT_EQ(tf1.get_position(), pos_truth); 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 2773610ad..0fa832529 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 @@ -12,7 +12,7 @@ static void assert_name_empty_frame_equal( const CartesianState& state1, const std::string& name, bool empty, const std::string& reference_frame ) { EXPECT_EQ(state1.get_name(), name); - EXPECT_EQ(state1.get_type(), StateType::CARTESIANSTATE); + EXPECT_EQ(state1.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(state1.is_empty(), empty); EXPECT_EQ(state1.get_reference_frame(), reference_frame); } diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp index b630bc2f2..845ee0715 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp @@ -14,6 +14,7 @@ static void expect_only_twist(CartesianTwist& twist) { TEST(CartesianTwistTest, RandomTwistInitialization) { CartesianTwist random = CartesianTwist::Random("test"); + EXPECT_EQ(random.get_type(), StateType::CARTESIAN_TWIST); EXPECT_GT(random.get_twist().norm(), 0); expect_only_twist(random); } @@ -21,12 +22,14 @@ TEST(CartesianTwistTest, RandomTwistInitialization) { TEST(CartesianTwistTest, CopyTwist) { CartesianTwist twist1 = CartesianTwist::Random("test"); CartesianTwist twist2(twist1); + EXPECT_EQ(twist1.get_type(), twist2.get_type()); EXPECT_EQ(twist1.get_name(), twist2.get_name()); EXPECT_EQ(twist1.get_reference_frame(), twist2.get_reference_frame()); EXPECT_EQ(twist1.data(), twist2.data()); expect_only_twist(twist2); CartesianTwist twist3 = twist1; + EXPECT_EQ(twist1.get_type(), twist3.get_type()); EXPECT_EQ(twist1.get_name(), twist3.get_name()); EXPECT_EQ(twist1.get_reference_frame(), twist3.get_reference_frame()); EXPECT_EQ(twist1.data(), twist3.data()); @@ -115,9 +118,11 @@ TEST(CartesianTwistTest, TestVelocityToAcceleration) { std::chrono::seconds dt1(1); std::chrono::milliseconds dt2(100); auto res1 = twist / dt1; + EXPECT_EQ(res1.get_type(), StateType::CARTESIAN_ACCELERATION); EXPECT_EQ(res1.get_linear_acceleration(), twist.get_linear_velocity()); EXPECT_EQ(res1.get_angular_acceleration(), twist.get_angular_velocity()); auto res2 = twist / dt2; + EXPECT_EQ(res2.get_type(), StateType::CARTESIAN_ACCELERATION); EXPECT_TRUE(res2.get_linear_acceleration().isApprox(10 * twist.get_linear_velocity())); EXPECT_TRUE(res2.get_angular_acceleration().isApprox(10 * twist.get_angular_velocity())); } @@ -127,6 +132,7 @@ TEST(CartesianTwistTest, TestImplicitConversion) { acc.set_linear_acceleration(Eigen::Vector3d(0.1, 0.1, 0.1)); acc.set_linear_acceleration(Eigen::Vector3d(M_PI, 0, 0)); CartesianTwist twist(acc); + EXPECT_EQ(twist.get_type(), StateType::CARTESIAN_TWIST); EXPECT_EQ(twist.get_linear_velocity(), acc.get_linear_acceleration()); EXPECT_EQ(twist.get_angular_velocity(), acc.get_angular_acceleration()); } diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp index 6a002dc1a..2b7701652 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp @@ -14,6 +14,7 @@ static void expect_only_wrench(CartesianWrench& wrench) { TEST(CartesianWrenchTest, RandomWrenchInitialization) { CartesianWrench random = CartesianWrench::Random("test"); + EXPECT_EQ(random.get_type(), StateType::CARTESIAN_WRENCH); EXPECT_NE(random.get_wrench().norm(), 0); expect_only_wrench(random); } @@ -21,12 +22,14 @@ TEST(CartesianWrenchTest, RandomWrenchInitialization) { TEST(CartesianWrenchTest, CopyWrench) { CartesianWrench wrench1 = CartesianWrench::Random("test"); CartesianWrench wrench2(wrench1); + EXPECT_EQ(wrench1.get_type(), wrench2.get_type()); EXPECT_EQ(wrench1.get_name(), wrench2.get_name()); EXPECT_EQ(wrench1.get_reference_frame(), wrench2.get_reference_frame()); EXPECT_EQ(wrench1.data(), wrench2.data()); expect_only_wrench(wrench2); CartesianWrench wrench3 = wrench1; + EXPECT_EQ(wrench1.get_type(), wrench3.get_type()); EXPECT_EQ(wrench1.get_name(), wrench3.get_name()); EXPECT_EQ(wrench1.get_reference_frame(), wrench3.get_reference_frame()); EXPECT_EQ(wrench1.data(), wrench3.data()); diff --git a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp index 272c28638..8683a4a2b 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp @@ -14,6 +14,7 @@ TEST(JointAccelerationsTest, Constructors) { std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d accelerations = Eigen::Vector2d::Random(); JointAccelerations ja1("test", accelerations); + EXPECT_EQ(ja1.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(ja1.get_name(), "test"); EXPECT_FALSE(ja1.is_empty()); EXPECT_EQ(ja1.get_size(), accelerations.size()); @@ -23,6 +24,7 @@ TEST(JointAccelerationsTest, Constructors) { EXPECT_EQ(ja1.data(), accelerations); JointAccelerations ja2("test", joint_names, accelerations); + EXPECT_EQ(ja2.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(ja2.get_name(), "test"); EXPECT_FALSE(ja2.is_empty()); EXPECT_EQ(ja2.get_size(), joint_names.size()); @@ -34,7 +36,9 @@ TEST(JointAccelerationsTest, Constructors) { TEST(JointAccelerationsTest, StateCopyConstructor) { JointState random_state = JointState::Random("test", 3); + EXPECT_EQ(random_state.get_type(), StateType::JOINT_STATE); JointAccelerations copy1(random_state); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(random_state.get_name(), copy1.get_name()); EXPECT_EQ(random_state.get_names(), copy1.get_names()); EXPECT_EQ(random_state.get_size(), copy1.get_size()); @@ -42,6 +46,7 @@ TEST(JointAccelerationsTest, StateCopyConstructor) { expect_only_accelerations(copy1); JointAccelerations copy2 = random_state; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(random_state.get_name(), copy2.get_name()); EXPECT_EQ(random_state.get_names(), copy2.get_names()); EXPECT_EQ(random_state.get_size(), copy2.get_size()); @@ -50,16 +55,21 @@ TEST(JointAccelerationsTest, StateCopyConstructor) { JointState empty_state; JointAccelerations copy3(empty_state); + EXPECT_EQ(copy3.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_TRUE(copy3.is_empty()); JointAccelerations copy4 = empty_state; + EXPECT_EQ(copy4.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_TRUE(copy4.is_empty()); JointAccelerations copy5 = empty_state.copy(); + EXPECT_EQ(copy5.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_TRUE(copy5.is_empty()); } TEST(JointAccelerationsTest, VelocitiesCopyConstructor) { JointVelocities random_velocities = JointVelocities::Random("test", 3); + EXPECT_EQ(random_velocities.get_type(), StateType::JOINT_VELOCITIES); JointAccelerations copy1(random_velocities); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(random_velocities.get_name(), copy1.get_name()); EXPECT_EQ(random_velocities.get_names(), copy1.get_names()); EXPECT_EQ(random_velocities.get_size(), copy1.get_size()); @@ -67,6 +77,7 @@ TEST(JointAccelerationsTest, VelocitiesCopyConstructor) { expect_only_accelerations(copy1); JointAccelerations copy2 = random_velocities; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(random_velocities.get_name(), copy1.get_name()); EXPECT_EQ(random_velocities.get_names(), copy1.get_names()); EXPECT_EQ(random_velocities.get_size(), copy1.get_size()); @@ -80,10 +91,12 @@ TEST(JointAccelerationsTest, VelocitiesCopyConstructor) { TEST(JointAccelerationsTest, RandomInitialization) { JointAccelerations random1 = JointAccelerations::Random("test", 3); + EXPECT_EQ(random1.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_NE(random1.get_accelerations().norm(), 0); expect_only_accelerations(random1); JointAccelerations random2 = JointAccelerations::Random("test", std::vector{"j0", "j1"}); + EXPECT_EQ(random2.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_NE(random2.get_accelerations().norm(), 0); expect_only_accelerations(random2); } @@ -107,6 +120,7 @@ TEST(JointAccelerationsTest, Clamping) { EXPECT_EQ(ja2.data(), accelerations); ja2.clamp(3 * Eigen::ArrayXd::Ones(ja2.get_size()), 0.5 * Eigen::ArrayXd::Ones(ja2.get_size())); EXPECT_EQ(ja2.data(), result); + EXPECT_EQ(ja2.get_type(), StateType::JOINT_ACCELERATIONS); } TEST(JointAccelerationsTest, GetSetData) { @@ -128,11 +142,13 @@ TEST(JointAccelerationsTest, GetSetData) { EXPECT_EQ(state_vec.at(i), ja1.data()(i)); } EXPECT_THROW(ja1.set_data(Eigen::Vector2d::Zero()), exceptions::IncompatibleSizeException); + EXPECT_EQ(ja1.get_type(), StateType::JOINT_ACCELERATIONS); } TEST(JointAccelerationsTest, ScalarMultiplication) { JointAccelerations ja = JointAccelerations::Random("test", 3); JointAccelerations jscaled = 0.5 * ja; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(jscaled.data(), 0.5 * ja.data()); JointAccelerations empty; @@ -144,9 +160,11 @@ TEST(JointAccelerationsTest, MatrixMultiplication) { Eigen::MatrixXd gains = Eigen::VectorXd::Random(ja.get_size()).asDiagonal(); JointAccelerations jscaled = gains * ja; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(jscaled.data(), gains * ja.data()); EXPECT_EQ((ja * gains).data(), jscaled.data()); ja *= gains; + EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(jscaled.data(), ja.data()); JointAccelerations jscaled2 = ja * gains; @@ -159,9 +177,11 @@ TEST(JointAccelerationsTest, ArrayMultiplication) { Eigen::ArrayXd gains = Eigen::ArrayXd::Random(ja.get_size()); JointAccelerations jscaled = gains * ja; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(jscaled.data(), (gains * ja.array()).matrix()); EXPECT_EQ((ja * gains).data(), jscaled.data()); ja *= gains; + EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(jscaled.data(), ja.data()); gains = Eigen::ArrayXd::Random(2 * ja.get_size()); @@ -172,8 +192,10 @@ TEST(JointAccelerationsTest, ChronoMultiplication) { JointAccelerations ja = JointAccelerations::Random("test", 3); auto time = std::chrono::seconds(2); JointVelocities jv1 = ja * time; + EXPECT_EQ(jv1.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jv1.get_velocities(), ja.get_accelerations() * time.count()); JointVelocities jv2 = time * ja; + EXPECT_EQ(jv2.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jv2.get_velocities(), ja.get_accelerations() * time.count()); JointAccelerations empty; diff --git a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp index b5bed40b2..ad829ba97 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp @@ -14,6 +14,7 @@ TEST(JointPositionsTest, Constructors) { std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d positions = Eigen::Vector2d::Random(); JointPositions jp1("test", positions); + EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jp1.get_name(), "test"); EXPECT_FALSE(jp1.is_empty()); EXPECT_EQ(jp1.get_size(), positions.size()); @@ -23,6 +24,7 @@ TEST(JointPositionsTest, Constructors) { EXPECT_EQ(jp1.data(), positions); JointPositions jp2("test", joint_names, positions); + EXPECT_EQ(jp2.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jp2.get_name(), "test"); EXPECT_FALSE(jp2.is_empty()); EXPECT_EQ(jp2.get_size(), joint_names.size()); @@ -34,7 +36,9 @@ TEST(JointPositionsTest, Constructors) { TEST(JointPositionsTest, StateCopyConstructor) { JointState random_state = JointState::Random("test", 3); + EXPECT_EQ(random_state.get_type(), StateType::JOINT_STATE); JointPositions copy1(random_state); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(random_state.get_name(), copy1.get_name()); EXPECT_EQ(random_state.get_names(), copy1.get_names()); EXPECT_EQ(random_state.get_size(), copy1.get_size()); @@ -42,6 +46,7 @@ TEST(JointPositionsTest, StateCopyConstructor) { expect_only_positions(copy1); JointPositions copy2 = random_state; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(random_state.get_name(), copy2.get_name()); EXPECT_EQ(random_state.get_names(), copy2.get_names()); EXPECT_EQ(random_state.get_size(), copy2.get_size()); @@ -50,16 +55,21 @@ TEST(JointPositionsTest, StateCopyConstructor) { JointState empty_state; JointPositions copy3(empty_state); + EXPECT_EQ(copy3.get_type(), StateType::JOINT_POSITIONS); EXPECT_TRUE(copy3.is_empty()); JointPositions copy4 = empty_state; + EXPECT_EQ(copy4.get_type(), StateType::JOINT_POSITIONS); EXPECT_TRUE(copy4.is_empty()); JointPositions copy5 = empty_state.copy(); + EXPECT_EQ(copy5.get_type(), StateType::JOINT_POSITIONS); EXPECT_TRUE(copy5.is_empty()); } TEST(JointPositionsTest, VelocitiesCopyConstructor) { JointVelocities random_velocities = JointVelocities::Random("test", 3); + EXPECT_EQ(random_velocities.get_type(), StateType::JOINT_VELOCITIES); JointPositions copy1(random_velocities); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(random_velocities.get_name(), copy1.get_name()); EXPECT_EQ(random_velocities.get_names(), copy1.get_names()); EXPECT_EQ(random_velocities.get_size(), copy1.get_size()); @@ -67,6 +77,7 @@ TEST(JointPositionsTest, VelocitiesCopyConstructor) { expect_only_positions(copy1); JointPositions copy2 = random_velocities; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(random_velocities.get_name(), copy1.get_name()); EXPECT_EQ(random_velocities.get_names(), copy1.get_names()); EXPECT_EQ(random_velocities.get_size(), copy1.get_size()); @@ -80,10 +91,12 @@ TEST(JointPositionsTest, VelocitiesCopyConstructor) { TEST(JointPositionsTest, RandomInitialization) { JointPositions random1 = JointPositions::Random("test", 3); + EXPECT_EQ(random1.get_type(), StateType::JOINT_POSITIONS); EXPECT_NE(random1.get_positions().norm(), 0); expect_only_positions(random1); JointPositions random2 = JointPositions::Random("test", std::vector{"j0", "j1"}); + EXPECT_EQ(random2.get_type(), StateType::JOINT_POSITIONS); EXPECT_NE(random2.get_positions().norm(), 0); expect_only_positions(random2); } @@ -133,6 +146,7 @@ TEST(JointPositionsTest, GetSetData) { TEST(JointPositionsTest, ScalarMultiplication) { JointPositions jp = JointPositions::Random("test", 3); JointPositions jscaled = 0.5 * jp; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jscaled.data(), 0.5 * jp.data()); JointPositions empty; @@ -144,9 +158,11 @@ TEST(JointPositionsTest, MatrixMultiplication) { Eigen::MatrixXd gains = Eigen::VectorXd::Random(jp.get_size()).asDiagonal(); JointPositions jscaled = gains * jp; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jscaled.data(), gains * jp.data()); EXPECT_EQ((jp * gains).data(), jscaled.data()); jp *= gains; + EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jscaled.data(), jp.data()); JointPositions jscaled2 = jp * gains; @@ -159,9 +175,11 @@ TEST(JointPositionsTest, ArrayMultiplication) { Eigen::ArrayXd gains = Eigen::ArrayXd::Random(jp.get_size()); JointPositions jscaled = gains * jp; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jscaled.data(), (gains * jp.array()).matrix()); EXPECT_EQ((jp * gains).data(), jscaled.data()); jp *= gains; + EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jscaled.data(), jp.data()); gains = Eigen::ArrayXd::Random(2 * jp.get_size()); @@ -170,8 +188,10 @@ TEST(JointPositionsTest, ArrayMultiplication) { TEST(JointStateTest, ChronoDivision) { JointPositions jp = JointPositions::Random("test", 3); + EXPECT_EQ(jp.get_type(), StateType::JOINT_POSITIONS); auto time = std::chrono::seconds(1); JointVelocities jv = jp / time; + EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jv.get_velocities(), jp.get_positions() / time.count()); JointPositions empty; 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 index 3ca46a4b3..91d6e63d2 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_state.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_state.cpp @@ -10,13 +10,14 @@ using namespace state_representation; TEST(JointStateTest, Constructors) { JointState empty; + EXPECT_EQ(empty.get_type(), StateType::JOINT_STATE); EXPECT_EQ(empty.get_name(), ""); - EXPECT_EQ(empty.get_type(), StateType::JOINTSTATE); EXPECT_TRUE(empty.is_empty()); EXPECT_EQ(empty.get_size(), 0); EXPECT_EQ(empty.data().norm(), 0); JointState js1("test", 3); + EXPECT_EQ(js1.get_type(), StateType::JOINT_STATE); EXPECT_EQ(js1.get_name(), "test"); EXPECT_TRUE(js1.is_empty()); EXPECT_EQ(js1.get_size(), 3); @@ -27,6 +28,7 @@ TEST(JointStateTest, Constructors) { std::vector joint_names{"joint_10", "joint_20"}; JointState js2("test", joint_names); + EXPECT_EQ(js2.get_type(), StateType::JOINT_STATE); EXPECT_EQ(js2.get_name(), "test"); EXPECT_TRUE(js2.is_empty()); EXPECT_EQ(js2.get_size(), joint_names.size()); @@ -38,22 +40,26 @@ TEST(JointStateTest, Constructors) { TEST(JointStateTest, ZeroInitialization) { JointState zero = JointState::Zero("test", 3); + EXPECT_EQ(zero.get_type(), StateType::JOINT_STATE); EXPECT_FALSE(zero.is_empty()); EXPECT_EQ(zero.data().norm(), 0); JointState zero2 = JointState::Zero("test", std::vector{"j0", "j1"}); + EXPECT_EQ(zero2.get_type(), StateType::JOINT_STATE); EXPECT_FALSE(zero2.is_empty()); EXPECT_EQ(zero2.data().norm(), 0); } TEST(JointStateTest, RandomStateInitialization) { JointState random = JointState::Random("test", 3); + EXPECT_EQ(random.get_type(), StateType::JOINT_STATE); EXPECT_NE(random.get_positions().norm(), 0); EXPECT_NE(random.get_velocities().norm(), 0); EXPECT_NE(random.get_accelerations().norm(), 0); EXPECT_NE(random.get_torques().norm(), 0); JointState random2 = JointState::Random("test", std::vector{"j0", "j1"}); + EXPECT_EQ(random2.get_type(), StateType::JOINT_STATE); EXPECT_NE(random2.get_positions().norm(), 0); EXPECT_NE(random2.get_velocities().norm(), 0); EXPECT_NE(random2.get_accelerations().norm(), 0); @@ -63,18 +69,21 @@ TEST(JointStateTest, RandomStateInitialization) { TEST(JointStateTest, CopyConstructor) { JointState random = JointState::Random("test", 3); JointState copy1(random); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_STATE); EXPECT_EQ(random.get_name(), copy1.get_name()); EXPECT_EQ(random.get_names(), copy1.get_names()); EXPECT_EQ(random.get_size(), copy1.get_size()); EXPECT_EQ(random.data(), copy1.data()); JointState copy2 = random; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_STATE); EXPECT_EQ(random.get_name(), copy2.get_name()); EXPECT_EQ(random.get_names(), copy2.get_names()); EXPECT_EQ(random.get_size(), copy2.get_size()); EXPECT_EQ(random.data(), copy2.data()); JointState copy3 = random.copy(); + EXPECT_EQ(copy3.get_type(), StateType::JOINT_STATE); EXPECT_EQ(random.get_name(), copy3.get_name()); EXPECT_EQ(random.get_names(), copy3.get_names()); EXPECT_EQ(random.get_size(), copy3.get_size()); @@ -82,10 +91,13 @@ TEST(JointStateTest, CopyConstructor) { JointState empty; JointState copy4(empty); + EXPECT_EQ(copy4.get_type(), StateType::JOINT_STATE); EXPECT_TRUE(copy4.is_empty()); JointState copy5 = empty; + EXPECT_EQ(copy5.get_type(), StateType::JOINT_STATE); EXPECT_TRUE(copy5.is_empty()); JointState copy6 = empty.copy(); + EXPECT_EQ(copy6.get_type(), StateType::JOINT_STATE); EXPECT_TRUE(copy6.is_empty()); } @@ -152,9 +164,11 @@ TEST(JointStateTest, GetSetFields) { EXPECT_THROW(js.set_torques(Eigen::VectorXd::Zero(7)), exceptions::IncompatibleSizeException); js.set_zero(); + EXPECT_EQ(js.get_type(), StateType::JOINT_STATE); EXPECT_EQ(js.data().norm(), 0); EXPECT_EQ(js.is_empty(), false); js.set_empty(); + EXPECT_EQ(js.get_type(), StateType::JOINT_STATE); EXPECT_EQ(js.is_empty(), true); } @@ -213,6 +227,7 @@ TEST(JointStateTest, SetZero) { JointState random2 = JointState::Random("test", 3); random2.set_zero(); + EXPECT_EQ(random2.get_type(), StateType::JOINT_STATE); EXPECT_EQ(random2.data().norm(), 0); } @@ -327,8 +342,10 @@ TEST(JointStateTest, Addition) { JointState js3 = JointState::Random("test", 4); EXPECT_THROW(js1 + js3, exceptions::IncompatibleStatesException); JointState jsum = js1 + js2; + EXPECT_EQ(jsum.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jsum.data(), js1.data() + js2.data()); js2 += js1; + EXPECT_EQ(js2.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jsum.data(), js2.data()); } @@ -338,17 +355,21 @@ TEST(JointStateTest, Subtraction) { JointState js3 = JointState::Random("test", 4); EXPECT_THROW(js1 - js3, exceptions::IncompatibleStatesException); JointState jdiff = js1 - js2; + EXPECT_EQ(jdiff.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jdiff.data(), js1.data() - js2.data()); js1 -= js2; + EXPECT_EQ(js1.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jdiff.data(), js1.data()); } TEST(JointStateTest, ScalarMultiplication) { JointState js = JointState::Random("test", 3); JointState jscaled = 0.5 * js; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), 0.5 * js.data()); EXPECT_EQ((js * 0.5).data(), jscaled.data()); js *= 0.5; + EXPECT_EQ(js.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), js.data()); JointState empty; @@ -358,8 +379,10 @@ TEST(JointStateTest, ScalarMultiplication) { TEST(JointStateTest, ScalarDivision) { JointState js = JointState::Random("test", 3); JointState jscaled = js / 0.5; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), js.data() / 0.5); js /= 0.5; + EXPECT_EQ(js.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), js.data()); JointState empty; @@ -371,9 +394,11 @@ TEST(JointStateTest, MatrixMultiplication) { Eigen::MatrixXd gains = Eigen::VectorXd::Random(4 * js.get_size()).asDiagonal(); JointState jscaled = gains * js; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), gains * js.data()); EXPECT_EQ((js * gains).data(), jscaled.data()); js *= gains; + EXPECT_EQ(js.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), js.data()); JointState jscaled2 = js * gains; @@ -386,9 +411,11 @@ TEST(JointStateTest, ArrayMultiplication) { Eigen::ArrayXd gains = Eigen::ArrayXd::Random(4 * js.get_size()); JointState jscaled = gains * js; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), (gains * js.array()).matrix()); EXPECT_EQ((js * gains).data(), jscaled.data()); js *= gains; + EXPECT_EQ(js.get_type(), StateType::JOINT_STATE); EXPECT_EQ(jscaled.data(), js.data()); gains = Eigen::ArrayXd::Random(js.get_size()); diff --git a/source/state_representation/test/tests/space/joint/test_joint_torques.cpp b/source/state_representation/test/tests/space/joint/test_joint_torques.cpp index 9f60846e7..1caaeb115 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_torques.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_torques.cpp @@ -14,6 +14,7 @@ TEST(JointTorquesTest, Constructors) { std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d torques = Eigen::Vector2d::Random(); JointTorques jt1("test", torques); + EXPECT_EQ(jt1.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jt1.get_name(), "test"); EXPECT_FALSE(jt1.is_empty()); EXPECT_EQ(jt1.get_size(), torques.size()); @@ -23,6 +24,7 @@ TEST(JointTorquesTest, Constructors) { EXPECT_EQ(jt1.data(), torques); JointTorques jt2("test", joint_names, torques); + EXPECT_EQ(jt2.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jt2.get_name(), "test"); EXPECT_FALSE(jt2.is_empty()); EXPECT_EQ(jt2.get_size(), joint_names.size()); @@ -35,6 +37,7 @@ TEST(JointTorquesTest, Constructors) { TEST(JointTorquesTest, StateCopyConstructor) { JointState random_state = JointState::Random("test", 3); JointTorques copy1(random_state); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(random_state.get_name(), copy1.get_name()); EXPECT_EQ(random_state.get_names(), copy1.get_names()); EXPECT_EQ(random_state.get_size(), copy1.get_size()); @@ -42,6 +45,7 @@ TEST(JointTorquesTest, StateCopyConstructor) { expect_only_torques(copy1); JointTorques copy2 = random_state; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(random_state.get_name(), copy2.get_name()); EXPECT_EQ(random_state.get_names(), copy2.get_names()); EXPECT_EQ(random_state.get_size(), copy2.get_size()); @@ -50,19 +54,24 @@ TEST(JointTorquesTest, StateCopyConstructor) { JointState empty_state; JointTorques copy3(empty_state); + EXPECT_EQ(copy3.get_type(), StateType::JOINT_TORQUES); EXPECT_TRUE(copy3.is_empty()); JointTorques copy4 = empty_state; + EXPECT_EQ(copy4.get_type(), StateType::JOINT_TORQUES); EXPECT_TRUE(copy4.is_empty()); JointTorques copy5 = empty_state.copy(); + EXPECT_EQ(copy5.get_type(), StateType::JOINT_TORQUES); EXPECT_TRUE(copy5.is_empty()); } TEST(JointTorquesTest, RandomInitialization) { JointTorques random1 = JointTorques::Random("test", 3); + EXPECT_EQ(random1.get_type(), StateType::JOINT_TORQUES); EXPECT_NE(random1.get_torques().norm(), 0); expect_only_torques(random1); JointTorques random2 = JointTorques::Random("test", std::vector{"j0", "j1"}); + EXPECT_EQ(random2.get_type(), StateType::JOINT_TORQUES); EXPECT_NE(random2.get_torques().norm(), 0); expect_only_torques(random2); } @@ -112,6 +121,7 @@ TEST(JointTorquesTest, GetSetData) { TEST(JointTorquesTest, ScalarMultiplication) { JointTorques jt = JointTorques::Random("test", 3); JointTorques jscaled = 0.5 * jt; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jscaled.data(), 0.5 * jt.data()); JointTorques empty; @@ -123,9 +133,11 @@ TEST(JointTorquesTest, MatrixMultiplication) { Eigen::MatrixXd gains = Eigen::VectorXd::Random(jt.get_size()).asDiagonal(); JointTorques jscaled = gains * jt; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jscaled.data(), gains * jt.data()); EXPECT_EQ((jt * gains).data(), jscaled.data()); jt *= gains; + EXPECT_EQ(jt.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jscaled.data(), jt.data()); JointTorques jscaled2 = jt * gains; @@ -138,9 +150,11 @@ TEST(JointTorquesTest, ArrayMultiplication) { Eigen::ArrayXd gains = Eigen::ArrayXd::Random(jt.get_size()); JointTorques jscaled = gains * jt; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jscaled.data(), (gains * jt.array()).matrix()); EXPECT_EQ((jt * gains).data(), jscaled.data()); jt *= gains; + EXPECT_EQ(jt.get_type(), StateType::JOINT_TORQUES); EXPECT_EQ(jscaled.data(), jt.data()); gains = Eigen::ArrayXd::Random(2 * jt.get_size()); diff --git a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp index 14d640e96..28a3996b2 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp @@ -14,6 +14,7 @@ TEST(JointVelocitiesTest, Constructors) { std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d velocities = Eigen::Vector2d::Random(); JointVelocities jv1("test", velocities); + EXPECT_EQ(jv1.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jv1.get_name(), "test"); EXPECT_FALSE(jv1.is_empty()); EXPECT_EQ(jv1.get_size(), velocities.size()); @@ -23,6 +24,7 @@ TEST(JointVelocitiesTest, Constructors) { EXPECT_EQ(jv1.data(), velocities); JointVelocities jv2("test", joint_names, velocities); + EXPECT_EQ(jv2.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jv2.get_name(), "test"); EXPECT_FALSE(jv2.is_empty()); EXPECT_EQ(jv2.get_size(), joint_names.size()); @@ -35,6 +37,7 @@ TEST(JointVelocitiesTest, Constructors) { TEST(JointVelocitiesTest, StateCopyConstructor) { JointState random_state = JointState::Random("test", 3); JointVelocities copy1(random_state); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(random_state.get_name(), copy1.get_name()); EXPECT_EQ(random_state.get_names(), copy1.get_names()); EXPECT_EQ(random_state.get_size(), copy1.get_size()); @@ -42,6 +45,7 @@ TEST(JointVelocitiesTest, StateCopyConstructor) { expect_only_velocities(copy1); JointVelocities copy2 = random_state; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(random_state.get_name(), copy2.get_name()); EXPECT_EQ(random_state.get_names(), copy2.get_names()); EXPECT_EQ(random_state.get_size(), copy2.get_size()); @@ -50,16 +54,20 @@ TEST(JointVelocitiesTest, StateCopyConstructor) { JointState empty_state; JointVelocities copy3(empty_state); + EXPECT_EQ(copy3.get_type(), StateType::JOINT_VELOCITIES); EXPECT_TRUE(copy3.is_empty()); JointVelocities copy4 = empty_state; + EXPECT_EQ(copy4.get_type(), StateType::JOINT_VELOCITIES); EXPECT_TRUE(copy4.is_empty()); JointVelocities copy5 = empty_state.copy(); + EXPECT_EQ(copy5.get_type(), StateType::JOINT_VELOCITIES); EXPECT_TRUE(copy5.is_empty()); } TEST(JointVelocitiesTest, AccelerationsCopyConstructor) { JointAccelerations random_accelerations = JointAccelerations::Random("test", 3); JointVelocities copy1(random_accelerations); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(random_accelerations.get_name(), copy1.get_name()); EXPECT_EQ(random_accelerations.get_names(), copy1.get_names()); EXPECT_EQ(random_accelerations.get_size(), copy1.get_size()); @@ -67,6 +75,7 @@ TEST(JointVelocitiesTest, AccelerationsCopyConstructor) { expect_only_velocities(copy1); JointVelocities copy2 = random_accelerations; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(random_accelerations.get_name(), copy1.get_name()); EXPECT_EQ(random_accelerations.get_names(), copy1.get_names()); EXPECT_EQ(random_accelerations.get_size(), copy1.get_size()); @@ -81,6 +90,7 @@ TEST(JointVelocitiesTest, AccelerationsCopyConstructor) { TEST(JointVelocitiesTest, PositionsCopyConstructor) { JointPositions random_positions = JointPositions::Random("test", 3); JointVelocities copy1(random_positions); + EXPECT_EQ(copy1.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(random_positions.get_name(), copy1.get_name()); EXPECT_EQ(random_positions.get_names(), copy1.get_names()); EXPECT_EQ(random_positions.get_size(), copy1.get_size()); @@ -88,6 +98,7 @@ TEST(JointVelocitiesTest, PositionsCopyConstructor) { expect_only_velocities(copy1); JointVelocities copy2 = random_positions; + EXPECT_EQ(copy2.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(random_positions.get_name(), copy1.get_name()); EXPECT_EQ(random_positions.get_names(), copy1.get_names()); EXPECT_EQ(random_positions.get_size(), copy1.get_size()); @@ -101,10 +112,12 @@ TEST(JointVelocitiesTest, PositionsCopyConstructor) { TEST(JointVelocitiesTest, RandomInitialization) { JointVelocities random1 = JointVelocities::Random("test", 3); + EXPECT_EQ(random1.get_type(), StateType::JOINT_VELOCITIES); EXPECT_NE(random1.get_velocities().norm(), 0); expect_only_velocities(random1); JointVelocities random2 = JointVelocities::Random("test", std::vector{"j0", "j1"}); + EXPECT_EQ(random2.get_type(), StateType::JOINT_VELOCITIES); EXPECT_NE(random2.get_velocities().norm(), 0); expect_only_velocities(random2); } @@ -154,6 +167,7 @@ TEST(JointVelocitiesTest, GetSetData) { TEST(JointVelocitiesTest, ScalarMultiplication) { JointVelocities jv = JointVelocities::Random("test", 3); JointVelocities jscaled = 0.5 * jv; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jscaled.data(), 0.5 * jv.data()); JointVelocities empty; @@ -165,9 +179,11 @@ TEST(JointVelocitiesTest, MatrixMultiplication) { Eigen::MatrixXd gains = Eigen::VectorXd::Random(jv.get_size()).asDiagonal(); JointVelocities jscaled = gains * jv; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jscaled.data(), gains * jv.data()); EXPECT_EQ((jv * gains).data(), jscaled.data()); jv *= gains; + EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jscaled.data(), jv.data()); JointVelocities jscaled2 = jv * gains; @@ -180,9 +196,11 @@ TEST(JointVelocitiesTest, ArrayMultiplication) { Eigen::ArrayXd gains = Eigen::ArrayXd::Random(jv.get_size()); JointVelocities jscaled = gains * jv; + EXPECT_EQ(jscaled.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jscaled.data(), (gains * jv.array()).matrix()); EXPECT_EQ((jv * gains).data(), jscaled.data()); jv *= gains; + EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); EXPECT_EQ(jscaled.data(), jv.data()); gains = Eigen::ArrayXd::Random(2 * jv.get_size()); @@ -191,10 +209,13 @@ TEST(JointVelocitiesTest, ArrayMultiplication) { TEST(JointVelocitiesTest, ChronoMultiplication) { JointVelocities jv = JointVelocities::Random("test", 3); + EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); auto time = std::chrono::seconds(2); JointPositions jp1 = jv * time; + EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jp1.get_positions(), jv.get_velocities() * time.count()); JointPositions jp2 = time * jv; + EXPECT_EQ(jp1.get_type(), StateType::JOINT_POSITIONS); EXPECT_EQ(jp2.get_positions(), jv.get_velocities() * time.count()); JointVelocities empty; @@ -203,8 +224,10 @@ TEST(JointVelocitiesTest, ChronoMultiplication) { TEST(JointVelocitiesTest, ChronoDivision) { JointVelocities jv = JointVelocities::Random("test", 3); + EXPECT_EQ(jv.get_type(), StateType::JOINT_VELOCITIES); auto time = std::chrono::seconds(2); JointAccelerations ja = jv / time; + EXPECT_EQ(ja.get_type(), StateType::JOINT_ACCELERATIONS); EXPECT_EQ(ja.get_accelerations(), jv.get_velocities() / time.count()); JointVelocities empty; diff --git a/source/state_representation/test/tests/space/test_jacobian.cpp b/source/state_representation/test/tests/space/test_jacobian.cpp index a9926446b..a94902b1b 100644 --- a/source/state_representation/test/tests/space/test_jacobian.cpp +++ b/source/state_representation/test/tests/space/test_jacobian.cpp @@ -7,6 +7,7 @@ using namespace state_representation::exceptions; TEST(JacobianTest, TestCreate) { Jacobian jac("robot", 7, "test"); + EXPECT_EQ(jac.get_type(), StateType::JACOBIAN); EXPECT_EQ(jac.rows(), 6); EXPECT_EQ(jac.cols(), 7); EXPECT_TRUE(jac.is_empty()); @@ -22,6 +23,7 @@ TEST(JacobianTest, TestCreate) { TEST(JacobianTest, TestCreateWithVectorOfJoints) { Jacobian jac("robot", std::vector{"j1", "j2"}, "test", "test_ref"); + EXPECT_EQ(jac.get_type(), StateType::JACOBIAN); EXPECT_EQ(jac.get_joint_names().at(0), "j1"); EXPECT_EQ(jac.get_joint_names().at(1), "j2"); EXPECT_EQ(jac.get_reference_frame(), "test_ref"); @@ -45,6 +47,7 @@ TEST(JacobianTest, TestSetData) { TEST(JacobianTest, TestRandomCreate) { Jacobian jac = Jacobian::Random("robot", 7, "test"); + EXPECT_EQ(jac.get_type(), StateType::JACOBIAN); EXPECT_FALSE(jac.is_empty()); for (std::size_t i = 0; i < jac.cols(); ++i) { EXPECT_GT(jac.col(i).norm(), 0); @@ -54,6 +57,7 @@ TEST(JacobianTest, TestRandomCreate) { TEST(JacobianTest, TestTranspose) { Jacobian jac = Jacobian::Random("robot", 7, "test"); Jacobian jact = jac.transpose(); + EXPECT_EQ(jact.get_type(), StateType::JACOBIAN); EXPECT_EQ(jact.rows(), 7); EXPECT_EQ(jact.cols(), 6); diff --git a/source/state_representation/test/tests/space/test_spatial_state.cpp b/source/state_representation/test/tests/space/test_spatial_state.cpp index 73d2ef87c..e6682fce2 100644 --- a/source/state_representation/test/tests/space/test_spatial_state.cpp +++ b/source/state_representation/test/tests/space/test_spatial_state.cpp @@ -5,28 +5,34 @@ using namespace state_representation; TEST(SpatialStateTest, Constructors) { - SpatialState state1(StateType::JOINTSTATE); - EXPECT_EQ(state1.get_type(), StateType::JOINTSTATE); + SpatialState state0; + EXPECT_EQ(state0.get_type(), StateType::SPATIAL_STATE); + EXPECT_EQ(state0.get_name(), ""); + EXPECT_EQ(state0.get_reference_frame(), "world"); + EXPECT_TRUE(state0.is_empty()); + + SpatialState state1(StateType::JOINT_STATE); + EXPECT_EQ(state1.get_type(), StateType::JOINT_STATE); EXPECT_EQ(state1.get_name(), ""); EXPECT_EQ(state1.get_reference_frame(), "world"); EXPECT_TRUE(state1.is_empty()); - SpatialState state2(StateType::CARTESIANSTATE, "test", "robot", false); - EXPECT_EQ(state2.get_type(), StateType::CARTESIANSTATE); + SpatialState state2(StateType::CARTESIAN_STATE, "test", "robot", false); + EXPECT_EQ(state2.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(state2.get_name(), "test"); EXPECT_EQ(state2.get_reference_frame(), "robot"); EXPECT_FALSE(state2.is_empty()); SpatialState state3(state2); - EXPECT_EQ(state3.get_type(), StateType::CARTESIANSTATE); + EXPECT_EQ(state3.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(state3.get_name(), "test"); EXPECT_EQ(state3.get_reference_frame(), "robot"); EXPECT_FALSE(state3.is_empty()); } TEST(SpatialStateTest, Compatibility) { - SpatialState state1(StateType::CARTESIANSTATE, "test", "robot", true); - SpatialState state2(StateType::CARTESIANSTATE, "robot"); + SpatialState state1(StateType::CARTESIAN_STATE, "test", "robot", true); + SpatialState state2(StateType::CARTESIAN_STATE, "robot"); EXPECT_FALSE(state1.is_compatible(state2)); state2.set_reference_frame("robot"); EXPECT_FALSE(state1.is_compatible(state2)); @@ -35,15 +41,15 @@ TEST(SpatialStateTest, Compatibility) { } TEST(SpatialStateTest, Swap) { - SpatialState state1(StateType::CARTESIANSTATE, "cartesian"); - SpatialState state2(StateType::JOINTSTATE, "joint", "robot", false); + SpatialState state1(StateType::CARTESIAN_STATE, "cartesian"); + SpatialState state2(StateType::JOINT_STATE, "joint", "robot", false); swap(state1, state2); - EXPECT_EQ(state1.get_type(), StateType::JOINTSTATE); + EXPECT_EQ(state1.get_type(), StateType::JOINT_STATE); EXPECT_EQ(state1.get_name(), "joint"); EXPECT_EQ(state1.get_reference_frame(), "robot"); EXPECT_FALSE(state1.is_empty()); - EXPECT_EQ(state2.get_type(), StateType::CARTESIANSTATE); + EXPECT_EQ(state2.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(state2.get_name(), "cartesian"); EXPECT_EQ(state2.get_reference_frame(), "world"); EXPECT_TRUE(state2.is_empty()); diff --git a/source/state_representation/test/tests/test_ellipsoid.cpp b/source/state_representation/test/tests/test_ellipsoid.cpp index 9c373f1a6..56d79c931 100644 --- a/source/state_representation/test/tests/test_ellipsoid.cpp +++ b/source/state_representation/test/tests/test_ellipsoid.cpp @@ -38,6 +38,7 @@ TEST(EllipsoidTest, Sampling) { } TEST(EllipsoidTest, EllipsoidFitting) { + GTEST_SKIP() << "Skipping Ellipsoid fit test to reduce computational burden"; Ellipsoid ellipse("test"); // sample from the parameterization diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp index e5b393071..3adfb848d 100644 --- a/source/state_representation/test/tests/test_parameter.cpp +++ b/source/state_representation/test/tests/test_parameter.cpp @@ -13,31 +13,42 @@ using namespace state_representation; TEST(ParameterTest, Conversion) { Parameter int_param("test"); EXPECT_TRUE(int_param.is_empty()); - EXPECT_EQ(int_param.get_type(), StateType::PARAMETER_INT); + EXPECT_EQ(int_param.get_type(), StateType::PARAMETER); + EXPECT_EQ(int_param.get_parameter_type(), ParameterType::INT); + EXPECT_EQ(int_param.get_parameter_state_type(), StateType::NONE); int_param.set_value(2); EXPECT_EQ(int_param.get_value(), 2); EXPECT_EQ(typeid(int_param.get_value()).name(), typeid(2.0).name()); Parameter double_param("double"); EXPECT_NO_THROW(double_param = int_param); - EXPECT_EQ(double_param.get_type(), StateType::PARAMETER_DOUBLE); + EXPECT_EQ(double_param.get_type(), StateType::PARAMETER); + EXPECT_EQ(double_param.get_parameter_type(), ParameterType::DOUBLE); + EXPECT_EQ(double_param.get_parameter_state_type(), StateType::NONE); EXPECT_EQ(double_param.get_name(), int_param.get_name()); EXPECT_EQ(double_param.get_value(), 2.0); std::vector values{1, 2, 3}; Parameter> int_array_param("test", values); EXPECT_FALSE(int_array_param.is_empty()); - EXPECT_EQ(int_array_param.get_type(), StateType::PARAMETER_INT_ARRAY); + EXPECT_EQ(int_array_param.get_type(), StateType::PARAMETER); + EXPECT_EQ(int_array_param.get_parameter_type(), ParameterType::INT_ARRAY); + EXPECT_EQ(int_array_param.get_parameter_state_type(), StateType::NONE); for (std::size_t i = 0; i < values.size(); ++i) { EXPECT_EQ(int_array_param.get_value().at(i), values.at(i)); } Parameter test1("test", CartesianPose::Random("test")); + EXPECT_EQ(test1.get_parameter_state_type(), StateType::CARTESIAN_POSE); Parameter test2(test1); - EXPECT_EQ(test2.get_type(), StateType::PARAMETER_CARTESIANSTATE); + EXPECT_EQ(test2.get_type(), StateType::PARAMETER); + EXPECT_EQ(test2.get_parameter_type(), ParameterType::STATE); + EXPECT_EQ(test2.get_parameter_state_type(), StateType::CARTESIAN_STATE); std::shared_ptr> test3 = std::make_shared>(Parameter("test", CartesianPose::Random("test"))); - EXPECT_EQ(test3->get_type(), StateType::PARAMETER_CARTESIANSTATE); + EXPECT_EQ(test3->get_type(), StateType::PARAMETER); + EXPECT_EQ(test3->get_parameter_type(), ParameterType::STATE); + EXPECT_EQ(test3->get_parameter_state_type(), StateType::CARTESIAN_STATE); } TEST(ParameterTest, Event) { @@ -82,7 +93,9 @@ TEST(ParameterTest, MakeShared) { auto pose = CartesianPose::Random("A", "B"); auto param = make_shared_parameter("name", pose); EXPECT_EQ(param->get_name(), "name"); - EXPECT_EQ(param->get_type(), StateType::PARAMETER_CARTESIANPOSE); + EXPECT_EQ(param->get_type(), StateType::PARAMETER); + EXPECT_EQ(param->get_parameter_type(), ParameterType::STATE); + EXPECT_EQ(param->get_parameter_state_type(), StateType::CARTESIAN_POSE); EXPECT_EQ(param->is_empty(), false); EXPECT_EQ(param->get_value().get_name(), "A"); EXPECT_EQ(param->get_value().get_reference_frame(), "B"); @@ -95,7 +108,9 @@ TEST(ParameterTest, ParameterThroughInterface) { auto param = param_interface->get_parameter(); EXPECT_EQ(param->get_name(), "name"); - EXPECT_EQ(param->get_type(), StateType::PARAMETER_CARTESIANPOSE); + EXPECT_EQ(param->get_type(), StateType::PARAMETER); + EXPECT_EQ(param->get_parameter_type(), ParameterType::STATE); + EXPECT_EQ(param->get_parameter_state_type(), StateType::CARTESIAN_POSE); EXPECT_EQ(param->is_empty(), false); EXPECT_EQ(param->get_value().get_name(), "A"); EXPECT_EQ(param->get_value().get_reference_frame(), "B"); @@ -115,7 +130,7 @@ TEST(ParameterTest, ParameterThroughInterface) { } TEST(ParameterTest, ParameterInterfaceBadPointer) { - ParameterInterface parameter_interface(StateType::PARAMETER_INT, "name"); + ParameterInterface parameter_interface("name", ParameterType::INT); // by default (validate_pointer = true), throw when the ParameterInterface instance is not managed by any pointer EXPECT_THROW(parameter_interface.get_parameter(), exceptions::InvalidPointerException); @@ -127,7 +142,7 @@ TEST(ParameterTest, ParameterInterfaceBadPointer) { } TEST(ParameterTest, ParameterInterfaceNullCast) { - auto parameter_interface_ptr = std::make_shared(StateType::PARAMETER_INT, "name"); + auto parameter_interface_ptr = std::make_shared("name", ParameterType::INT); std::shared_ptr> parameter; // by default (validate_pointer = true), throw when the pointer does not address a Parameter instance diff --git a/source/state_representation/test/tests/test_state.cpp b/source/state_representation/test/tests/test_state.cpp index 4da2a32ea..ef3c86992 100644 --- a/source/state_representation/test/tests/test_state.cpp +++ b/source/state_representation/test/tests/test_state.cpp @@ -12,20 +12,20 @@ TEST(StateTest, Constructors) { EXPECT_EQ(empty1.get_name(), ""); EXPECT_TRUE(empty1.is_empty()); - State empty2(StateType::JOINTSTATE); - EXPECT_EQ(empty2.get_type(), StateType::JOINTSTATE); + State empty2(StateType::JOINT_STATE); + EXPECT_EQ(empty2.get_type(), StateType::JOINT_STATE); EXPECT_EQ(empty2.get_name(), ""); EXPECT_TRUE(empty2.is_empty()); - State empty3(StateType::CARTESIANSTATE, "test", true); - EXPECT_EQ(empty3.get_type(), StateType::CARTESIANSTATE); + State empty3(StateType::CARTESIAN_STATE, "test", true); + EXPECT_EQ(empty3.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(empty3.get_name(), "test"); EXPECT_TRUE(empty3.is_empty()); empty3.set_filled(); EXPECT_FALSE(empty3.is_empty()); State state(empty3); - EXPECT_EQ(state.get_type(), StateType::CARTESIANSTATE); + EXPECT_EQ(state.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(state.get_name(), "test"); EXPECT_FALSE(state.is_empty()); state.set_empty(); @@ -59,13 +59,13 @@ TEST(StateTest, Timestamp) { } TEST(StateTest, Swap) { - State state1(StateType::CARTESIANSTATE, "cartesian", true); + State state1(StateType::CARTESIAN_STATE, "cartesian", true); State state2(StateType::STATE, "state", false); swap(state1, state2); EXPECT_EQ(state1.get_type(), StateType::STATE); EXPECT_EQ(state1.get_name(), "state"); EXPECT_FALSE(state1.is_empty()); - EXPECT_EQ(state2.get_type(), StateType::CARTESIANSTATE); + EXPECT_EQ(state2.get_type(), StateType::CARTESIAN_STATE); EXPECT_EQ(state2.get_name(), "cartesian"); EXPECT_TRUE(state2.is_empty()); } \ No newline at end of file From 67491f11b78bc16c5c224ef3f631d83235959e84 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Fri, 8 Apr 2022 11:33:31 +0200 Subject: [PATCH 04/17] Refactor StateType in protocol (#278) * Update StateType definition in protocol to match the refactored enum, causing a breaking change to past encoded messages * CHANGELOG * 5.1.5 -> 5.1.6 --- .../test/tests/test_cartesian_space_proto.cpp | 2 +- .../clproto_cpp/test/tests/test_messages.cpp | 2 +- .../proto/state_representation/state.proto | 41 ++++++++----------- 3 files changed, 20 insertions(+), 25 deletions(-) diff --git a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp index f9d1da137..1909b7b9a 100644 --- a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp +++ b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp @@ -13,7 +13,7 @@ using namespace state_representation; TEST(CartesianProtoTest, EncodeDecodeSpatialState) { - auto send_state = SpatialState(StateType::PARAMETER_BOOL, "A", "B", false); + auto send_state = SpatialState(StateType::SPATIAL_STATE, "A", "B", false); std::string msg = clproto::encode(send_state); EXPECT_TRUE(clproto::is_valid(msg)); EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE); diff --git a/protocol/clproto_cpp/test/tests/test_messages.cpp b/protocol/clproto_cpp/test/tests/test_messages.cpp index 2c6a05d5a..aac920bbb 100644 --- a/protocol/clproto_cpp/test/tests/test_messages.cpp +++ b/protocol/clproto_cpp/test/tests/test_messages.cpp @@ -14,7 +14,7 @@ TEST(MessageProtoTest, EncodeDecodeState) { EXPECT_TRUE(clproto::is_valid(msg)); EXPECT_TRUE(clproto::check_message_type(msg) == clproto::STATE_MESSAGE); - State recv_state(StateType::PARAMETER_MATRIX); + State recv_state(StateType::STATE); EXPECT_NO_THROW(clproto::decode(msg)); EXPECT_TRUE(clproto::decode(msg, recv_state)); diff --git a/protocol/protobuf/proto/state_representation/state.proto b/protocol/protobuf/proto/state_representation/state.proto index 81d88c892..81fe33817 100644 --- a/protocol/protobuf/proto/state_representation/state.proto +++ b/protocol/protobuf/proto/state_representation/state.proto @@ -5,29 +5,24 @@ package state_representation.proto; // The values and order of this enumeration must match the original // state_representation::StateType in C++. enum StateType { - STATE = 0; - CARTESIANSTATE = 1; - DUALQUATERNIONSTATE = 2; - JOINTSTATE = 3; - JACOBIANMATRIX = 4; - TRAJECTORY = 5; - GEOMETRY_SHAPE = 6; - GEOMETRY_ELLIPSOID = 7; - PARAMETER_DOUBLE = 8; - PARAMETER_DOUBLE_ARRAY = 9; - PARAMETER_BOOL = 10; - PARAMETER_BOOL_ARRAY = 11; - PARAMETER_STRING = 12; - PARAMETER_STRING_ARRAY = 13; - PARAMETER_CARTESIANSTATE = 14; - PARAMETER_CARTESIANPOSE = 15; - PARAMETER_JOINTSTATE = 16; - PARAMETER_JOINTPOSITIONS = 17; - PARAMETER_ELLIPSOID = 18; - PARAMETER_MATRIX = 19; - PARAMETER_VECTOR = 20; - PARAMETER_INT = 21; - PARAMETER_INT_ARRAY = 22; + NONE = 0; + STATE = 1; + SPATIAL_STATE = 2; + CARTESIAN_STATE = 3; + CARTESIAN_POSE = 4; + CARTESIAN_TWIST = 5; + CARTESIAN_ACCELERATION = 6; + CARTESIAN_WRENCH = 7; + JOINT_STATE = 8; + JOINT_POSITIONS = 9; + JOINT_VELOCITIES = 10; + JOINT_ACCELERATIONS = 11; + JOINT_TORQUES = 12; + JACOBIAN = 13; + PARAMETER = 14; + GEOMETRY_SHAPE = 15; + GEOMETRY_ELLIPSOID = 16; + TRAJECTORY = 17; }; message State { From f0a22374185c2561d08af90582850811ef8e4372 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Mon, 11 Apr 2022 17:09:53 +0200 Subject: [PATCH 05/17] Refactor StateType in python bindings (#280) * Update StateType binding * Refactor ParameterContainer * Update ParameterInterface binding to reflect new constructor and methods * Update ParameterContainer constructor * Add ParameterType binding * Fix StateType and ParameterType references * Bind empty constructor for SpatialState * Update clproto bindings * Implement parameter state type switch cases * Get and set parameter state values by state type switch using pointer injection and dynamic downcasting * Update tests * Raise exceptions in unsupported or invalid cases * Update CHANGELOG * 5.1.6 -> 5.1.7 --- CHANGELOG.md | 2 +- python/include/parameter_container.h | 17 +- python/source/clproto/bind_clproto.cpp | 42 +-- python/source/common/parameter_container.cpp | 304 +++++++++++------- .../bind_cartesian_space.cpp | 1 + .../state_representation/bind_parameters.cpp | 120 +++++-- .../state_representation/bind_state.cpp | 34 +- .../test/controllers/test_compliant_twist.py | 16 +- python/test/controllers/test_controller.py | 8 +- .../controllers/test_dissipative_impedance.py | 2 +- .../test_cartesian_point_attractor.py | 8 +- .../test/dynamical_systems/test_circular.py | 6 +- python/test/dynamical_systems/test_ds.py | 4 +- .../test_joint_point_attractor.py | 4 +- python/test/dynamical_systems/test_ring.py | 16 +- .../space/cartesian/test_cartesian_state.py | 6 +- .../space/test_spatial_state.py | 12 +- .../state_representation/test_parameters.py | 123 +++---- .../test/state_representation/test_state.py | 10 +- python/test/test_clproto.py | 22 +- 20 files changed, 451 insertions(+), 306 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 64a4f99ec..112476687 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,7 @@ Release Versions: ## Upcoming changes (in development) - Fix if else conditions in setup.py to correctly install modules (#285) -- Refactor state type (#277) +- Refactor state type (#277, #278, #280) ## 5.2.0 diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h index 463fa04a1..10b6c522f 100644 --- a/python/include/parameter_container.h +++ b/python/include/parameter_container.h @@ -5,8 +5,6 @@ #include #include #include -#include -#include namespace py_parameter { @@ -19,19 +17,20 @@ struct ParameterValues { std::vector bool_array_value; std::string string_value; std::vector string_array_value; - CartesianState cartesian_state; - CartesianPose cartesian_pose; - JointState joint_state; - JointPositions joint_positions; - Ellipsoid ellipsoid; + std::shared_ptr state_pointer; Eigen::MatrixXd matrix_value; Eigen::VectorXd vector_value; }; class ParameterContainer : public ParameterInterface { public: - ParameterContainer(const std::string& name, const StateType& type); - ParameterContainer(const std::string& name, const py::object& value, const StateType& type); + ParameterContainer( + const std::string& name, const ParameterType& type, const StateType& parameter_state_type = StateType::NONE + ); + ParameterContainer( + const std::string& name, const py::object& value, const ParameterType& type, + const StateType& parameter_state_type = StateType::NONE + ); ParameterContainer(const ParameterContainer& parameter); void set_value(const py::object& value); diff --git a/python/source/clproto/bind_clproto.cpp b/python/source/clproto/bind_clproto.cpp index 8a95f6d29..34a58c83f 100644 --- a/python/source/clproto/bind_clproto.cpp +++ b/python/source/clproto/bind_clproto.cpp @@ -28,26 +28,26 @@ inline py::bytes encode_bytes(const T& object) { } py::bytes encode_parameter_container(const ParameterContainer& container) { - switch (container.get_type()) { - case StateType::PARAMETER_INT: + switch (container.get_parameter_type()) { + case ParameterType::INT: return py::bytes(encode(Parameter(container.get_name(), container.values.int_value))); - case StateType::PARAMETER_INT_ARRAY: + case ParameterType::INT_ARRAY: return py::bytes(encode(Parameter(container.get_name(), container.values.int_array_value))); - case StateType::PARAMETER_DOUBLE: + case ParameterType::DOUBLE: return py::bytes(encode(Parameter(container.get_name(), container.values.double_value))); - case StateType::PARAMETER_DOUBLE_ARRAY: + case ParameterType::DOUBLE_ARRAY: return py::bytes(encode(Parameter(container.get_name(), container.values.double_array_value))); - case StateType::PARAMETER_BOOL: + case ParameterType::BOOL: return py::bytes(encode(Parameter(container.get_name(), container.values.bool_value))); - case StateType::PARAMETER_BOOL_ARRAY: + case ParameterType::BOOL_ARRAY: return py::bytes(encode(Parameter(container.get_name(), container.values.bool_array_value))); - case StateType::PARAMETER_STRING: + case ParameterType::STRING: return py::bytes(encode(Parameter(container.get_name(), container.values.string_value))); - case StateType::PARAMETER_STRING_ARRAY: + case ParameterType::STRING_ARRAY: return py::bytes(encode(Parameter(container.get_name(), container.values.string_array_value))); - case StateType::PARAMETER_MATRIX: + case ParameterType::MATRIX: return py::bytes(encode(Parameter(container.get_name(), container.values.matrix_value))); - case StateType::PARAMETER_VECTOR: + case ParameterType::VECTOR: return py::bytes(encode(Parameter(container.get_name(), container.values.vector_value))); default: throw std::invalid_argument("This StateType is not a valid Parameter."); @@ -60,43 +60,43 @@ py::object decode_parameter(const std::string& msg) { switch (check_parameter_message_type(msg)) { case ParameterMessageType::INT: { auto param = decode>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::INT_ARRAY: { auto param = decode>>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::DOUBLE: { auto param = decode>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::DOUBLE_ARRAY: { auto param = decode>>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::BOOL: { auto param = decode>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::BOOL_ARRAY: { auto param = decode>>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::STRING: { auto param = decode>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::STRING_ARRAY: { auto param = decode>>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::VECTOR: { auto param = decode>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } case ParameterMessageType::MATRIX: { auto param = decode>(msg); - return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_type()); + return PyParameter(param.get_name(), py::cast(param.get_value()), param.get_parameter_type()); } default: throw std::invalid_argument("The message is not a valid encoded Parameter."); diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp index 879bede17..7b33d8fe3 100644 --- a/python/source/common/parameter_container.cpp +++ b/python/source/common/parameter_container.cpp @@ -1,180 +1,260 @@ #include "parameter_container.h" +#include +#include +#include +#include +#include + namespace py_parameter { -ParameterContainer::ParameterContainer(const std::string& name, const StateType& type) : - ParameterInterface(type, name) {} +ParameterContainer::ParameterContainer( + const std::string& name, const ParameterType& type, const StateType& parameter_state_type +) : ParameterInterface(name, type, parameter_state_type) {} -ParameterContainer::ParameterContainer(const std::string& name, const py::object& value, const StateType& type) : - ParameterInterface(type, name) { +ParameterContainer::ParameterContainer( + const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type +) : ParameterInterface(name, type, parameter_state_type) { set_value(value); } ParameterContainer::ParameterContainer(const ParameterContainer& parameter) : - ParameterInterface(parameter.get_type(), parameter.get_name()), values(parameter.values) {} + ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()), + values(parameter.values) {} void ParameterContainer::set_value(const py::object& value) { - switch (this->get_type()) { - case StateType::PARAMETER_INT: + switch (this->get_parameter_type()) { + case ParameterType::INT: values.int_value = value.cast(); break; - case StateType::PARAMETER_INT_ARRAY: + case ParameterType::INT_ARRAY: values.int_array_value = value.cast>(); break; - case StateType::PARAMETER_DOUBLE: + case ParameterType::DOUBLE: values.double_value = value.cast(); break; - case StateType::PARAMETER_DOUBLE_ARRAY: + case ParameterType::DOUBLE_ARRAY: values.double_array_value = value.cast>(); break; - case StateType::PARAMETER_BOOL: + case ParameterType::BOOL: values.bool_value = value.cast(); break; - case StateType::PARAMETER_BOOL_ARRAY: + case ParameterType::BOOL_ARRAY: values.bool_array_value = value.cast>(); break; - case StateType::PARAMETER_STRING: + case ParameterType::STRING: values.string_value = value.cast(); break; - case StateType::PARAMETER_STRING_ARRAY: + case ParameterType::STRING_ARRAY: values.string_array_value = value.cast>(); break; - case StateType::PARAMETER_CARTESIANSTATE: - values.cartesian_state = value.cast(); - break; - case StateType::PARAMETER_CARTESIANPOSE: - values.cartesian_pose = value.cast(); - break; - case StateType::PARAMETER_JOINTSTATE: - values.joint_state = value.cast(); - break; - case StateType::PARAMETER_JOINTPOSITIONS: - values.joint_positions = value.cast(); + case ParameterType::STATE: + switch (this->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + values.state_pointer = std::make_shared(value.cast()); + break; + case StateType::CARTESIAN_POSE: + values.state_pointer = std::make_shared(value.cast()); + break; + case StateType::JOINT_STATE: + values.state_pointer = std::make_shared(value.cast()); + break; + case StateType::JOINT_POSITIONS: + values.state_pointer = std::make_shared(value.cast()); + break; + case StateType::GEOMETRY_ELLIPSOID: + values.state_pointer = std::make_shared(value.cast()); + break; + default: + throw std::invalid_argument("The StateType contained by parameter " + this->get_name() + " is unsupported."); + break; + } break; - case StateType::PARAMETER_ELLIPSOID: - values.ellipsoid = value.cast(); - break; - case StateType::PARAMETER_MATRIX: + case ParameterType::MATRIX: values.matrix_value = value.cast(); break; - case StateType::PARAMETER_VECTOR: + case ParameterType::VECTOR: values.vector_value = value.cast(); break; default: - throw std::invalid_argument("This StateType is not a valid Parameter."); - break; + throw std::invalid_argument("The ParameterType of parameter " + this->get_name() + " is invalid."); } } py::object ParameterContainer::get_value() { - switch (this->get_type()) { - case StateType::PARAMETER_INT: + switch (this->get_parameter_type()) { + case ParameterType::INT: return py::cast(values.int_value); - case StateType::PARAMETER_INT_ARRAY: + case ParameterType::INT_ARRAY: return py::cast(values.int_array_value); - case StateType::PARAMETER_DOUBLE: + case ParameterType::DOUBLE: return py::cast(values.double_value); - case StateType::PARAMETER_DOUBLE_ARRAY: + case ParameterType::DOUBLE_ARRAY: return py::cast(values.double_array_value); - case StateType::PARAMETER_BOOL: + case ParameterType::BOOL: return py::cast(values.bool_value); - case StateType::PARAMETER_BOOL_ARRAY: + case ParameterType::BOOL_ARRAY: return py::cast(values.bool_array_value); - case StateType::PARAMETER_STRING: + case ParameterType::STRING: return py::cast(values.string_value); - case StateType::PARAMETER_STRING_ARRAY: + case ParameterType::STRING_ARRAY: return py::cast(values.string_array_value); - case StateType::PARAMETER_CARTESIANSTATE: - return py::cast(values.cartesian_state); - case StateType::PARAMETER_CARTESIANPOSE: - return py::cast(values.cartesian_pose); - case StateType::PARAMETER_JOINTSTATE: - return py::cast(values.joint_state); - case StateType::PARAMETER_JOINTPOSITIONS: - return py::cast(values.joint_positions); - case StateType::PARAMETER_ELLIPSOID: - return py::cast(values.ellipsoid); - case StateType::PARAMETER_MATRIX: + case ParameterType::STATE: + try { + switch (this->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + return py::cast(*std::dynamic_pointer_cast(values.state_pointer)); + case StateType::CARTESIAN_POSE: + return py::cast(*std::dynamic_pointer_cast(values.state_pointer)); + case StateType::JOINT_STATE: + return py::cast(*std::dynamic_pointer_cast(values.state_pointer)); + case StateType::JOINT_POSITIONS: + return py::cast(*std::dynamic_pointer_cast(values.state_pointer)); + case StateType::GEOMETRY_ELLIPSOID: + return py::cast(*std::dynamic_pointer_cast(values.state_pointer)); + default: + throw std::invalid_argument( + "The StateType contained by parameter " + this->get_name() + " is unsupported."); + } + } catch (const std::exception&) { + throw std::runtime_error("The ParameterType of parameter " + this->get_name() + " is invalid."); + } + break; + case ParameterType::MATRIX: return py::cast(values.matrix_value); - case StateType::PARAMETER_VECTOR: + case ParameterType::VECTOR: return py::cast(values.vector_value); default: - return py::none(); + break; } + throw std::invalid_argument("Could not get the value of parameter " + this->get_name()); } ParameterContainer interface_ptr_to_container(const std::shared_ptr& parameter) { - switch (parameter->get_type()) { - case StateType::PARAMETER_INT: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_INT_ARRAY: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value>()), parameter->get_type()); - case StateType::PARAMETER_DOUBLE: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_DOUBLE_ARRAY: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value>()), parameter->get_type()); - case StateType::PARAMETER_BOOL: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_BOOL_ARRAY: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value>()), parameter->get_type()); - case StateType::PARAMETER_STRING: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_STRING_ARRAY: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value>()), parameter->get_type()); - case StateType::PARAMETER_CARTESIANSTATE: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_CARTESIANPOSE: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_JOINTSTATE: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_JOINTPOSITIONS: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_ELLIPSOID: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_MATRIX: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); - case StateType::PARAMETER_VECTOR: - return ParameterContainer(parameter->get_name(), py::cast(parameter->get_parameter_value()), parameter->get_type()); + switch (parameter->get_parameter_type()) { + case ParameterType::INT: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::INT); + case ParameterType::INT_ARRAY: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value>()), + ParameterType::INT_ARRAY); + case ParameterType::DOUBLE: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::DOUBLE); + case ParameterType::DOUBLE_ARRAY: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value>()), + ParameterType::DOUBLE_ARRAY); + case ParameterType::BOOL: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::BOOL); + case ParameterType::BOOL_ARRAY: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value>()), + ParameterType::BOOL_ARRAY); + case ParameterType::STRING: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::STRING); + case ParameterType::STRING_ARRAY: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value>()), + ParameterType::STRING_ARRAY); + case ParameterType::STATE: + try { + switch (parameter->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::STATE, + StateType::CARTESIAN_STATE); + case StateType::CARTESIAN_POSE: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::STATE, + StateType::CARTESIAN_POSE); + case StateType::JOINT_STATE: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::STATE, + StateType::JOINT_STATE); + case StateType::JOINT_POSITIONS: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::STATE, + StateType::JOINT_POSITIONS); + case StateType::GEOMETRY_ELLIPSOID: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::STATE, + StateType::GEOMETRY_ELLIPSOID); + default: + throw std::invalid_argument( + "The StateType contained by parameter " + parameter->get_name() + " is unsupported."); + } + } catch (const std::exception&) { + throw std::runtime_error("The ParameterType of parameter " + parameter->get_name() + " is invalid."); + } + break; + case ParameterType::MATRIX: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::MATRIX); + case ParameterType::VECTOR: + return ParameterContainer( + parameter->get_name(), py::cast(parameter->get_parameter_value()), ParameterType::VECTOR); default: - throw std::invalid_argument("The conversion from this StateType to ParameterContainer is not supported yet."); + break; } + throw std::invalid_argument("The conversion from this Parameter to a ParameterContainer is not supported."); } std::shared_ptr container_to_interface_ptr(const ParameterContainer& parameter) { - switch (parameter.get_type()) { - case StateType::PARAMETER_INT: + switch (parameter.get_parameter_type()) { + case ParameterType::INT: return make_shared_parameter(parameter.get_name(), parameter.values.int_value); - case StateType::PARAMETER_INT_ARRAY: + case ParameterType::INT_ARRAY: return make_shared_parameter(parameter.get_name(), parameter.values.int_array_value); - case StateType::PARAMETER_DOUBLE: + case ParameterType::DOUBLE: return make_shared_parameter(parameter.get_name(), parameter.values.double_value); - case StateType::PARAMETER_DOUBLE_ARRAY: + case ParameterType::DOUBLE_ARRAY: return make_shared_parameter(parameter.get_name(), parameter.values.double_array_value); - case StateType::PARAMETER_BOOL: + case ParameterType::BOOL: return make_shared_parameter(parameter.get_name(), parameter.values.bool_value); - case StateType::PARAMETER_BOOL_ARRAY: + case ParameterType::BOOL_ARRAY: return make_shared_parameter(parameter.get_name(), parameter.values.bool_array_value); - case StateType::PARAMETER_STRING: + case ParameterType::STRING: return make_shared_parameter(parameter.get_name(), parameter.values.string_value); - case StateType::PARAMETER_STRING_ARRAY: + case ParameterType::STRING_ARRAY: return make_shared_parameter(parameter.get_name(), parameter.values.string_array_value); - case StateType::PARAMETER_CARTESIANSTATE: - return make_shared_parameter(parameter.get_name(), parameter.values.cartesian_state); - case StateType::PARAMETER_CARTESIANPOSE: - return make_shared_parameter(parameter.get_name(), parameter.values.cartesian_pose); - case StateType::PARAMETER_JOINTSTATE: - return make_shared_parameter(parameter.get_name(), parameter.values.joint_state); - case StateType::PARAMETER_JOINTPOSITIONS: - return make_shared_parameter(parameter.get_name(), parameter.values.joint_positions); - case StateType::PARAMETER_ELLIPSOID: - return make_shared_parameter(parameter.get_name(), parameter.values.ellipsoid); - case StateType::PARAMETER_MATRIX: + case ParameterType::STATE: + try { + switch (parameter.get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + return make_shared_parameter( + parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + case StateType::CARTESIAN_POSE: + return make_shared_parameter( + parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + case StateType::JOINT_STATE: + return make_shared_parameter( + parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + case StateType::JOINT_POSITIONS: + return make_shared_parameter( + parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + case StateType::GEOMETRY_ELLIPSOID: + return make_shared_parameter( + parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + default: + throw std::invalid_argument( + "The StateType contained by parameter " + parameter.get_name() + " is unsupported."); + } + } catch (const std::exception&) { + throw std::runtime_error("The ParameterType of parameter " + parameter.get_name() + " is invalid."); + } + break; + case ParameterType::MATRIX: return make_shared_parameter(parameter.get_name(), parameter.values.matrix_value); - case StateType::PARAMETER_VECTOR: - return make_shared_parameter(parameter.get_name(), parameter.values.vector_value); + case ParameterType::VECTOR: + return make_shared_parameter(parameter.get_name(), parameter.values.vector_value); default: - return {}; + break; } + throw std::invalid_argument("The conversion from this ParameterContainer to a Parameter is not supported."); } std::map @@ -191,7 +271,11 @@ std::map> container_to_interface_ptr_map(const std::map& parameters) { std::map> parameter_list; for (const auto& param_it: parameters) { - parameter_list.insert(std::pair>(param_it.first, container_to_interface_ptr(param_it.second))); + parameter_list.insert( + std::pair>( + param_it.first, container_to_interface_ptr(param_it.second) + ) + ); } return parameter_list; } diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp index f821d519a..d55a7af35 100644 --- a/python/source/state_representation/bind_cartesian_space.cpp +++ b/python/source/state_representation/bind_cartesian_space.cpp @@ -12,6 +12,7 @@ void spatial_state(py::module_& m) { py::class_, State> c(m, "SpatialState"); + c.def(py::init(), "Empty constructor."); c.def(py::init(), "Constructor only specifying the type.", "type"_a); c.def(py::init(), "Constructor with name and reference frame specification.", "type"_a, "name"_a, "reference_frame"_a=std::string("world"), "empty"_a=true); c.def(py::init(), "Copy constructor from another SpatialState.", "state"_a); diff --git a/python/source/state_representation/bind_parameters.cpp b/python/source/state_representation/bind_parameters.cpp index 62d644af6..22a94a74f 100644 --- a/python/source/state_representation/bind_parameters.cpp +++ b/python/source/state_representation/bind_parameters.cpp @@ -1,25 +1,51 @@ #include "state_representation_bindings.h" +#include #include #include +#include +#include +#include +#include +#include + #include "parameter_container.h" #include "py_parameter_map.h" using namespace py_parameter; +void parameter_type(py::module_& m) { + py::enum_(m, "ParameterType") + .value("INT", ParameterType::INT) + .value("INT_ARRAY", ParameterType::INT_ARRAY) + .value("DOUBLE", ParameterType::DOUBLE) + .value("DOUBLE_ARRAY", ParameterType::DOUBLE_ARRAY) + .value("BOOL", ParameterType::BOOL) + .value("BOOL_ARRAY", ParameterType::BOOL_ARRAY) + .value("STRING", ParameterType::STRING) + .value("STRING_ARRAY", ParameterType::STRING_ARRAY) + .value("STATE", ParameterType::STATE) + .value("VECTOR", ParameterType::VECTOR) + .value("MATRIX", ParameterType::MATRIX) + .export_values(); +} + void parameter_interface(py::module_& m) { py::class_, State> c(m, "ParameterInterface"); - c.def(py::init(), "Constructor with parameter name and type of the parameter", "type"_a, "name"_a); + c.def(py::init(), "Constructor of a ParameterInterface with name, parameter type and parameter state type", "name"_a, "type"_a, "parameter_state_type"_a=StateType::NONE); c.def(py::init(), "Copy constructor from another ParameterInterface", "parameter"_a); + + c.def("get_parameter_type", &ParameterInterface::get_parameter_type, "Get the parameter type."); + c.def("get_parameter_state_type", &ParameterInterface::get_parameter_state_type, "Get the state type of the parameter."); } void parameter(py::module_& m) { py::class_, ParameterInterface> c(m, "Parameter"); - c.def(py::init(), "Constructor of a parameter with name and type", "name"_a, "type"_a); - c.def(py::init(), "Constructor of a parameter with name, value and type", "name"_a, "value"_a, "type"_a); + c.def(py::init(), "Constructor of a parameter with name, parameter type and parameter state type", "name"_a, "type"_a, "parameter_state_type"_a=StateType::NONE); + c.def(py::init(), "Constructor of a parameter with name, value, parameter type and parameter state type", "name"_a, "value"_a, "type"_a, "parameter_state_type"_a=StateType::NONE); c.def(py::init(), "Copy constructor from another Parameter", "parameter"_a); c.def(py::init([](const std::shared_ptr& parameter) { return interface_ptr_to_container(parameter); }), "Constructor from a parameter interface pointer", "parameter"_a); @@ -34,73 +60,96 @@ void parameter(py::module_& m) { }, "memo"_a); c.def("__repr__", [](const ParameterContainer& parameter) { std::stringstream buffer; - switch (parameter.get_type()) { - case StateType::PARAMETER_INT: { + switch (parameter.get_parameter_type()) { + case ParameterType::INT: { Parameter param(parameter.get_name(), parameter.values.int_value); buffer << param; break; } - case StateType::PARAMETER_INT_ARRAY: { + case ParameterType::INT_ARRAY: { Parameter> param(parameter.get_name(), parameter.values.int_array_value); buffer << param; break; } - case StateType::PARAMETER_DOUBLE: { + case ParameterType::DOUBLE: { Parameter param(parameter.get_name(), parameter.values.double_value); buffer << param; break; } - case StateType::PARAMETER_DOUBLE_ARRAY: { + case ParameterType::DOUBLE_ARRAY: { Parameter> param(parameter.get_name(), parameter.values.double_array_value); buffer << param; break; } - case StateType::PARAMETER_BOOL: { + case ParameterType::BOOL: { Parameter param(parameter.get_name(), parameter.values.bool_value); buffer << param; break; } - case StateType::PARAMETER_BOOL_ARRAY: { + case ParameterType::BOOL_ARRAY: { Parameter> param(parameter.get_name(), parameter.values.bool_array_value); buffer << param; break; } - case StateType::PARAMETER_STRING: { + case ParameterType::STRING: { Parameter param(parameter.get_name(), parameter.values.string_value); buffer << param; break; } - case StateType::PARAMETER_STRING_ARRAY: { + case ParameterType::STRING_ARRAY: { Parameter> param(parameter.get_name(), parameter.values.string_array_value); buffer << param; break; } - case StateType::PARAMETER_CARTESIANSTATE: { - Parameter param(parameter.get_name(), parameter.values.cartesian_state); - buffer << param; - break; - } - case StateType::PARAMETER_CARTESIANPOSE: { - Parameter param(parameter.get_name(), parameter.values.cartesian_pose); - buffer << param; - break; - } - case StateType::PARAMETER_JOINTSTATE: { - Parameter param(parameter.get_name(), parameter.values.joint_state); - buffer << param; - break; - } - case StateType::PARAMETER_JOINTPOSITIONS: { - Parameter param(parameter.get_name(), parameter.values.joint_positions); - buffer << param; + case ParameterType::STATE: { + try { + switch (parameter.get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: { + Parameter param + (parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + buffer << param; + break; + } + case StateType::CARTESIAN_POSE: { + Parameter param + (parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + buffer << param; + break; + } + case StateType::JOINT_STATE: { + Parameter param + (parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + buffer << param; + break; + } + case StateType::JOINT_POSITIONS: { + Parameter param + (parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + buffer << param; + break; + } + case StateType::GEOMETRY_ELLIPSOID: { + Parameter param + (parameter.get_name(), *std::dynamic_pointer_cast(parameter.values.state_pointer)); + buffer << param; + break; + } + default: + buffer << "Parameter " << parameter.get_name() << " contains an unsupported state type" << std::endl; + break; + } + } catch (const std::exception&) { + buffer << "Parameter " << parameter.get_name() << " is invalid" << std::endl; + break; + } break; } - case StateType::PARAMETER_MATRIX: { + case ParameterType::MATRIX: { Parameter param(parameter.get_name(), parameter.values.matrix_value); buffer << param; break; } - case StateType::PARAMETER_VECTOR: { + case ParameterType::VECTOR: { Parameter param(parameter.get_name(), parameter.values.vector_value); buffer << param; break; @@ -159,14 +208,15 @@ void parameter_map(py::module_& m) { self.set_parameters(container_to_interface_ptr_map(parameters)); }, "Set parameters from a map with pairs", "parameters"_a); c.def( - "set_parameter_value", [](ParameterMap& self, const std::string& name, const py::object& value, const StateType& type) -> void { - auto param = ParameterContainer(name, value, type); + "set_parameter_value", [](ParameterMap& self, const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type) -> void { + auto param = ParameterContainer(name, value, type, parameter_state_type); self.set_parameter(container_to_interface_ptr(param)); - }, "Set a parameter value by its name", "name"_a, "value"_a, "type"_a + }, "Set a parameter value by its name", "name"_a, "value"_a, "type"_a, "parameter_state_type"_a=StateType::NONE ); } void bind_parameters(py::module_& m) { + parameter_type(m); parameter_interface(m); parameter(m); parameter_map(m); diff --git a/python/source/state_representation/bind_state.cpp b/python/source/state_representation/bind_state.cpp index b89737276..b9aed8bd5 100644 --- a/python/source/state_representation/bind_state.cpp +++ b/python/source/state_representation/bind_state.cpp @@ -5,28 +5,24 @@ void state_type(py::module_& m) { py::enum_(m, "StateType") + .value("NONE", StateType::NONE) .value("STATE", StateType::STATE) - .value("CARTESIANSTATE", StateType::CARTESIANSTATE) - .value("DUALQUATERNIONSTATE", StateType::DUALQUATERNIONSTATE) - .value("JOINTSTATE", StateType::JOINTSTATE) - .value("JACOBIANMATRIX", StateType::JACOBIANMATRIX) + .value("SPATIAL_STATE", StateType::SPATIAL_STATE) + .value("CARTESIAN_STATE", StateType::CARTESIAN_STATE) + .value("CARTESIAN_POSE", StateType::CARTESIAN_POSE) + .value("CARTESIAN_TWIST", StateType::CARTESIAN_TWIST) + .value("CARTESIAN_ACCELERATION", StateType::CARTESIAN_ACCELERATION) + .value("CARTESIAN_WRENCH", StateType::CARTESIAN_WRENCH) + .value("JOINT_STATE", StateType::JOINT_STATE) + .value("JOINT_POSITIONS", StateType::JOINT_POSITIONS) + .value("JOINT_VELOCITIES", StateType::JOINT_VELOCITIES) + .value("JOINT_ACCELERATIONS", StateType::JOINT_ACCELERATIONS) + .value("JOINT_TORQUES", StateType::JOINT_TORQUES) + .value("JACOBIAN", StateType::JACOBIAN) + .value("PARAMETER", StateType::PARAMETER) .value("GEOMETRY_SHAPE", StateType::GEOMETRY_SHAPE) .value("GEOMETRY_ELLIPSOID", StateType::GEOMETRY_ELLIPSOID) - .value("PARAMETER_INT", StateType::PARAMETER_INT) - .value("PARAMETER_INT_ARRAY", StateType::PARAMETER_INT_ARRAY) - .value("PARAMETER_DOUBLE", StateType::PARAMETER_DOUBLE) - .value("PARAMETER_DOUBLE_ARRAY", StateType::PARAMETER_DOUBLE_ARRAY) - .value("PARAMETER_BOOL", StateType::PARAMETER_BOOL) - .value("PARAMETER_BOOL_ARRAY", StateType::PARAMETER_BOOL_ARRAY) - .value("PARAMETER_STRING", StateType::PARAMETER_STRING) - .value("PARAMETER_STRING_ARRAY", StateType::PARAMETER_STRING_ARRAY) - .value("PARAMETER_CARTESIANSTATE", StateType::PARAMETER_CARTESIANSTATE) - .value("PARAMETER_CARTESIANPOSE", StateType::PARAMETER_CARTESIANPOSE) - .value("PARAMETER_JOINTSTATE", StateType::PARAMETER_JOINTSTATE) - .value("PARAMETER_JOINTPOSITIONS", StateType::PARAMETER_JOINTPOSITIONS) - .value("PARAMETER_ELLIPSOID", StateType::PARAMETER_ELLIPSOID) - .value("PARAMETER_MATRIX", StateType::PARAMETER_MATRIX) - .value("PARAMETER_VECTOR", StateType::PARAMETER_VECTOR) + .value("TRAJECTORY", StateType::TRAJECTORY) .export_values(); } diff --git a/python/test/controllers/test_compliant_twist.py b/python/test/controllers/test_compliant_twist.py index 5d066b392..af424bd9f 100644 --- a/python/test/controllers/test_compliant_twist.py +++ b/python/test/controllers/test_compliant_twist.py @@ -18,10 +18,10 @@ def setUpClass(cls): @classmethod def set_gains(cls, lpd, lod, ast, ad): - cls.ctrl.set_parameter_value("linear_principle_damping", lpd, sr.StateType.PARAMETER_DOUBLE) - cls.ctrl.set_parameter_value("linear_orthogonal_damping", lod, sr.StateType.PARAMETER_DOUBLE) - cls.ctrl.set_parameter_value("angular_stiffness", ast, sr.StateType.PARAMETER_DOUBLE) - cls.ctrl.set_parameter_value("angular_damping", ad, sr.StateType.PARAMETER_DOUBLE) + cls.ctrl.set_parameter_value("linear_principle_damping", lpd, sr.ParameterType.DOUBLE) + cls.ctrl.set_parameter_value("linear_orthogonal_damping", lod, sr.ParameterType.DOUBLE) + cls.ctrl.set_parameter_value("angular_stiffness", ast, sr.ParameterType.DOUBLE) + cls.ctrl.set_parameter_value("angular_damping", ad, sr.ParameterType.DOUBLE) # FIXME this line sometimes fails with github actions # def test_cartesian_wrench(self): @@ -50,22 +50,22 @@ def test_get_and_set_parameters(self): if param.get_name() == "linear_principle_damping": self.assertTrue(abs(param.get_value() - 1) < 1e-5) self.assertTrue(abs(self.ctrl.get_parameter_value("linear_principle_damping") - 1) < 1e-5) - self.ctrl.set_parameter_value("linear_principle_damping", 11, sr.StateType.PARAMETER_DOUBLE) + self.ctrl.set_parameter_value("linear_principle_damping", 11, sr.ParameterType.DOUBLE) self.assertTrue(abs(self.ctrl.get_parameter_value("linear_principle_damping") - 11) < 1e-5) if param.get_name() == "linear_orthogonal_damping": self.assertTrue(abs(param.get_value() - 2) < 1e-5) self.assertTrue(abs(self.ctrl.get_parameter_value("linear_orthogonal_damping") - 2) < 1e-5) - self.ctrl.set_parameter_value("linear_orthogonal_damping", 12, sr.StateType.PARAMETER_DOUBLE) + self.ctrl.set_parameter_value("linear_orthogonal_damping", 12, sr.ParameterType.DOUBLE) self.assertTrue(abs(self.ctrl.get_parameter_value("linear_orthogonal_damping") - 12) < 1e-5) if param.get_name() == "angular_stiffness": self.assertTrue(abs(param.get_value() - 3) < 1e-5) self.assertTrue(abs(self.ctrl.get_parameter_value("angular_stiffness") - 3) < 1e-5) - self.ctrl.set_parameter_value("angular_stiffness", 13, sr.StateType.PARAMETER_DOUBLE) + self.ctrl.set_parameter_value("angular_stiffness", 13, sr.ParameterType.DOUBLE) self.assertTrue(abs(self.ctrl.get_parameter_value("angular_stiffness") - 13) < 1e-5) if param.get_name() == "angular_damping": self.assertTrue(abs(param.get_value() - 4) < 1e-5) self.assertTrue(abs(self.ctrl.get_parameter_value("angular_damping") - 4) < 1e-5) - self.ctrl.set_parameter_value("angular_damping", 14, sr.StateType.PARAMETER_DOUBLE) + self.ctrl.set_parameter_value("angular_damping", 14, sr.ParameterType.DOUBLE) self.assertTrue(abs(self.ctrl.get_parameter_value("angular_damping") - 14) < 1e-5) diff --git a/python/test/controllers/test_controller.py b/python/test/controllers/test_controller.py index 987fa623e..b0b58d17e 100644 --- a/python/test/controllers/test_controller.py +++ b/python/test/controllers/test_controller.py @@ -109,9 +109,9 @@ def test_joint_controller(self): self.assertRaises(RuntimeError, create_joint_controller, CONTROLLER_TYPE.COMPLIANT_TWIST) def test_controller_with_params(self): - param_list = [sr.Parameter("damping", 0.0, sr.StateType.PARAMETER_DOUBLE), - sr.Parameter("stiffness", 5.0, sr.StateType.PARAMETER_DOUBLE), - sr.Parameter("inertia", 10.0, sr.StateType.PARAMETER_DOUBLE)] + param_list = [sr.Parameter("damping", 0.0, sr.ParameterType.DOUBLE), + sr.Parameter("stiffness", 5.0, sr.ParameterType.DOUBLE), + sr.Parameter("inertia", 10.0, sr.ParameterType.DOUBLE)] ctrl = create_cartesian_controller(CONTROLLER_TYPE.IMPEDANCE, param_list) self.assertFalse(ctrl is None) @@ -150,7 +150,7 @@ def test_controller_with_robot(self): robot.get_number_of_joints() * robot.get_number_of_joints()) def test_controller_with_robot_and_params(self): - parameters = [sr.Parameter("damping", 5.0, sr.StateType.PARAMETER_DOUBLE)] + parameters = [sr.Parameter("damping", 5.0, sr.ParameterType.DOUBLE)] robot = Model("robot", os.path.join(os.path.dirname(os.path.realpath(__file__)), "panda_arm.urdf")) self.assertRaises(RuntimeError, create_joint_controller, CONTROLLER_TYPE.NONE, parameters, robot) diff --git a/python/test/controllers/test_dissipative_impedance.py b/python/test/controllers/test_dissipative_impedance.py index ad26ba2d0..badaf65fe 100644 --- a/python/test/controllers/test_dissipative_impedance.py +++ b/python/test/controllers/test_dissipative_impedance.py @@ -13,7 +13,7 @@ def test_compute_command_with_colinear_velocity(self): eigenvalues = ctrl.get_parameter_value("damping_eigenvalues") eigenvalues[0] = 10 - ctrl.set_parameter_value("damping_eigenvalues", eigenvalues, sr.StateType.PARAMETER_VECTOR) + ctrl.set_parameter_value("damping_eigenvalues", eigenvalues, sr.ParameterType.VECTOR) desired_twist = sr.CartesianTwist("test", [1, 0, 0]) feedback_twist = sr.CartesianTwist("test", [1, 1, 0]) diff --git a/python/test/dynamical_systems/test_cartesian_point_attractor.py b/python/test/dynamical_systems/test_cartesian_point_attractor.py index f85b239ff..96cd32e83 100644 --- a/python/test/dynamical_systems/test_cartesian_point_attractor.py +++ b/python/test/dynamical_systems/test_cartesian_point_attractor.py @@ -19,7 +19,7 @@ def test_empty_constructor(self): self.assertTrue(ds.get_parameter_value("attractor").is_empty()) self.assertTrue(ds.get_base_frame().is_empty()) - ds.set_parameter(sr.Parameter("attractor", attractor, sr.StateType.PARAMETER_CARTESIANSTATE)) + ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)) self.assertFalse(ds.get_parameter_value("attractor").is_empty()) self.assertFalse(ds.get_base_frame().is_empty()) self.assertEqual(ds.get_base_frame().get_name(), attractor.get_reference_frame()) @@ -45,7 +45,7 @@ def test_is_compatible(self): ds.evaluate(state4) ds.set_parameter(sr.Parameter("attractor", sr.CartesianState.Identity("CAttractor", "A"), - sr.StateType.PARAMETER_CARTESIANSTATE)) + sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)) self.assertTrue(ds.is_compatible(state1)) self.assertFalse(ds.is_compatible(state2)) self.assertTrue(ds.is_compatible(state3)) @@ -54,7 +54,7 @@ def test_is_compatible(self): def test_pose(self): ds = CartesianPointAttractorDS() target = sr.CartesianPose.Random("B") - ds.set_parameter(sr.Parameter("attractor", target, sr.StateType.PARAMETER_CARTESIANSTATE)) + ds.set_parameter(sr.Parameter("attractor", target, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)) current_pose = sr.CartesianPose.Identity("B") for i in range(100): @@ -70,7 +70,7 @@ def test_stacked_moving_reference_frames(self): CinA = sr.CartesianState(sr.CartesianPose.Random("C", "A")) ds = CartesianPointAttractorDS() - ds.set_parameter(sr.Parameter("attractor", BinA, sr.StateType.PARAMETER_CARTESIANSTATE)) + ds.set_parameter(sr.Parameter("attractor", BinA, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)) twist = sr.CartesianTwist(ds.evaluate(CinA)) diff --git a/python/test/dynamical_systems/test_circular.py b/python/test/dynamical_systems/test_circular.py index 6ad6af9e9..25a65a482 100644 --- a/python/test/dynamical_systems/test_circular.py +++ b/python/test/dynamical_systems/test_circular.py @@ -29,7 +29,7 @@ def test_empty_constructor(self): self.assertTrue(ds.get_parameter_value("limit_cycle").get_center_state().is_empty()) self.assertTrue(ds.get_base_frame().is_empty()) - ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID)) + ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID)) self.assertFalse(ds.get_parameter_value("limit_cycle").get_center_state().is_empty()) self.assertFalse(ds.get_base_frame().is_empty()) @@ -55,7 +55,7 @@ def test_is_compatible(self): with self.assertRaises(RuntimeError): ds.evaluate(state4) - ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID)) + ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID)) self.assertTrue(ds.is_compatible(state1)) self.assertFalse(ds.is_compatible(state2)) self.assertTrue(ds.is_compatible(state3)) @@ -64,7 +64,7 @@ def test_is_compatible(self): def test_points_on_radius_random_center(self): ds = CartesianCircularDS() self.limit_cycle.set_center_position(np.random.rand(3, 1)) - ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.StateType.PARAMETER_ELLIPSOID)) + ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID)) current_pose = sr.CartesianPose("A", 10 * np.random.rand(3, 1)) for i in range(self.nb_steps): diff --git a/python/test/dynamical_systems/test_ds.py b/python/test/dynamical_systems/test_ds.py index 092500e1e..c8e4d3af6 100644 --- a/python/test/dynamical_systems/test_ds.py +++ b/python/test/dynamical_systems/test_ds.py @@ -30,7 +30,7 @@ def test_cartesian_factory(self): self.assertTrue(cart_ds.evaluate(sr.CartesianState.Identity("C", "A")).is_empty()) self.assertTrue(len(cart_ds.get_parameter_list()) == 0) - param_list = [sr.Parameter("test", 1.0, sr.StateType.PARAMETER_DOUBLE)] + param_list = [sr.Parameter("test", 1.0, sr.ParameterType.DOUBLE)] with self.assertRaises(RuntimeError): cart_ds.set_parameters(param_list) @@ -58,7 +58,7 @@ def test_joint_factory(self): self.assertTrue(joint_ds.evaluate(sr.JointState.Random("robot", 3)).is_empty()) self.assertTrue(len(joint_ds.get_parameter_list()) == 0) - param_list = [sr.Parameter("test", 1.0, sr.StateType.PARAMETER_DOUBLE)] + param_list = [sr.Parameter("test", 1.0, sr.ParameterType.DOUBLE)] with self.assertRaises(RuntimeError): joint_ds.set_parameters(param_list) diff --git a/python/test/dynamical_systems/test_joint_point_attractor.py b/python/test/dynamical_systems/test_joint_point_attractor.py index 15a6dc56b..7b6127a00 100644 --- a/python/test/dynamical_systems/test_joint_point_attractor.py +++ b/python/test/dynamical_systems/test_joint_point_attractor.py @@ -11,13 +11,13 @@ def test_empty_constructor(self): self.assertTrue(ds.get_parameter_value("attractor").is_empty()) self.assertTrue(ds.get_base_frame().is_empty()) - ds.set_parameter(sr.Parameter("attractor", attractor, sr.StateType.PARAMETER_JOINTSTATE)) + ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)) self.assertFalse(ds.get_parameter_value("attractor").is_empty()) def test_convergence(self): ds = JointPointAttractorDS() attractor = sr.JointPositions.Random("robot", 3) - ds.set_parameter(sr.Parameter("attractor", attractor, sr.StateType.PARAMETER_JOINTSTATE)) + ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)) current_state = sr.JointPositions.Random("robot", 3) current_state.set_data(10 * current_state.data()) diff --git a/python/test/dynamical_systems/test_ring.py b/python/test/dynamical_systems/test_ring.py index be22fd5e7..3a25738ca 100644 --- a/python/test/dynamical_systems/test_ring.py +++ b/python/test/dynamical_systems/test_ring.py @@ -29,7 +29,7 @@ def test_empty_constructor(self): center = sr.CartesianPose.Identity("CAttractor", "A") self.assertTrue(ds.get_parameter_value("center").is_empty()) - ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE)) + ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)) self.assertFalse(ds.get_parameter_value("center").is_empty()) self.assertFalse(ds.get_base_frame().is_empty()) @@ -56,7 +56,7 @@ def test_is_compatible(self): with self.assertRaises(RuntimeError): ds.evaluate(state4) - ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE)) + ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)) self.assertTrue(ds.is_compatible(state1)) self.assertFalse(ds.is_compatible(state2)) self.assertTrue(ds.is_compatible(state3)) @@ -64,10 +64,10 @@ def test_is_compatible(self): def test_points_on_radius(self): ds = CartesianRingDS() - ds.set_parameter(sr.Parameter("center", self.center, sr.StateType.PARAMETER_CARTESIANPOSE)) - ds.set_parameter(sr.Parameter("radius", self.radius, sr.StateType.PARAMETER_DOUBLE)) - ds.set_parameter(sr.Parameter("width", self.width, sr.StateType.PARAMETER_DOUBLE)) - ds.set_parameter(sr.Parameter("speed", self.speed, sr.StateType.PARAMETER_DOUBLE)) + ds.set_parameter(sr.Parameter("center", self.center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)) + ds.set_parameter(sr.Parameter("radius", self.radius, sr.ParameterType.DOUBLE)) + ds.set_parameter(sr.Parameter("width", self.width, sr.ParameterType.DOUBLE)) + ds.set_parameter(sr.Parameter("speed", self.speed, sr.ParameterType.DOUBLE)) current_pose = copy.deepcopy(self.center) twist = sr.CartesianTwist(ds.evaluate(current_pose)) @@ -100,8 +100,8 @@ def test_points_on_radius(self): def test_convergence_on_radius_random_center(self): center = sr.CartesianPose.Random("A") ds = CartesianRingDS() - ds.set_parameter(sr.Parameter("center", center, sr.StateType.PARAMETER_CARTESIANPOSE)) - ds.set_parameter(sr.Parameter("radius", self.radius, sr.StateType.PARAMETER_DOUBLE)) + ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)) + ds.set_parameter(sr.Parameter("radius", self.radius, sr.ParameterType.DOUBLE)) current_pose = sr.CartesianPose("B", self.radius * np.random.rand(3, 1)) for i in range(self.nb_steps): 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 262c55f84..d00e581e3 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_state.py @@ -107,19 +107,19 @@ def test_constructors(self): empty1 = CartesianState() self.assertTrue(isinstance(empty1, State)) self.assertEqual(type(empty1), CartesianState) - self.assertEqual(empty1.get_type(), StateType.CARTESIANSTATE) + self.assertEqual(empty1.get_type(), StateType.CARTESIAN_STATE) self.assert_name_empty_frame_equal(empty1, "", True, "world") self.assertAlmostEqual(np.linalg.norm(empty1.data()), 1) empty2 = CartesianState("test") self.assertEqual(type(empty1), CartesianState) - self.assertEqual(empty2.get_type(), StateType.CARTESIANSTATE) + self.assertEqual(empty2.get_type(), StateType.CARTESIAN_STATE) self.assert_name_empty_frame_equal(empty2, "test", True, "world") self.assertAlmostEqual(np.linalg.norm(empty2.data()), 1) empty3 = CartesianState("test", "reference") self.assertEqual(type(empty1), CartesianState) - self.assertEqual(empty3.get_type(), StateType.CARTESIANSTATE) + self.assertEqual(empty3.get_type(), StateType.CARTESIAN_STATE) self.assert_name_empty_frame_equal(empty3, "test", True, "reference") self.assertAlmostEqual(np.linalg.norm(empty3.data()), 1) diff --git a/python/test/state_representation/space/test_spatial_state.py b/python/test/state_representation/space/test_spatial_state.py index e1cbd781e..0e3b0c3b3 100644 --- a/python/test/state_representation/space/test_spatial_state.py +++ b/python/test/state_representation/space/test_spatial_state.py @@ -21,14 +21,14 @@ def test_callable_methods(self): self.assertIn(expected, methods) def test_constructors(self): - state1 = SpatialState(StateType.JOINTSTATE) - self.assertEqual(state1.get_type(), StateType.JOINTSTATE) + state1 = SpatialState(StateType.JOINT_STATE) + self.assertEqual(state1.get_type(), StateType.JOINT_STATE) self.assertEqual(state1.get_name(), "") self.assertEqual(state1.get_reference_frame(), "world") self.assertTrue(state1.is_empty()) - state2 = SpatialState(StateType.CARTESIANSTATE, "test", "robot", False) - self.assertEqual(state2.get_type(), StateType.CARTESIANSTATE) + state2 = SpatialState(StateType.CARTESIAN_STATE, "test", "robot", False) + self.assertEqual(state2.get_type(), StateType.CARTESIAN_STATE) self.assertEqual(state2.get_name(), "test") self.assertEqual(state2.get_reference_frame(), "robot") self.assertFalse(state2.is_empty()) @@ -40,7 +40,7 @@ def test_constructors(self): self.assertEqual(state2.is_empty(), state3.is_empty()) def test_copy(self): - state = SpatialState(StateType.CARTESIANSTATE, "test", "robot", False) + state = SpatialState(StateType.CARTESIAN_STATE, "test", "robot", False) for state_copy in [copy.copy(state), copy.deepcopy(state)]: self.assertEqual(state.get_type(), state_copy.get_type()) self.assertEqual(state.get_name(), state_copy.get_name()) @@ -48,7 +48,7 @@ def test_copy(self): self.assertEqual(state.is_empty(), state_copy.is_empty()) def test_compatibility(self): - state1 = SpatialState(StateType.CARTESIANSTATE, "test", "robot", True) + state1 = SpatialState(StateType.CARTESIAN_STATE, "test", "robot", True) state2 = SpatialState(StateType.STATE, "test") self.assertFalse(state1.is_compatible(state2)) state2.set_reference_frame("robot") diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py index 6fb6ef6db..89f25ce0d 100755 --- a/python/test/state_representation/test_parameters.py +++ b/python/test/state_representation/test_parameters.py @@ -23,178 +23,193 @@ def joint_equal(self, joint1, joint2): self.assert_np_array_equal(joint1.data(), joint2.data()) def test_param_invalid(self): - param = sr.Parameter("test", sr.StateType.CARTESIANSTATE) - self.assertRaises(ValueError, param.set_value, 1) + param = sr.Parameter("test", sr.ParameterType.STRING) + self.assertRaises(RuntimeError, param.set_value, 1) def test_param_copy(self): - param = sr.Parameter("test", 1, sr.StateType.PARAMETER_INT) + param = sr.Parameter("test", 1, sr.ParameterType.INT) self.assertEqual(param.get_name(), "test") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT) self.assertEqual(param.get_value(), 1) param_copy = sr.Parameter(param) self.assertEqual(param_copy.get_name(), "test") - self.assertEqual(param_copy.get_type(), sr.StateType.PARAMETER_INT) + self.assertEqual(param_copy.get_parameter_type(), sr.ParameterType.INT) self.assertEqual(param_copy.get_value(), 1) def test_param_int(self): - param = sr.Parameter("int", sr.StateType.PARAMETER_INT) + param = sr.Parameter("int", sr.ParameterType.INT) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "int") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) param.set_value(1) self.assertEqual(param.get_value(), 1) - param1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_INT) + param1 = sr.Parameter("int", 1, sr.ParameterType.INT) self.assertEqual(param1.get_value(), 1) def test_param_int_array(self): - param = sr.Parameter("int_array", sr.StateType.PARAMETER_INT_ARRAY) + param = sr.Parameter("int_array", sr.ParameterType.INT_ARRAY) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "int_array") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_INT_ARRAY) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT_ARRAY) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) values = [2, 3, 4] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] - param1 = sr.Parameter("int_array", values, sr.StateType.PARAMETER_INT_ARRAY) + param1 = sr.Parameter("int_array", values, sr.ParameterType.INT_ARRAY) [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))] def test_param_double(self): - param = sr.Parameter("double", sr.StateType.PARAMETER_DOUBLE) + param = sr.Parameter("double", sr.ParameterType.DOUBLE) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "double") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) param.set_value(1.5) self.assertEqual(param.get_value(), 1.5) - param1 = sr.Parameter("double", 1.5, sr.StateType.PARAMETER_DOUBLE) + param1 = sr.Parameter("double", 1.5, sr.ParameterType.DOUBLE) self.assertEqual(param1.get_value(), 1.5) def test_param_double_array(self): - param = sr.Parameter("double_array", sr.StateType.PARAMETER_DOUBLE_ARRAY) + param = sr.Parameter("double_array", sr.ParameterType.DOUBLE_ARRAY) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "double_array") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_DOUBLE_ARRAY) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE_ARRAY) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) values = [2.2, 3.3, 4.4] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] - param1 = sr.Parameter("double_array", values, sr.StateType.PARAMETER_DOUBLE_ARRAY) + param1 = sr.Parameter("double_array", values, sr.ParameterType.DOUBLE_ARRAY) [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))] def test_param_bool(self): - param = sr.Parameter("bool", sr.StateType.PARAMETER_BOOL) + param = sr.Parameter("bool", sr.ParameterType.BOOL) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "bool") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) param.set_value(False) self.assertEqual(param.get_value(), False) - param1 = sr.Parameter("bool", False, sr.StateType.PARAMETER_BOOL) + param1 = sr.Parameter("bool", False, sr.ParameterType.BOOL) self.assertEqual(param1.get_value(), False) def test_param_bool_array(self): - param = sr.Parameter("bool_array", sr.StateType.PARAMETER_BOOL_ARRAY) + param = sr.Parameter("bool_array", sr.ParameterType.BOOL_ARRAY) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "bool_array") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_BOOL_ARRAY) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL_ARRAY) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) values = [True, False, False] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] - param1 = sr.Parameter("bool_array", values, sr.StateType.PARAMETER_BOOL_ARRAY) + param1 = sr.Parameter("bool_array", values, sr.ParameterType.BOOL_ARRAY) [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))] def test_param_string(self): - param = sr.Parameter("string", sr.StateType.PARAMETER_STRING) + param = sr.Parameter("string", sr.ParameterType.STRING) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "string") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) param.set_value("parameter") self.assertEqual(param.get_value(), "parameter") - param1 = sr.Parameter("string", "parameter", sr.StateType.PARAMETER_STRING) + param1 = sr.Parameter("string", "parameter", sr.ParameterType.STRING) self.assertEqual(param1.get_value(), "parameter") def test_param_string_array(self): - param = sr.Parameter("string_array", sr.StateType.PARAMETER_STRING_ARRAY) + param = sr.Parameter("string_array", sr.ParameterType.STRING_ARRAY) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "string_array") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_STRING_ARRAY) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING_ARRAY) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) values = ["test", "parameter", "bindings"] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] - param1 = sr.Parameter("string_array", values, sr.StateType.PARAMETER_STRING_ARRAY) + param1 = sr.Parameter("string_array", values, sr.ParameterType.STRING_ARRAY) [self.assertEqual(param1.get_value()[i], values[i]) for i in range(len(values))] def test_param_cartesian_state(self): - param = sr.Parameter("cartesian_state", sr.StateType.PARAMETER_CARTESIANSTATE) + param = sr.Parameter("cartesian_state", sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "cartesian_state") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_CARTESIANSTATE) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_STATE) values = sr.CartesianState.Random("test") param.set_value(values) self.cartesian_equal(param.get_value(), values) - param1 = sr.Parameter("cartesian_state", values, sr.StateType.PARAMETER_CARTESIANSTATE) + param1 = sr.Parameter("cartesian_state", values, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE) self.cartesian_equal(param1.get_value(), values) def test_param_cartesian_pose(self): - param = sr.Parameter("cartesian_pose", sr.StateType.PARAMETER_CARTESIANPOSE) + param = sr.Parameter("cartesian_pose", sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "cartesian_pose") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_CARTESIANPOSE) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_POSE) values = sr.CartesianPose.Random("test") param.set_value(values) self.cartesian_equal(param.get_value(), values) - param1 = sr.Parameter("cartesian_pose", values, sr.StateType.PARAMETER_CARTESIANPOSE) + param1 = sr.Parameter("cartesian_pose", values, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE) self.cartesian_equal(param1.get_value(), values) def test_param_joint_state(self): - param = sr.Parameter("joint_state", sr.StateType.PARAMETER_JOINTSTATE) + param = sr.Parameter("joint_state", sr.ParameterType.STATE, sr.StateType.JOINT_STATE) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "joint_state") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTSTATE) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_STATE) values = sr.JointState.Random("test", 3) param.set_value(values) self.joint_equal(param.get_value(), values) - param1 = sr.Parameter("joint_state", values, sr.StateType.PARAMETER_JOINTSTATE) + param1 = sr.Parameter("joint_state", values, sr.ParameterType.STATE, sr.StateType.JOINT_STATE) self.joint_equal(param1.get_value(), values) def test_param_joint_positions(self): - param = sr.Parameter("joint_positions", sr.StateType.PARAMETER_JOINTPOSITIONS) + param = sr.Parameter("joint_positions", sr.ParameterType.STATE, sr.StateType.JOINT_POSITIONS) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "joint_positions") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_JOINTPOSITIONS) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_POSITIONS) values = sr.JointPositions.Random("test", 3) param.set_value(values) self.joint_equal(param.get_value(), values) - param1 = sr.Parameter("joint_positions", values, sr.StateType.PARAMETER_JOINTPOSITIONS) + param1 = sr.Parameter("joint_positions", values, sr.ParameterType.STATE, sr.StateType.JOINT_POSITIONS) self.joint_equal(param1.get_value(), values) def test_param_ellipsoid(self): - param = sr.Parameter("ellipse", sr.StateType.PARAMETER_ELLIPSOID) + param = sr.Parameter("ellipse", sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "ellipse") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_ELLIPSOID) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.GEOMETRY_ELLIPSOID) values = sr.Ellipsoid("test") param.set_value(values) self.assertTrue(param.get_value().get_name(), values.get_name()) - param1 = sr.Parameter("ellipse", values, sr.StateType.PARAMETER_ELLIPSOID) + param1 = sr.Parameter("ellipse", values, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID) self.assertTrue(param1.get_value().get_name(), values.get_name()) def test_param_matrix(self): - param = sr.Parameter("matrix", sr.StateType.PARAMETER_MATRIX) + param = sr.Parameter("matrix", sr.ParameterType.MATRIX) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "matrix") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_MATRIX) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.MATRIX) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) values = np.random.rand(3, 2) param.set_value(values) self.assert_np_array_equal(param.get_value(), values) - param1 = sr.Parameter("matrix", values, sr.StateType.PARAMETER_MATRIX) + param1 = sr.Parameter("matrix", values, sr.ParameterType.MATRIX) self.assert_np_array_equal(param1.get_value(), values) def test_param_vector(self): - param = sr.Parameter("vector", sr.StateType.PARAMETER_VECTOR) + param = sr.Parameter("vector", sr.ParameterType.VECTOR) self.assertTrue(param.is_empty()) self.assertEqual(param.get_name(), "vector") - self.assertEqual(param.get_type(), sr.StateType.PARAMETER_VECTOR) + self.assertEqual(param.get_parameter_type(), sr.ParameterType.VECTOR) + self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) values = np.random.rand(3) param.set_value(values) self.assert_np_array_equal(param.get_value(), values) - param1 = sr.Parameter("vector", values, sr.StateType.PARAMETER_VECTOR) + param1 = sr.Parameter("vector", values, sr.ParameterType.VECTOR) self.assert_np_array_equal(param1.get_value(), values) def param_map_equal(self, param_dict, param_map): @@ -218,9 +233,9 @@ def simple_param_equal(param1, param2): def test_param_map(self): - param_dict = {"int": sr.Parameter("int", 1, sr.StateType.PARAMETER_INT), - "double": sr.Parameter("double", 2.2, sr.StateType.PARAMETER_DOUBLE), - "string": sr.Parameter("string", "test", sr.StateType.PARAMETER_STRING)} + param_dict = {"int": sr.Parameter("int", 1, sr.ParameterType.INT), + "double": sr.Parameter("double", 2.2, sr.ParameterType.DOUBLE), + "string": sr.Parameter("string", "test", sr.ParameterType.STRING)} param_list = [param for name, param in param_dict.items()] m = sr.ParameterMap() @@ -230,7 +245,7 @@ def test_param_map(self): m = sr.ParameterMap() for name, param in param_dict.items(): - m.set_parameter_value(param.get_name(), param.get_value(), param.get_type()) + m.set_parameter_value(param.get_name(), param.get_value(), param.get_parameter_type(), param.get_parameter_state_type()) self.param_map_equal(param_dict, m) m = sr.ParameterMap() diff --git a/python/test/state_representation/test_state.py b/python/test/state_representation/test_state.py index 66ce225ce..0b80e54d9 100644 --- a/python/test/state_representation/test_state.py +++ b/python/test/state_representation/test_state.py @@ -34,20 +34,20 @@ def test_constructors(self): self.assertEqual(empty1.get_name(), "") self.assertTrue(empty1.is_empty()) - empty2 = State(StateType.JOINTSTATE) - self.assertEqual(empty2.get_type(), StateType.JOINTSTATE) + empty2 = State(StateType.JOINT_STATE) + self.assertEqual(empty2.get_type(), StateType.JOINT_STATE) self.assertEqual(empty2.get_name(), "") self.assertTrue(empty2.is_empty()) - empty3 = State(StateType.CARTESIANSTATE, "test", True) - self.assertEqual(empty3.get_type(), StateType.CARTESIANSTATE) + empty3 = State(StateType.CARTESIAN_STATE, "test", True) + self.assertEqual(empty3.get_type(), StateType.CARTESIAN_STATE) self.assertEqual(empty3.get_name(), "test") self.assertTrue(empty3.is_empty()) empty3.set_filled() self.assertFalse(empty3.is_empty()) state = State(empty3) - self.assertEqual(state.get_type(), StateType.CARTESIANSTATE) + self.assertEqual(state.get_type(), StateType.CARTESIAN_STATE) self.assertEqual(state.get_name(), "test") self.assertFalse(state.is_empty()) state.set_empty() diff --git a/python/test/test_clproto.py b/python/test/test_clproto.py index b1dafa672..75a3a87f7 100755 --- a/python/test/test_clproto.py +++ b/python/test/test_clproto.py @@ -149,47 +149,47 @@ def param_encode_decode_tester(self, obj1, message_type): self.assertEqual(obj1.get_value(), obj2.get_value()) def test_encode_decode_param_int(self): - obj1 = sr.Parameter("int", 1, sr.StateType.PARAMETER_INT) + obj1 = sr.Parameter("int", 1, sr.ParameterType.INT) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.INT) def test_encode_decode_param_int_array(self): - obj1 = sr.Parameter("int", [1, 2, 3], sr.StateType.PARAMETER_INT_ARRAY) + obj1 = sr.Parameter("int", [1, 2, 3], sr.ParameterType.INT_ARRAY) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.INT_ARRAY) def test_encode_decode_param_double(self): - obj1 = sr.Parameter("double", 1.1, sr.StateType.PARAMETER_DOUBLE) + obj1 = sr.Parameter("double", 1.1, sr.ParameterType.DOUBLE) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.DOUBLE) def test_encode_decode_param_double_array(self): - obj1 = sr.Parameter("double", [1.1, 2.2, 3.3], sr.StateType.PARAMETER_DOUBLE_ARRAY) + obj1 = sr.Parameter("double", [1.1, 2.2, 3.3], sr.ParameterType.DOUBLE_ARRAY) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.DOUBLE_ARRAY) def test_encode_decode_param_bool(self): - obj1 = sr.Parameter("bool", True, sr.StateType.PARAMETER_BOOL) + obj1 = sr.Parameter("bool", True, sr.ParameterType.BOOL) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.BOOL) def test_encode_decode_param_bool_array(self): - obj1 = sr.Parameter("bool", [True, False, True], sr.StateType.PARAMETER_BOOL_ARRAY) + obj1 = sr.Parameter("bool", [True, False, True], sr.ParameterType.BOOL_ARRAY) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.BOOL_ARRAY) def test_encode_decode_param_string(self): - obj1 = sr.Parameter("string", "test", sr.StateType.PARAMETER_STRING) + obj1 = sr.Parameter("string", "test", sr.ParameterType.STRING) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.STRING) def test_encode_decode_param_string_array(self): - obj1 = sr.Parameter("string", ["1", "2", "3"], sr.StateType.PARAMETER_STRING_ARRAY) + obj1 = sr.Parameter("string", ["1", "2", "3"], sr.ParameterType.STRING_ARRAY) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.STRING_ARRAY) def test_encode_decode_param_vector(self): - obj1 = sr.Parameter("vector", np.random.rand(3), sr.StateType.PARAMETER_VECTOR) + obj1 = sr.Parameter("vector", np.random.rand(3), sr.ParameterType.VECTOR) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.VECTOR) def test_encode_decode_param_matrix(self): - obj1 = sr.Parameter("matrix", np.random.rand(3, 2), sr.StateType.PARAMETER_MATRIX) + obj1 = sr.Parameter("matrix", np.random.rand(3, 2), sr.ParameterType.MATRIX) self.param_encode_decode_tester(obj1, clproto.ParameterMessageType.MATRIX) def test_encode_decode_param_invalid(self): - obj = sr.Parameter("cartesian_state", sr.StateType.PARAMETER_CARTESIANSTATE) + obj = sr.Parameter("cartesian_state", sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE) self.assertRaises(ValueError, clproto.encode, obj, clproto.MessageType.PARAMETER_MESSAGE) if __name__ == '__main__': From bfd1bb314aa998b80a7dcdadeab1477ce672dda1 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 12 Apr 2022 13:58:24 +0200 Subject: [PATCH 06/17] StateType default construction (#284) * Set StateType in empty constructors * For Cartesian pose, twist, acceleration and wrench, and for joint positions, velocities, accelerations and toruqes, set the StateType in the empty constructor for correct behaviour * Increase StateType test coverage * Changelog * 5.1.7 -> 5.1.8 * Fix ParameterInterface constructors * In copy and assignment constructors, set the parameter type and parameter state type properties * Add test for copied properties Co-authored-by: Dominic Reber 5.2.1 -> 5.2.2 --- CHANGELOG.md | 2 +- VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- .../clproto_cpp/test/tests/test_cartesian_space_proto.cpp | 5 +++++ protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp | 6 ++++++ python/setup.py | 2 +- source/CMakeLists.txt | 2 +- .../space/cartesian/CartesianAcceleration.hpp | 2 +- .../state_representation/space/cartesian/CartesianPose.hpp | 2 +- .../space/cartesian/CartesianTwist.hpp | 2 +- .../space/cartesian/CartesianWrench.hpp | 2 +- .../space/joint/JointAccelerations.hpp | 2 +- .../state_representation/space/joint/JointPositions.hpp | 2 +- .../state_representation/space/joint/JointTorques.hpp | 2 +- .../state_representation/space/joint/JointVelocities.hpp | 2 +- .../src/parameters/ParameterInterface.cpp | 7 ++++++- .../src/space/cartesian/CartesianAcceleration.cpp | 4 ++++ .../src/space/cartesian/CartesianPose.cpp | 4 ++++ .../src/space/cartesian/CartesianTwist.cpp | 4 ++++ .../src/space/cartesian/CartesianWrench.cpp | 4 ++++ .../src/space/joint/JointAccelerations.cpp | 4 ++++ .../src/space/joint/JointPositions.cpp | 4 ++++ .../state_representation/src/space/joint/JointTorques.cpp | 4 ++++ .../src/space/joint/JointVelocities.cpp | 4 ++++ .../tests/space/cartesian/test_cartesian_acceleration.cpp | 5 +++++ .../test/tests/space/cartesian/test_cartesian_pose.cpp | 5 +++++ .../test/tests/space/cartesian/test_cartesian_state.cpp | 1 + .../test/tests/space/cartesian/test_cartesian_twist.cpp | 5 +++++ .../test/tests/space/cartesian/test_cartesian_wrench.cpp | 5 +++++ .../test/tests/space/joint/test_joint_accelerations.cpp | 3 +++ .../test/tests/space/joint/test_joint_positions.cpp | 3 +++ .../test/tests/space/joint/test_joint_torques.cpp | 3 +++ .../test/tests/space/joint/test_joint_velocities.cpp | 3 +++ source/state_representation/test/tests/test_parameter.cpp | 7 +++++++ 35 files changed, 103 insertions(+), 15 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 112476687..51e72ea82 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,7 +14,7 @@ Release Versions: ## Upcoming changes (in development) - Fix if else conditions in setup.py to correctly install modules (#285) -- Refactor state type (#277, #278, #280) +- Refactor state type (#277, #278, #280, #284) ## 5.2.0 diff --git a/VERSION b/VERSION index 26d99a283..ce7f2b425 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.1 +5.2.2 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index eb5ea7ce0..9ceb7f3b0 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.2.1 +PROJECT_NUMBER = 5.2.2 # 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 78a644e68..71500cb7a 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.2.1) +project(clproto VERSION 5.2.2) set(CMAKE_CXX_STANDARD 17) diff --git a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp index 1909b7b9a..f9c3ba54f 100644 --- a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp +++ b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp @@ -38,6 +38,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianState) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); @@ -54,6 +55,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianPose) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); @@ -70,6 +72,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianTwist) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); @@ -86,6 +89,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianAcceleration) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); @@ -102,6 +106,7 @@ TEST(CartesianProtoTest, EncodeDecodeCartesianWrench) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); diff --git a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp index 20aa85505..cfc9ebd55 100644 --- a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp +++ b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp @@ -22,6 +22,7 @@ TEST(JointProtoTest, EncodeDecodeJacobian) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_frame().c_str(), recv_state.get_frame().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); @@ -46,6 +47,7 @@ TEST(JointProtoTest, EncodeDecodeJointState) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); ASSERT_EQ(send_state.get_size(), recv_state.get_size()); for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { @@ -65,6 +67,7 @@ TEST(JointProtoTest, EncodeDecodeJointPositions) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); ASSERT_EQ(send_state.get_size(), recv_state.get_size()); for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { @@ -84,6 +87,7 @@ TEST(JointProtoTest, EncodeDecodeJointVelocities) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); ASSERT_EQ(send_state.get_size(), recv_state.get_size()); for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { @@ -103,6 +107,7 @@ TEST(JointProtoTest, EncodeDecodeJointAccelerations) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); ASSERT_EQ(send_state.get_size(), recv_state.get_size()); for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { @@ -122,6 +127,7 @@ TEST(JointProtoTest, EncodeDecodeJointTorques) { EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); ASSERT_EQ(send_state.get_size(), recv_state.get_size()); for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { diff --git a/python/setup.py b/python/setup.py index b162efc39..627598ca4 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.2.1" +__version__ = "5.2.2" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index ec7f1e7d8..e52ded235 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.1) +project(control_libraries VERSION 5.2.2) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp index abc545b88..44260ec9c 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp @@ -49,7 +49,7 @@ class CartesianAcceleration : public CartesianState { /** * @brief Empty constructor */ - explicit CartesianAcceleration() = default; + explicit CartesianAcceleration(); /** * @brief Constructor with name and reference frame provided diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp index b9031ef39..43152bc1d 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp @@ -44,7 +44,7 @@ class CartesianPose : public CartesianState { /** * @brief Empty constructor */ - explicit CartesianPose() = default; + explicit CartesianPose(); /** * @brief Constructor with name and reference frame provided diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp index 8efa82f6d..064c49b25 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp @@ -51,7 +51,7 @@ class CartesianTwist : public CartesianState { /** * @brief Empty constructor */ - explicit CartesianTwist() = default; + explicit CartesianTwist(); /** * @brief Constructor with name and reference frame provided diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp index 182e88fd2..92fc01ef2 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp @@ -50,7 +50,7 @@ class CartesianWrench : public CartesianState { /** * @brief Empty constructor */ - explicit CartesianWrench() = default; + explicit CartesianWrench(); /** * @brief Constructor with name and reference frame provided diff --git a/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp b/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp index d63703754..696d1b378 100644 --- a/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp +++ b/source/state_representation/include/state_representation/space/joint/JointAccelerations.hpp @@ -40,7 +40,7 @@ class JointAccelerations : public JointState { /** * @brief Empty constructor */ - explicit JointAccelerations() = default; + explicit JointAccelerations(); /** * @brief Constructor with name and number of joints provided diff --git a/source/state_representation/include/state_representation/space/joint/JointPositions.hpp b/source/state_representation/include/state_representation/space/joint/JointPositions.hpp index 3ed0042ef..9912ff229 100644 --- a/source/state_representation/include/state_representation/space/joint/JointPositions.hpp +++ b/source/state_representation/include/state_representation/space/joint/JointPositions.hpp @@ -40,7 +40,7 @@ class JointPositions : public JointState { /** * @brief Empty constructor */ - explicit JointPositions() = default; + explicit JointPositions(); /** * @brief Constructor with name and number of joints provided diff --git a/source/state_representation/include/state_representation/space/joint/JointTorques.hpp b/source/state_representation/include/state_representation/space/joint/JointTorques.hpp index 561826229..fdceb1637 100644 --- a/source/state_representation/include/state_representation/space/joint/JointTorques.hpp +++ b/source/state_representation/include/state_representation/space/joint/JointTorques.hpp @@ -37,7 +37,7 @@ class JointTorques : public JointState { /** * @brief Empty constructor */ - explicit JointTorques() = default; + explicit JointTorques(); /** * @brief Constructor with name and number of joints provided diff --git a/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp b/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp index 3a35ee979..1a1b25250 100644 --- a/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp +++ b/source/state_representation/include/state_representation/space/joint/JointVelocities.hpp @@ -42,7 +42,7 @@ class JointVelocities : public JointState { /** * @brief Empty constructor */ - explicit JointVelocities() = default; + explicit JointVelocities(); /** * @brief Constructor with name and number of joints provided diff --git a/source/state_representation/src/parameters/ParameterInterface.cpp b/source/state_representation/src/parameters/ParameterInterface.cpp index 442aef4fb..dfb7a47d4 100644 --- a/source/state_representation/src/parameters/ParameterInterface.cpp +++ b/source/state_representation/src/parameters/ParameterInterface.cpp @@ -6,10 +6,15 @@ ParameterInterface::ParameterInterface( const std::string& name, const ParameterType& type, const StateType& parameter_state_type ) : State(StateType::PARAMETER, name), parameter_type_(type), parameter_state_type_(parameter_state_type) {} -ParameterInterface::ParameterInterface(const ParameterInterface& parameter) : State(parameter) {} +ParameterInterface::ParameterInterface(const ParameterInterface& parameter) : + State(parameter), + parameter_type_(parameter.get_parameter_type()), + parameter_state_type_(parameter.get_parameter_state_type()) {} ParameterInterface& ParameterInterface::operator=(const ParameterInterface& state) { State::operator=(state); + this->parameter_type_ = state.get_parameter_type(); + this->parameter_state_type_ = state.get_parameter_state_type(); return (*this); } diff --git a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp index db0961240..8eae5eb17 100644 --- a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp +++ b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp @@ -4,6 +4,10 @@ using namespace state_representation::exceptions; namespace state_representation { +CartesianAcceleration::CartesianAcceleration() { + this->set_type(StateType::CARTESIAN_ACCELERATION); +} + CartesianAcceleration::CartesianAcceleration(const std::string& name, const std::string& reference) : CartesianState(name, reference) { this->set_type(StateType::CARTESIAN_ACCELERATION); diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp index 5cabd1a91..3d832b73a 100644 --- a/source/state_representation/src/space/cartesian/CartesianPose.cpp +++ b/source/state_representation/src/space/cartesian/CartesianPose.cpp @@ -5,6 +5,10 @@ using namespace state_representation::exceptions; namespace state_representation { +CartesianPose::CartesianPose() { + this->set_type(StateType::CARTESIAN_POSE); +} + CartesianPose::CartesianPose(const std::string& name, const std::string& reference) : CartesianState(name, reference) { this->set_type(StateType::CARTESIAN_POSE); } diff --git a/source/state_representation/src/space/cartesian/CartesianTwist.cpp b/source/state_representation/src/space/cartesian/CartesianTwist.cpp index 20e64a31f..49cc49f24 100644 --- a/source/state_representation/src/space/cartesian/CartesianTwist.cpp +++ b/source/state_representation/src/space/cartesian/CartesianTwist.cpp @@ -4,6 +4,10 @@ using namespace state_representation::exceptions; namespace state_representation { +CartesianTwist::CartesianTwist() { + this->set_type(StateType::CARTESIAN_TWIST); +} + CartesianTwist::CartesianTwist(const std::string& name, const std::string& reference) : CartesianState(name, reference) { this->set_type(StateType::CARTESIAN_TWIST); diff --git a/source/state_representation/src/space/cartesian/CartesianWrench.cpp b/source/state_representation/src/space/cartesian/CartesianWrench.cpp index 31221618e..e3a1714d1 100644 --- a/source/state_representation/src/space/cartesian/CartesianWrench.cpp +++ b/source/state_representation/src/space/cartesian/CartesianWrench.cpp @@ -3,6 +3,10 @@ using namespace state_representation::exceptions; namespace state_representation { +CartesianWrench::CartesianWrench() { + this->set_type(StateType::CARTESIAN_WRENCH); +} + CartesianWrench::CartesianWrench(const std::string& name, const std::string& reference) : CartesianState(name, reference) { this->set_type(StateType::CARTESIAN_WRENCH); diff --git a/source/state_representation/src/space/joint/JointAccelerations.cpp b/source/state_representation/src/space/joint/JointAccelerations.cpp index e109262e9..4fd7f2c02 100644 --- a/source/state_representation/src/space/joint/JointAccelerations.cpp +++ b/source/state_representation/src/space/joint/JointAccelerations.cpp @@ -4,6 +4,10 @@ using namespace state_representation::exceptions; namespace state_representation { +JointAccelerations::JointAccelerations() { + this->set_type(StateType::JOINT_ACCELERATIONS); +} + JointAccelerations::JointAccelerations(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) { this->set_type(StateType::JOINT_ACCELERATIONS); diff --git a/source/state_representation/src/space/joint/JointPositions.cpp b/source/state_representation/src/space/joint/JointPositions.cpp index ae9f45334..7e23f38a8 100644 --- a/source/state_representation/src/space/joint/JointPositions.cpp +++ b/source/state_representation/src/space/joint/JointPositions.cpp @@ -5,6 +5,10 @@ using namespace state_representation::exceptions; namespace state_representation { +JointPositions::JointPositions() { + this->set_type(StateType::JOINT_POSITIONS); +} + JointPositions::JointPositions(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) { this->set_type(StateType::JOINT_POSITIONS); diff --git a/source/state_representation/src/space/joint/JointTorques.cpp b/source/state_representation/src/space/joint/JointTorques.cpp index a59a5872e..e5e394ecb 100644 --- a/source/state_representation/src/space/joint/JointTorques.cpp +++ b/source/state_representation/src/space/joint/JointTorques.cpp @@ -3,6 +3,10 @@ using namespace state_representation::exceptions; namespace state_representation { +JointTorques::JointTorques() { + this->set_type(StateType::JOINT_TORQUES); +} + JointTorques::JointTorques(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) { this->set_type(StateType::JOINT_TORQUES); } diff --git a/source/state_representation/src/space/joint/JointVelocities.cpp b/source/state_representation/src/space/joint/JointVelocities.cpp index cfb597e60..737c69a4b 100644 --- a/source/state_representation/src/space/joint/JointVelocities.cpp +++ b/source/state_representation/src/space/joint/JointVelocities.cpp @@ -4,6 +4,10 @@ using namespace state_representation::exceptions; namespace state_representation { +JointVelocities::JointVelocities() { + this->set_type(StateType::JOINT_VELOCITIES); +} + JointVelocities::JointVelocities(const std::string& robot_name, unsigned int nb_joints) : JointState(robot_name, nb_joints) { this->set_type(StateType::JOINT_VELOCITIES); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp index f569217c3..262e543f7 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_acceleration.cpp @@ -12,6 +12,11 @@ static void expect_only_acceleration(CartesianAcceleration& acceleration) { EXPECT_EQ(static_cast(acceleration).get_wrench().norm(), 0); } +TEST(CartesianAccelerationTest, EmptyConstructor) { + CartesianAcceleration empty; + EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_ACCELERATION); +} + TEST(CartesianAccelerationTest, RandomAccelerationInitialization) { CartesianAcceleration random = CartesianAcceleration::Random("test"); EXPECT_EQ(random.get_type(), StateType::CARTESIAN_ACCELERATION); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp index 18398d37e..8d4721598 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_pose.cpp @@ -34,6 +34,11 @@ class CartesianPoseTestClass : public testing::Test { double tol = 1e-5; }; +TEST(CartesianPoseTest, EmptyConstructor) { + CartesianPose empty; + EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_POSE); +} + TEST(CartesianPoseTest, RandomPoseInitialization) { CartesianPose random = CartesianPose::Random("test"); EXPECT_EQ(random.get_type(), StateType::CARTESIAN_POSE); 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 0fa832529..26646af30 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 @@ -45,6 +45,7 @@ void test_clamping( TEST(CartesianStateTest, Constructors) { CartesianState empty1; + EXPECT_EQ(empty1.get_type(), StateType::CARTESIAN_STATE); assert_name_empty_frame_equal(empty1, "", true, "world"); EXPECT_FLOAT_EQ(empty1.data().norm(), 1); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp index 845ee0715..c390787e8 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_twist.cpp @@ -12,6 +12,11 @@ static void expect_only_twist(CartesianTwist& twist) { EXPECT_EQ(static_cast(twist).get_wrench().norm(), 0); } +TEST(CartesianTwistTest, EmptyConstructor) { + CartesianTwist empty; + EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_TWIST); +} + TEST(CartesianTwistTest, RandomTwistInitialization) { CartesianTwist random = CartesianTwist::Random("test"); EXPECT_EQ(random.get_type(), StateType::CARTESIAN_TWIST); diff --git a/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp b/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp index 2b7701652..92caf2051 100644 --- a/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp +++ b/source/state_representation/test/tests/space/cartesian/test_cartesian_wrench.cpp @@ -12,6 +12,11 @@ static void expect_only_wrench(CartesianWrench& wrench) { EXPECT_EQ(static_cast(wrench).get_acceleration().norm(), 0); } +TEST(CartesianWrenchTest, EmptyConstructor) { + CartesianWrench empty; + EXPECT_EQ(empty.get_type(), StateType::CARTESIAN_WRENCH); +} + TEST(CartesianWrenchTest, RandomWrenchInitialization) { CartesianWrench random = CartesianWrench::Random("test"); EXPECT_EQ(random.get_type(), StateType::CARTESIAN_WRENCH); diff --git a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp index 8683a4a2b..f44c61152 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_accelerations.cpp @@ -11,6 +11,9 @@ static void expect_only_accelerations(JointAccelerations& acc) { } TEST(JointAccelerationsTest, Constructors) { + JointAccelerations empty; + EXPECT_EQ(empty.get_type(), StateType::JOINT_ACCELERATIONS); + std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d accelerations = Eigen::Vector2d::Random(); JointAccelerations ja1("test", accelerations); diff --git a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp index ad829ba97..220c76c27 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_positions.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_positions.cpp @@ -11,6 +11,9 @@ static void expect_only_positions(JointPositions& pos) { } TEST(JointPositionsTest, Constructors) { + JointPositions empty; + EXPECT_EQ(empty.get_type(), StateType::JOINT_POSITIONS); + std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d positions = Eigen::Vector2d::Random(); JointPositions jp1("test", positions); diff --git a/source/state_representation/test/tests/space/joint/test_joint_torques.cpp b/source/state_representation/test/tests/space/joint/test_joint_torques.cpp index 1caaeb115..2ea564cfe 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_torques.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_torques.cpp @@ -11,6 +11,9 @@ static void expect_only_torques(JointTorques& tor) { } TEST(JointTorquesTest, Constructors) { + JointTorques empty; + EXPECT_EQ(empty.get_type(), StateType::JOINT_TORQUES); + std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d torques = Eigen::Vector2d::Random(); JointTorques jt1("test", torques); diff --git a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp index 28a3996b2..deb8cbfb6 100644 --- a/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp +++ b/source/state_representation/test/tests/space/joint/test_joint_velocities.cpp @@ -11,6 +11,9 @@ static void expect_only_velocities(JointVelocities& vel) { } TEST(JointVelocitiesTest, Constructors) { + JointVelocities empty; + EXPECT_EQ(empty.get_type(), StateType::JOINT_VELOCITIES); + std::vector joint_names{"joint_10", "joint_20"}; Eigen::Vector2d velocities = Eigen::Vector2d::Random(); JointVelocities jv1("test", velocities); diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp index 3adfb848d..03f8a1a04 100644 --- a/source/state_representation/test/tests/test_parameter.cpp +++ b/source/state_representation/test/tests/test_parameter.cpp @@ -10,6 +10,12 @@ using namespace state_representation; +TEST(ParameterTest, CopyConstructor) { + Parameter int_param("test", 1); + ParameterInterface int_param_interface(int_param); + EXPECT_EQ(int_param_interface.get_parameter_type(), int_param.get_parameter_type()); +} + TEST(ParameterTest, Conversion) { Parameter int_param("test"); EXPECT_TRUE(int_param.is_empty()); @@ -117,6 +123,7 @@ TEST(ParameterTest, ParameterThroughInterface) { EXPECT_TRUE(param->get_value().data().isApprox(pose.data())); auto param_value = param_interface->get_parameter_value(); + EXPECT_EQ(param_value.get_type(), StateType::CARTESIAN_POSE); EXPECT_EQ(param_value.get_name(), "A"); EXPECT_EQ(param_value.get_reference_frame(), "B"); EXPECT_TRUE(param_value.data().isApprox(pose.data())); From e95465ecb6e67acfbd759b94e7bbea17a4f34fb2 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 14 Apr 2022 08:19:25 +0200 Subject: [PATCH 07/17] Refactor dynamical systems factory (#288) * Add constructors with parameters * Refactor DS factory with non templated type and parameters and add typedefs * Update cpp tests * Rename controller files for clarity * Rename DS files for clarity * Update bindings * Update python tests * 5.2.2 -> 5.2.3 * Update CHANGELOG --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/Dockerfile.python | 1 + python/include/controllers_bindings.h | 2 +- python/include/dynamical_systems_bindings.h | 6 +-- python/setup.py | 2 +- ...bind_type.cpp => bind_controller_type.cpp} | 2 +- .../controllers/controllers_bindings.cpp | 2 +- .../dynamical_systems/bind_cartesian.cpp | 51 ------------------- .../dynamical_systems/bind_cartesian_ds.cpp | 32 ++++++++++++ .../source/dynamical_systems/bind_ds_type.cpp | 14 +++++ .../source/dynamical_systems/bind_joint.cpp | 41 --------------- .../dynamical_systems/bind_joint_ds.cpp | 33 ++++++++++++ python/source/dynamical_systems/bind_type.cpp | 15 ------ .../dynamical_systems_bindings.cpp | 6 +-- .../test_cartesian_point_attractor.py | 10 ++-- .../test/dynamical_systems/test_circular.py | 8 +-- python/test/dynamical_systems/test_ds.py | 14 ++--- .../test_joint_point_attractor.py | 12 +++-- python/test/dynamical_systems/test_ring.py | 10 ++-- source/CMakeLists.txt | 2 +- .../include/dynamical_systems/Circular.hpp | 6 +++ .../DynamicalSystemFactory.hpp | 35 +++++++++---- .../dynamical_systems/DynamicalSystemType.hpp | 15 ++++++ .../dynamical_systems/PointAttractor.hpp | 14 +++++ .../include/dynamical_systems/Ring.hpp | 6 +++ .../InvalidDynamicalSystemException.hpp | 11 ++++ source/dynamical_systems/src/Circular.cpp | 11 +++- .../src/DynamicalSystemFactory.cpp | 34 ++++++++----- source/dynamical_systems/src/Ring.cpp | 13 +++-- .../test/tests/test_circular.cpp | 4 +- .../test/tests/test_factory.cpp | 8 +-- .../test/tests/test_point_attractor.cpp | 25 ++++----- .../test/tests/test_ring.cpp | 5 +- 36 files changed, 258 insertions(+), 199 deletions(-) rename python/source/controllers/{bind_type.cpp => bind_controller_type.cpp} (93%) delete mode 100644 python/source/dynamical_systems/bind_cartesian.cpp create mode 100644 python/source/dynamical_systems/bind_cartesian_ds.cpp create mode 100644 python/source/dynamical_systems/bind_ds_type.cpp delete mode 100644 python/source/dynamical_systems/bind_joint.cpp create mode 100644 python/source/dynamical_systems/bind_joint_ds.cpp delete mode 100644 python/source/dynamical_systems/bind_type.cpp create mode 100644 source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp create mode 100644 source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 51e72ea82..0ffc2a240 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ Release Versions: - Fix if else conditions in setup.py to correctly install modules (#285) - Refactor state type (#277, #278, #280, #284) +- Refactor dynamical systems factory (#288) ## 5.2.0 diff --git a/VERSION b/VERSION index ce7f2b425..c0baecbaa 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.2 +5.2.3 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 9ceb7f3b0..bd5058c48 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.2.2 +PROJECT_NUMBER = 5.2.3 # 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 71500cb7a..53f249ca4 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.2.2) +project(clproto VERSION 5.2.3) set(CMAKE_CXX_STANDARD 17) diff --git a/python/Dockerfile.python b/python/Dockerfile.python index 14f236063..787571ec1 100644 --- a/python/Dockerfile.python +++ b/python/Dockerfile.python @@ -6,6 +6,7 @@ RUN git clone --depth 1 --branch $BRANCH https://github.com/epfl-lasa/control-li RUN bash control-libraries/source/install.sh --auto RUN bash control-libraries/protocol/install.sh --auto +RUN rm -rf control-libraries/python/include control-libraries/python/source COPY include control-libraries/python/include COPY source control-libraries/python/source COPY pyproject.toml setup.py control-libraries/python/ diff --git a/python/include/controllers_bindings.h b/python/include/controllers_bindings.h index bdce1c281..954b381b9 100644 --- a/python/include/controllers_bindings.h +++ b/python/include/controllers_bindings.h @@ -11,7 +11,7 @@ namespace py = pybind11; using namespace pybind11::literals; using namespace controllers; -void bind_type(py::module_& m); +void bind_controller_type(py::module_& m); void bind_computational_space(py::module_& m); void bind_cartesian_controllers(py::module_& m); void bind_joint_controllers(py::module_& m); diff --git a/python/include/dynamical_systems_bindings.h b/python/include/dynamical_systems_bindings.h index 5d5e59517..2c30985c3 100644 --- a/python/include/dynamical_systems_bindings.h +++ b/python/include/dynamical_systems_bindings.h @@ -11,6 +11,6 @@ namespace py = pybind11; using namespace pybind11::literals; using namespace dynamical_systems; -void bind_type(py::module_& m); -void bind_cartesian(py::module_& m); -void bind_joint(py::module_& m); +void bind_ds_type(py::module_& m); +void bind_cartesian_ds(py::module_& m); +void bind_joint_ds(py::module_& m); diff --git a/python/setup.py b/python/setup.py index 627598ca4..9aa7a71cd 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.2.2" +__version__ = "5.2.3" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/python/source/controllers/bind_type.cpp b/python/source/controllers/bind_controller_type.cpp similarity index 93% rename from python/source/controllers/bind_type.cpp rename to python/source/controllers/bind_controller_type.cpp index 46540dddc..e671bff17 100644 --- a/python/source/controllers/bind_type.cpp +++ b/python/source/controllers/bind_controller_type.cpp @@ -2,7 +2,7 @@ #include -void bind_type(py::module_& m) { +void bind_controller_type(py::module_& m) { py::enum_(m, "CONTROLLER_TYPE") .value("NONE", CONTROLLER_TYPE::NONE) .value("IMPEDANCE", CONTROLLER_TYPE::IMPEDANCE) diff --git a/python/source/controllers/controllers_bindings.cpp b/python/source/controllers/controllers_bindings.cpp index 40841efe1..5b42b9717 100644 --- a/python/source/controllers/controllers_bindings.cpp +++ b/python/source/controllers/controllers_bindings.cpp @@ -14,7 +14,7 @@ PYBIND11_MODULE(controllers, m) { py::module_::import("state_representation"); - bind_type(m); + bind_controller_type(m); bind_computational_space(m); bind_cartesian_controllers(m); bind_joint_controllers(m); diff --git a/python/source/dynamical_systems/bind_cartesian.cpp b/python/source/dynamical_systems/bind_cartesian.cpp deleted file mode 100644 index 8c0c1d300..000000000 --- a/python/source/dynamical_systems/bind_cartesian.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "dynamical_systems_bindings.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "py_dynamical_system.h" - -using namespace state_representation; - -void cartesian(py::module_& m) { - py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap"); - py::class_, std::shared_ptr>, PyDynamicalSystem> c(m, "ICartesianDS", parameter_map); - - c.def(py::init<>()); - - c.def("is_compatible", &IDynamicalSystem::is_compatible); - c.def("evaluate", &IDynamicalSystem::evaluate); - c.def("get_base_frame", &IDynamicalSystem::get_base_frame); - c.def("set_base_frame", &IDynamicalSystem::set_base_frame); -} - -void bind_cartesian(py::module_& m) { - cartesian(m); - py::class_, IDynamicalSystem>(m, "CartesianCircularDS").def(py::init<>()); - py::class_, std::shared_ptr>, IDynamicalSystem>(m, "CartesianDefaultDS").def(py::init<>()); - py::class_, std::shared_ptr>, IDynamicalSystem>(m, "CartesianPointAttractorDS").def(py::init<>()); - py::class_, IDynamicalSystem>(m, "CartesianRingDS").def(py::init<>()); - - m.def("create_cartesian_ds", [](DynamicalSystemFactory::DYNAMICAL_SYSTEM type) -> py::object { - switch (type) { - case DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR: - return py::cast(Circular()); - break; - case DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR: - return py::cast(PointAttractor()); - break; - case DynamicalSystemFactory::DYNAMICAL_SYSTEM::RING: - return py::cast(Ring()); - break; - default: - case DynamicalSystemFactory::DYNAMICAL_SYSTEM::NONE: - return py::cast(DefaultDynamicalSystem()); - break; - } - }); -} diff --git a/python/source/dynamical_systems/bind_cartesian_ds.cpp b/python/source/dynamical_systems/bind_cartesian_ds.cpp new file mode 100644 index 000000000..6358292ab --- /dev/null +++ b/python/source/dynamical_systems/bind_cartesian_ds.cpp @@ -0,0 +1,32 @@ +#include "dynamical_systems_bindings.h" + +#include +#include +#include + +#include "py_dynamical_system.h" + +using namespace state_representation; +using namespace py_parameter; + +void cartesian(py::module_& m) { + py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap"); + py::class_, std::shared_ptr>, PyDynamicalSystem> c(m, "ICartesianDS", parameter_map); + + c.def("is_compatible", &IDynamicalSystem::is_compatible); + c.def("evaluate", &IDynamicalSystem::evaluate); + c.def("get_base_frame", &IDynamicalSystem::get_base_frame); + c.def("set_base_frame", &IDynamicalSystem::set_base_frame); +} + +void bind_cartesian_ds(py::module_& m) { + cartesian(m); + + m.def("create_cartesian_ds", [](DYNAMICAL_SYSTEM_TYPE type, const std::list& parameters) -> py::object { + return py::cast(CartesianDynamicalSystemFactory::create_dynamical_system(type, container_to_interface_ptr_list(parameters))); + }, "Create a dynamical system of the desired type with initial parameters.", "type"_a, "parameters"_a); + + m.def("create_cartesian_ds", [](DYNAMICAL_SYSTEM_TYPE type) -> py::object { + return py::cast(CartesianDynamicalSystemFactory::create_dynamical_system(type, std::list>())); + }, "Create a dynamical system of the desired type.", "type"_a); +} diff --git a/python/source/dynamical_systems/bind_ds_type.cpp b/python/source/dynamical_systems/bind_ds_type.cpp new file mode 100644 index 000000000..1416ec256 --- /dev/null +++ b/python/source/dynamical_systems/bind_ds_type.cpp @@ -0,0 +1,14 @@ +#include "dynamical_systems_bindings.h" + +#include + +using namespace state_representation; + +void bind_ds_type(py::module_& m) { + py::enum_(m, "DYNAMICAL_SYSTEM_TYPE") + .value("NONE", DYNAMICAL_SYSTEM_TYPE::NONE) + .value("CIRCULAR", DYNAMICAL_SYSTEM_TYPE::CIRCULAR) + .value("POINT_ATTRACTOR", DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR) + .value("RING", DYNAMICAL_SYSTEM_TYPE::RING) + .export_values(); +} diff --git a/python/source/dynamical_systems/bind_joint.cpp b/python/source/dynamical_systems/bind_joint.cpp deleted file mode 100644 index 5283235b1..000000000 --- a/python/source/dynamical_systems/bind_joint.cpp +++ /dev/null @@ -1,41 +0,0 @@ -#include "dynamical_systems_bindings.h" - -#include -#include -#include -#include -#include - -#include "py_dynamical_system.h" - -using namespace state_representation; - -void joint(py::module_& m) { - py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap"); - py::class_, std::shared_ptr>, PyDynamicalSystem> c(m, "IJointDS", parameter_map); - - c.def(py::init<>()); - - c.def("is_compatible", &IDynamicalSystem::is_compatible); - c.def("evaluate", &IDynamicalSystem::evaluate); - c.def("get_base_frame", &IDynamicalSystem::get_base_frame); - c.def("set_base_frame", &IDynamicalSystem::set_base_frame); -} - -void bind_joint(py::module_& m) { - joint(m); - py::class_, std::shared_ptr>, IDynamicalSystem>(m, "JointDefaultDS").def(py::init<>()); - py::class_, std::shared_ptr>, IDynamicalSystem>(m, "JointPointAttractorDS").def(py::init<>()); - - m.def("create_joint_ds", [](DynamicalSystemFactory::DYNAMICAL_SYSTEM type) -> py::object { - switch (type) { - case DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR: - return py::cast(PointAttractor()); - break; - default: - case DynamicalSystemFactory::DYNAMICAL_SYSTEM::NONE: - return py::cast(DefaultDynamicalSystem()); - break; - } - }); -} diff --git a/python/source/dynamical_systems/bind_joint_ds.cpp b/python/source/dynamical_systems/bind_joint_ds.cpp new file mode 100644 index 000000000..ff6a3a0c6 --- /dev/null +++ b/python/source/dynamical_systems/bind_joint_ds.cpp @@ -0,0 +1,33 @@ +#include "dynamical_systems_bindings.h" + +#include +#include +#include + +#include "py_dynamical_system.h" + +using namespace state_representation; +using namespace py_parameter; + +void joint(py::module_& m) { + py::object parameter_map = py::module_::import("state_representation").attr("ParameterMap"); + py::class_, std::shared_ptr>, PyDynamicalSystem> c(m, "IJointDS", parameter_map); + + c.def("is_compatible", &IDynamicalSystem::is_compatible); + c.def("evaluate", &IDynamicalSystem::evaluate); + c.def("get_base_frame", &IDynamicalSystem::get_base_frame); + c.def("set_base_frame", &IDynamicalSystem::set_base_frame); +} + +void bind_joint_ds(py::module_& m) { + joint(m); + + m.def("create_joint_ds", [](DYNAMICAL_SYSTEM_TYPE type, const std::list& parameters) -> py::object { + return py::cast(JointDynamicalSystemFactory::create_dynamical_system(type, container_to_interface_ptr_list(parameters))); + }, "Create a dynamical system of the desired type with initial parameters.", "type"_a, "parameters"_a); + + + m.def("create_joint_ds", [](DYNAMICAL_SYSTEM_TYPE type) -> py::object { + return py::cast(JointDynamicalSystemFactory::create_dynamical_system(type, std::list>())); + }, "Create a dynamical system of the desired type.", "type"_a); +} diff --git a/python/source/dynamical_systems/bind_type.cpp b/python/source/dynamical_systems/bind_type.cpp deleted file mode 100644 index cfe122fe5..000000000 --- a/python/source/dynamical_systems/bind_type.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "dynamical_systems_bindings.h" - -#include -#include - -using namespace state_representation; - -void bind_type(py::module_& m) { - py::enum_::DYNAMICAL_SYSTEM>(m, "DYNAMICAL_SYSTEM") - .value("NONE", DynamicalSystemFactory::DYNAMICAL_SYSTEM::NONE) - .value("CIRCULAR", DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR) - .value("POINT_ATTRACTOR", DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR) - .value("RING", DynamicalSystemFactory::DYNAMICAL_SYSTEM::RING) - .export_values(); -} diff --git a/python/source/dynamical_systems/dynamical_systems_bindings.cpp b/python/source/dynamical_systems/dynamical_systems_bindings.cpp index 5b852bba6..c590c2aa0 100644 --- a/python/source/dynamical_systems/dynamical_systems_bindings.cpp +++ b/python/source/dynamical_systems/dynamical_systems_bindings.cpp @@ -14,7 +14,7 @@ PYBIND11_MODULE(dynamical_systems, m) { py::module_::import("state_representation"); - bind_type(m); - bind_cartesian(m); - bind_joint(m); + bind_ds_type(m); + bind_cartesian_ds(m); + bind_joint_ds(m); } diff --git a/python/test/dynamical_systems/test_cartesian_point_attractor.py b/python/test/dynamical_systems/test_cartesian_point_attractor.py index 96cd32e83..ec6e7681a 100644 --- a/python/test/dynamical_systems/test_cartesian_point_attractor.py +++ b/python/test/dynamical_systems/test_cartesian_point_attractor.py @@ -2,7 +2,7 @@ import state_representation as sr import unittest from datetime import timedelta -from dynamical_systems import CartesianPointAttractorDS +from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE class TestCartesianPointAttractor(unittest.TestCase): @@ -14,7 +14,7 @@ def assert_np_array_equal(self, a: np.array, b: np.array, places=3): self.fail(f'{e}') def test_empty_constructor(self): - ds = CartesianPointAttractorDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) attractor = sr.CartesianState.Identity("CAttractor", "A") self.assertTrue(ds.get_parameter_value("attractor").is_empty()) @@ -27,7 +27,7 @@ def test_empty_constructor(self): self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4)) def test_is_compatible(self): - ds = CartesianPointAttractorDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) state1 = sr.CartesianState.Identity("B", "A") state2 = sr.CartesianState.Identity("D", "C") state3 = sr.CartesianState.Identity("C", "A") @@ -52,7 +52,7 @@ def test_is_compatible(self): self.assertTrue(ds.is_compatible(state4)) def test_pose(self): - ds = CartesianPointAttractorDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) target = sr.CartesianPose.Random("B") ds.set_parameter(sr.Parameter("attractor", target, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)) @@ -69,7 +69,7 @@ def test_stacked_moving_reference_frames(self): BinA = sr.CartesianState.Random("B", "A") CinA = sr.CartesianState(sr.CartesianPose.Random("C", "A")) - ds = CartesianPointAttractorDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) ds.set_parameter(sr.Parameter("attractor", BinA, sr.ParameterType.STATE, sr.StateType.CARTESIAN_STATE)) twist = sr.CartesianTwist(ds.evaluate(CinA)) diff --git a/python/test/dynamical_systems/test_circular.py b/python/test/dynamical_systems/test_circular.py index 25a65a482..c1c0fa11f 100644 --- a/python/test/dynamical_systems/test_circular.py +++ b/python/test/dynamical_systems/test_circular.py @@ -2,7 +2,7 @@ import state_representation as sr import unittest from datetime import timedelta -from dynamical_systems import CartesianCircularDS +from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE class TestCircular(unittest.TestCase): @@ -25,7 +25,7 @@ def assert_np_array_equal(self, a: np.array, b: np.array, places=3): self.fail(f'{e}') def test_empty_constructor(self): - ds = CartesianCircularDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR) self.assertTrue(ds.get_parameter_value("limit_cycle").get_center_state().is_empty()) self.assertTrue(ds.get_base_frame().is_empty()) @@ -38,7 +38,7 @@ def test_empty_constructor(self): self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4)) def test_is_compatible(self): - ds = CartesianCircularDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR) state1 = sr.CartesianState.Identity("world", "A") state2 = sr.CartesianState("D", "C") state3 = sr.CartesianState("C", "A") @@ -62,7 +62,7 @@ def test_is_compatible(self): self.assertTrue(ds.is_compatible(state4)) def test_points_on_radius_random_center(self): - ds = CartesianCircularDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR) self.limit_cycle.set_center_position(np.random.rand(3, 1)) ds.set_parameter(sr.Parameter("limit_cycle", self.limit_cycle, sr.ParameterType.STATE, sr.StateType.GEOMETRY_ELLIPSOID)) diff --git a/python/test/dynamical_systems/test_ds.py b/python/test/dynamical_systems/test_ds.py index c8e4d3af6..7a34e268e 100644 --- a/python/test/dynamical_systems/test_ds.py +++ b/python/test/dynamical_systems/test_ds.py @@ -1,6 +1,6 @@ import state_representation as sr import unittest -from dynamical_systems import ICartesianDS, DYNAMICAL_SYSTEM, create_cartesian_ds, create_joint_ds +from dynamical_systems import ICartesianDS, DYNAMICAL_SYSTEM_TYPE, create_cartesian_ds, create_joint_ds DS_METHOD_EXPECTS = [ 'is_compatible', @@ -24,7 +24,7 @@ def test_callable_methods(self): self.assertIn(expected, methods) def test_cartesian_factory(self): - cart_ds = create_cartesian_ds(DYNAMICAL_SYSTEM.NONE) + cart_ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.NONE) cart_ds.set_base_frame(sr.CartesianState.Identity("A")) cart_ds.evaluate(sr.CartesianState.Identity("C", "A")) self.assertTrue(cart_ds.evaluate(sr.CartesianState.Identity("C", "A")).is_empty()) @@ -35,24 +35,24 @@ def test_cartesian_factory(self): cart_ds.set_parameters(param_list) def test_cartesian_ds(self): - ds = create_cartesian_ds(DYNAMICAL_SYSTEM.POINT_ATTRACTOR) + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) expected_params = ["attractor", "gain"] for param in list(ds.get_parameters().keys()): self.assertIn(param, expected_params) - ds = create_cartesian_ds(DYNAMICAL_SYSTEM.RING) + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING) expected_params = ["center", "rotation_offset", "radius", "width", "speed", "field_strength", "normal_gain", "angular_gain"] for param in list(ds.get_parameters().keys()): self.assertIn(param, expected_params) - ds = create_cartesian_ds(DYNAMICAL_SYSTEM.CIRCULAR) + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.CIRCULAR) expected_params = ["limit_cycle", "planar_gain", "normal_gain", "circular_velocity"] for param in list(ds.get_parameters().keys()): self.assertIn(param, expected_params) def test_joint_factory(self): - joint_ds = create_joint_ds(DYNAMICAL_SYSTEM.NONE) + joint_ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.NONE) joint_ds.set_base_frame(sr.JointState.Zero("robot", 3)) joint_ds.evaluate(sr.JointState.Random("robot", 3)) self.assertTrue(joint_ds.evaluate(sr.JointState.Random("robot", 3)).is_empty()) @@ -63,7 +63,7 @@ def test_joint_factory(self): joint_ds.set_parameters(param_list) def test_joint_ds(self): - ds = create_joint_ds(DYNAMICAL_SYSTEM.POINT_ATTRACTOR) + ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) expected_params = ["attractor", "gain"] for param in list(ds.get_parameters().keys()): self.assertIn(param, expected_params) diff --git a/python/test/dynamical_systems/test_joint_point_attractor.py b/python/test/dynamical_systems/test_joint_point_attractor.py index 7b6127a00..23ff7c959 100644 --- a/python/test/dynamical_systems/test_joint_point_attractor.py +++ b/python/test/dynamical_systems/test_joint_point_attractor.py @@ -1,12 +1,12 @@ import state_representation as sr import unittest from datetime import timedelta -from dynamical_systems import JointPointAttractorDS +from dynamical_systems import create_joint_ds, DYNAMICAL_SYSTEM_TYPE class TestJointPointAttractor(unittest.TestCase): - def test_empty_constructor(self): - ds = JointPointAttractorDS() + def test_constructor(self): + ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) attractor = sr.JointState.Zero("robot", 3) self.assertTrue(ds.get_parameter_value("attractor").is_empty()) @@ -14,8 +14,12 @@ def test_empty_constructor(self): ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)) self.assertFalse(ds.get_parameter_value("attractor").is_empty()) + parameters = [sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)] + ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR, parameters) + self.assertFalse(ds.get_parameter_value("attractor").is_empty()) + def test_convergence(self): - ds = JointPointAttractorDS() + ds = create_joint_ds(DYNAMICAL_SYSTEM_TYPE.POINT_ATTRACTOR) attractor = sr.JointPositions.Random("robot", 3) ds.set_parameter(sr.Parameter("attractor", attractor, sr.ParameterType.STATE, sr.StateType.JOINT_STATE)) diff --git a/python/test/dynamical_systems/test_ring.py b/python/test/dynamical_systems/test_ring.py index 3a25738ca..52d5f3e2b 100644 --- a/python/test/dynamical_systems/test_ring.py +++ b/python/test/dynamical_systems/test_ring.py @@ -3,7 +3,7 @@ import state_representation as sr import unittest from datetime import timedelta -from dynamical_systems import CartesianRingDS +from dynamical_systems import create_cartesian_ds, DYNAMICAL_SYSTEM_TYPE class TestRing(unittest.TestCase): @@ -25,7 +25,7 @@ def assert_np_array_equal(self, a: np.array, b: np.array, places=3): self.fail(f'{e}') def test_empty_constructor(self): - ds = CartesianRingDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING) center = sr.CartesianPose.Identity("CAttractor", "A") self.assertTrue(ds.get_parameter_value("center").is_empty()) @@ -38,7 +38,7 @@ def test_empty_constructor(self): self.assert_np_array_equal(ds.get_base_frame().get_transformation_matrix(), np.eye(4)) def test_is_compatible(self): - ds = CartesianRingDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING) center = sr.CartesianPose.Identity("CAttractor", "A") state1 = sr.CartesianState.Identity("B", "A") state2 = sr.CartesianState.Identity("D", "C") @@ -63,7 +63,7 @@ def test_is_compatible(self): self.assertTrue(ds.is_compatible(state4)) def test_points_on_radius(self): - ds = CartesianRingDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING) ds.set_parameter(sr.Parameter("center", self.center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)) ds.set_parameter(sr.Parameter("radius", self.radius, sr.ParameterType.DOUBLE)) ds.set_parameter(sr.Parameter("width", self.width, sr.ParameterType.DOUBLE)) @@ -99,7 +99,7 @@ def test_points_on_radius(self): def test_convergence_on_radius_random_center(self): center = sr.CartesianPose.Random("A") - ds = CartesianRingDS() + ds = create_cartesian_ds(DYNAMICAL_SYSTEM_TYPE.RING) ds.set_parameter(sr.Parameter("center", center, sr.ParameterType.STATE, sr.StateType.CARTESIAN_POSE)) ds.set_parameter(sr.Parameter("radius", self.radius, sr.ParameterType.DOUBLE)) diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index e52ded235..d67adefae 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.2) +project(control_libraries VERSION 5.2.3) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/dynamical_systems/include/dynamical_systems/Circular.hpp b/source/dynamical_systems/include/dynamical_systems/Circular.hpp index 235c88e42..cda113200 100644 --- a/source/dynamical_systems/include/dynamical_systems/Circular.hpp +++ b/source/dynamical_systems/include/dynamical_systems/Circular.hpp @@ -17,6 +17,12 @@ class Circular : public IDynamicalSystem { */ Circular(); + /** + * @brief Constructor from an initial parameter list + * @param parameters A parameter list containing initial parameters + */ + explicit Circular(const std::list>& parameters); + /** * @copydoc IDynamicalSystem::set_parameter */ diff --git a/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp index 77db8ce0a..c3ea3652b 100644 --- a/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp +++ b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemFactory.hpp @@ -1,7 +1,11 @@ #pragma once +#include "dynamical_systems/DynamicalSystemType.hpp" #include "dynamical_systems/IDynamicalSystem.hpp" +#include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/space/joint/JointState.hpp" + namespace dynamical_systems { /** @@ -12,18 +16,31 @@ namespace dynamical_systems { template class DynamicalSystemFactory { public: - /** - * @brief Enumeration of the implemented dynamical systems - */ - enum class DYNAMICAL_SYSTEM { - NONE, CIRCULAR, POINT_ATTRACTOR, RING - }; - /** * @brief Create a dynamical system of the desired type. - * @param type Underlying state type of the dynamical system + * @param type The type of dynamical system * @return The shared pointer to the dynamical system */ - static std::shared_ptr> create_dynamical_system(DYNAMICAL_SYSTEM type); + static std::shared_ptr> create_dynamical_system(DYNAMICAL_SYSTEM_TYPE type); + + /** + * @brief Create a dynamical system of the desired type with initial parameters. + * @param type The type of dynamical system + * @param parameters A list of parameters to set on the dynamical system + * @return The shared pointer to the dynamical system + */ + static std::shared_ptr> create_dynamical_system( + DYNAMICAL_SYSTEM_TYPE type, const std::list>& parameters + ); }; + +template +std::shared_ptr> DynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE type) { + std::list> parameters; + return DynamicalSystemFactory::create_dynamical_system(type, parameters); +} + +typedef DynamicalSystemFactory CartesianDynamicalSystemFactory; +typedef DynamicalSystemFactory JointDynamicalSystemFactory; + }// namespace dynamical_systems diff --git a/source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp new file mode 100644 index 000000000..bdc367b15 --- /dev/null +++ b/source/dynamical_systems/include/dynamical_systems/DynamicalSystemType.hpp @@ -0,0 +1,15 @@ +#pragma once + +namespace dynamical_systems { + +/** + * @brief Enumeration of the implemented dynamical systems + */ +enum class DYNAMICAL_SYSTEM_TYPE { + NONE, + CIRCULAR, + POINT_ATTRACTOR, + RING +}; + +} \ No newline at end of file diff --git a/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp b/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp index 7e83b8fcf..29c68438f 100644 --- a/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp +++ b/source/dynamical_systems/include/dynamical_systems/PointAttractor.hpp @@ -18,6 +18,12 @@ class PointAttractor : public IDynamicalSystem { */ PointAttractor(); + /** + * @brief Constructor from an initial parameter list + * @param parameters A parameter list containing initial attractor and gain values + */ + explicit PointAttractor(const std::list>& parameters); + /** * @copydoc IDynamicalSystem::set_base_frame */ @@ -54,4 +60,12 @@ class PointAttractor : public IDynamicalSystem { std::shared_ptr> attractor_; ///< attractor of the dynamical system in the space std::shared_ptr> gain_; ///< gain associate to the system }; + +template +PointAttractor::PointAttractor( + const std::list>& parameters +) : PointAttractor() { + this->set_parameters(parameters); +} + }// namespace dynamical_systems diff --git a/source/dynamical_systems/include/dynamical_systems/Ring.hpp b/source/dynamical_systems/include/dynamical_systems/Ring.hpp index b62e37ace..6297bf7b9 100644 --- a/source/dynamical_systems/include/dynamical_systems/Ring.hpp +++ b/source/dynamical_systems/include/dynamical_systems/Ring.hpp @@ -20,6 +20,12 @@ class Ring : public IDynamicalSystem { */ Ring(); + /** + * @brief Constructor from an initial parameter list + * @param parameters A parameter list containing initial parameters + */ + explicit Ring(const std::list>& parameters); + /** * @copydoc IDynamicalSystem::set_parameter */ diff --git a/source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp b/source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp new file mode 100644 index 000000000..239bc1a72 --- /dev/null +++ b/source/dynamical_systems/include/dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace dynamical_systems::exceptions { +class InvalidDynamicalSystemException : public std::logic_error { +public: + explicit InvalidDynamicalSystemException(const std::string& msg) : std::logic_error(msg) {}; +}; +} diff --git a/source/dynamical_systems/src/Circular.cpp b/source/dynamical_systems/src/Circular.cpp index 2dd6e63e0..ab87903ea 100644 --- a/source/dynamical_systems/src/Circular.cpp +++ b/source/dynamical_systems/src/Circular.cpp @@ -21,6 +21,11 @@ Circular::Circular() : this->parameters_.insert(std::make_pair("circular_velocity", this->circular_velocity_)); } +Circular::Circular(const std::list>& parameters) : + Circular() { + this->set_parameters(parameters); +} + void Circular::set_limit_cycle(Ellipsoid& limit_cycle) { const auto& center = limit_cycle.get_center_state(); if (center.is_empty()) { @@ -36,7 +41,8 @@ void Circular::set_limit_cycle(Ellipsoid& limit_cycle) { throw state_representation::exceptions::IncompatibleReferenceFramesException( "The reference frame of the center " + center.get_name() + " in frame " + center.get_reference_frame() + " is incompatible with the base frame of the dynamical system " + this->get_base_frame().get_name() - + " in frame " + this->get_base_frame().get_reference_frame() + "."); + + " in frame " + this->get_base_frame().get_reference_frame() + "." + ); } limit_cycle.set_center_state(this->get_base_frame().inverse() * center); } @@ -71,7 +77,8 @@ void Circular::validate_and_set_parameter(const std::shared_ptrcircular_velocity_->set_value(std::static_pointer_cast>(parameter)->get_value()); } else { throw state_representation::exceptions::InvalidParameterException( - "No parameter with name '" + parameter->get_name() + "' found"); + "No parameter with name '" + parameter->get_name() + "' found" + ); } } diff --git a/source/dynamical_systems/src/DynamicalSystemFactory.cpp b/source/dynamical_systems/src/DynamicalSystemFactory.cpp index b5f87eb23..b52703967 100644 --- a/source/dynamical_systems/src/DynamicalSystemFactory.cpp +++ b/source/dynamical_systems/src/DynamicalSystemFactory.cpp @@ -4,6 +4,7 @@ #include "dynamical_systems/DefaultDynamicalSystem.hpp" #include "dynamical_systems/PointAttractor.hpp" #include "dynamical_systems/Ring.hpp" +#include "dynamical_systems/exceptions/InvalidDynamicalSystemException.hpp" #include "state_representation/space/joint/JointState.hpp" using namespace state_representation; @@ -11,29 +12,34 @@ using namespace state_representation; namespace dynamical_systems { template<> -std::shared_ptr> -DynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM type) { +std::shared_ptr> DynamicalSystemFactory::create_dynamical_system( + DYNAMICAL_SYSTEM_TYPE type, const std::list>& parameters +) { switch (type) { - case DYNAMICAL_SYSTEM::POINT_ATTRACTOR: - return std::make_shared>(); - case DYNAMICAL_SYSTEM::CIRCULAR: - return std::make_shared(); - case DYNAMICAL_SYSTEM::RING: - return std::make_shared(); + case DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR: + return std::make_shared>(parameters); + case DYNAMICAL_SYSTEM_TYPE::CIRCULAR: + return std::make_shared(parameters); + case DYNAMICAL_SYSTEM_TYPE::RING: + return std::make_shared(parameters); default: - case DYNAMICAL_SYSTEM::NONE: + case DYNAMICAL_SYSTEM_TYPE::NONE: return std::make_shared>(); } } template<> -std::shared_ptr> -DynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM type) { +std::shared_ptr> DynamicalSystemFactory::create_dynamical_system( + DYNAMICAL_SYSTEM_TYPE type, const std::list>& parameters +) { switch (type) { - case DYNAMICAL_SYSTEM::POINT_ATTRACTOR: - return std::make_shared>(); + case DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR: + return std::make_shared>(parameters); + case DYNAMICAL_SYSTEM_TYPE::CIRCULAR: + case DYNAMICAL_SYSTEM_TYPE::RING: + throw exceptions::InvalidDynamicalSystemException("This JointState DS is not valid"); default: - case DYNAMICAL_SYSTEM::NONE: + case DYNAMICAL_SYSTEM_TYPE::NONE: return std::make_shared>(); } } diff --git a/source/dynamical_systems/src/Ring.cpp b/source/dynamical_systems/src/Ring.cpp index 6faa00df6..411d4b5c5 100644 --- a/source/dynamical_systems/src/Ring.cpp +++ b/source/dynamical_systems/src/Ring.cpp @@ -32,6 +32,10 @@ Ring::Ring() : this->parameters_.insert(std::make_pair("angular_gain", this->angular_gain_)); } +Ring::Ring(const std::list>& parameters) : Ring() { + this->set_parameters(parameters); +} + void Ring::set_center(const CartesianPose& center) { if (center.is_empty()) { throw state_representation::exceptions::EmptyStateException(center.get_name() + " state is empty"); @@ -46,7 +50,8 @@ void Ring::set_center(const CartesianPose& center) { throw state_representation::exceptions::IncompatibleReferenceFramesException( "The reference frame of the center " + center.get_name() + " in frame " + center.get_reference_frame() + " is incompatible with the base frame of the dynamical system " + this->get_base_frame().get_name() - + " in frame " + this->get_base_frame().get_reference_frame() + "."); + + " in frame " + this->get_base_frame().get_reference_frame() + "." + ); } this->center_->set_value(this->get_base_frame().inverse() * center); } else { @@ -104,7 +109,8 @@ void Ring::validate_and_set_parameter(const std::shared_ptr& this->angular_gain_->set_value(std::static_pointer_cast>(parameter)->get_value()); } else { throw state_representation::exceptions::InvalidParameterException( - "No parameter with name '" + parameter->get_name() + "' found"); + "No parameter with name '" + parameter->get_name() + "' found" + ); } } @@ -212,7 +218,8 @@ CartesianState Ring::compute_dynamics(const CartesianState& state) const { twist.set_linear_velocity(this->calculate_local_linear_velocity(pose, local_field_strength)); twist.set_angular_velocity( this->calculate_local_angular_velocity( - pose, twist.get_linear_velocity(), local_field_strength)); + pose, twist.get_linear_velocity(), local_field_strength + )); // transform the twist back to the base reference frame return CartesianState(this->center_->get_value()) * twist; diff --git a/source/dynamical_systems/test/tests/test_circular.cpp b/source/dynamical_systems/test/tests/test_circular.cpp index efff74860..6f44e8f21 100644 --- a/source/dynamical_systems/test/tests/test_circular.cpp +++ b/source/dynamical_systems/test/tests/test_circular.cpp @@ -16,9 +16,7 @@ using namespace std::literals::chrono_literals; class CircularDSTest : public testing::Test { protected: void SetUp() override { - ds = DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR - ); + ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR); current_pose = CartesianPose("A", 10 * Eigen::Vector3d::Random()); center = CartesianPose::Identity("B"); limit_cycle.set_center_pose(center); diff --git a/source/dynamical_systems/test/tests/test_factory.cpp b/source/dynamical_systems/test/tests/test_factory.cpp index c2a59882c..4f80e7994 100644 --- a/source/dynamical_systems/test/tests/test_factory.cpp +++ b/source/dynamical_systems/test/tests/test_factory.cpp @@ -13,8 +13,8 @@ TEST(DSFactoryTest, CreateDS) { std::list> param_list; param_list.emplace_back(make_shared_parameter("test", 1.0)); - auto cart_ds = dynamical_systems::DynamicalSystemFactory::create_dynamical_system( - dynamical_systems::DynamicalSystemFactory::DYNAMICAL_SYSTEM::NONE + auto cart_ds = dynamical_systems::CartesianDynamicalSystemFactory::create_dynamical_system( + dynamical_systems::DYNAMICAL_SYSTEM_TYPE::NONE ); cart_ds->set_base_frame(CartesianState::Identity("A")); ASSERT_NO_THROW(auto res = cart_ds->evaluate(CartesianState::Identity("C", "A"))); @@ -22,8 +22,8 @@ TEST(DSFactoryTest, CreateDS) { EXPECT_EQ(cart_ds->get_parameters().size(), 0); EXPECT_THROW(cart_ds->set_parameters(param_list), exceptions::InvalidParameterException); - auto joint_ds = dynamical_systems::DynamicalSystemFactory::create_dynamical_system( - dynamical_systems::DynamicalSystemFactory::DYNAMICAL_SYSTEM::NONE + auto joint_ds = dynamical_systems::JointDynamicalSystemFactory::create_dynamical_system( + dynamical_systems::DYNAMICAL_SYSTEM_TYPE::NONE ); ASSERT_NO_THROW(auto res = joint_ds->evaluate(JointState::Random("robot", 3))); EXPECT_TRUE(joint_ds->evaluate(JointState::Random("robot", 3)).is_empty()); diff --git a/source/dynamical_systems/test/tests/test_point_attractor.cpp b/source/dynamical_systems/test/tests/test_point_attractor.cpp index 9bcb9e58b..f36286077 100644 --- a/source/dynamical_systems/test/tests/test_point_attractor.cpp +++ b/source/dynamical_systems/test/tests/test_point_attractor.cpp @@ -13,7 +13,6 @@ #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" #include "state_representation/exceptions/IncompatibleStatesException.hpp" - using namespace state_representation; using namespace dynamical_systems; using namespace std::literals::chrono_literals; @@ -23,9 +22,7 @@ class PointAttractorTest : public testing::Test { void SetUp() override { current_pose = CartesianPose("A", 10 * Eigen::Vector3d::Random()); target_pose = CartesianPose("B", 10 * Eigen::Vector3d::Random(), Eigen::Quaterniond::UnitRandom()); - ds = DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - ); + ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR); } void print_current_and_target_pose() { std::cout << current_pose << std::endl; @@ -270,10 +267,8 @@ TEST_F(PointAttractorTest, UpdateAttractorFrame) { EXPECT_NO_THROW(ds->evaluate(D)); } -TEST(JointPointAttractorTest, EmptyConstructor) { - auto ds = DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - ); +TEST(JointPointAttractorTest, Constructor) { + auto ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR); // construct empty cartesian state DS auto attractor = JointState::Zero("robot", 3); @@ -283,12 +278,16 @@ TEST(JointPointAttractorTest, EmptyConstructor) { ds->set_parameter_value("attractor", attractor); EXPECT_FALSE(ds->get_parameter_value("attractor").is_empty()); EXPECT_TRUE(ds->get_base_frame().is_empty()); + + std::list> parameters; + parameters.push_back(make_shared_parameter("attractor", JointState::Zero("robot", 3))); + ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR, parameters); + EXPECT_FALSE(ds->get_parameter_value("attractor").is_empty()); + EXPECT_TRUE(ds->get_base_frame().is_empty()); } TEST(JointPointAttractorTest, EmptyCompatible) { - auto ds = DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - ); + auto ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR); JointState state1 = JointState::Zero("robot", 3); JointState state2 = JointState("test", 3); JointState state3 = JointState("robot", 4); @@ -308,9 +307,7 @@ TEST(JointPointAttractorTest, EmptyCompatible) { } TEST(JointPointAttractorTest, Convergence) { - auto ds = DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - ); + auto ds = JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR); auto attractor = JointPositions::Random("robot", 3); auto current_state = JointPositions::Random("robot", 3); current_state.set_data(10 * current_state.data()); diff --git a/source/dynamical_systems/test/tests/test_ring.cpp b/source/dynamical_systems/test/tests/test_ring.cpp index 0e209315f..445731890 100644 --- a/source/dynamical_systems/test/tests/test_ring.cpp +++ b/source/dynamical_systems/test/tests/test_ring.cpp @@ -6,7 +6,6 @@ #include "state_representation/space/cartesian/CartesianState.hpp" #include "state_representation/space/cartesian/CartesianPose.hpp" -#include "state_representation/parameters/Parameter.hpp" #include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp" #include "state_representation/exceptions/EmptyStateException.hpp" @@ -17,9 +16,7 @@ using namespace std::literals::chrono_literals; class RingDSTest : public ::testing::Test { protected: RingDSTest() { - ds = DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::RING - ); + ds = CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::RING); center = CartesianPose::Identity("A"); current_pose = CartesianPose("B", radius * Eigen::Vector3d::Random()); } From 4b47e9a7638621a6195192b117163d38de89bc95 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 19 Apr 2022 11:15:38 +0200 Subject: [PATCH 08/17] Use pyquaternion in bindings (#283) * Install pyquaternion in setup.py * Bind overloaded set_orientation with pyquaternion * Update tests * Update CHANGELOG * 5.2.3 -> 5.2.4 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 5 +++- .../bind_cartesian_space.cpp | 24 ++++++++++++++++--- .../space/cartesian/test_cartesian_pose.py | 7 +++--- .../space/cartesian/test_cartesian_state.py | 20 +++++++++++----- source/CMakeLists.txt | 2 +- 9 files changed, 48 insertions(+), 17 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ffc2a240..063b08c53 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,7 @@ Release Versions: - Fix if else conditions in setup.py to correctly install modules (#285) - Refactor state type (#277, #278, #280, #284) - Refactor dynamical systems factory (#288) +- Use pyquaternion in bindings (#283) ## 5.2.0 diff --git a/VERSION b/VERSION index c0baecbaa..73ce95023 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.3 +5.2.4 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index bd5058c48..eaee01b49 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.2.3 +PROJECT_NUMBER = 5.2.4 # 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 53f249ca4..6965b01f0 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.2.3) +project(clproto VERSION 5.2.4) set(CMAKE_CXX_STANDARD 17) diff --git a/python/setup.py b/python/setup.py index 9aa7a71cd..3f81e4cd0 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.2.3" +__version__ = "5.2.4" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] @@ -131,6 +131,9 @@ ext_modules=ext_modules, test_suite='tests', python_requires='>=3', + install_requires=[ + 'pyquaternion>=0.9.9' + ], license='GNU GPL v3', zip_safe=False, ) diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp index d55a7af35..d50d1c3c8 100644 --- a/python/source/state_representation/bind_cartesian_space.cpp +++ b/python/source/state_representation/bind_cartesian_space.cpp @@ -64,7 +64,11 @@ void cartesian_state(py::module_& m) { c.def_static("Random", &CartesianState::Random, "Constructor for a random state", "name"_a, "reference"_a=std::string("world")); c.def("get_position", &CartesianState::get_position, "Getter of the position attribute"); - c.def("get_orientation", &CartesianState::get_orientation_coefficients, "Getter of the orientation attribute as quaternion coefficients (w, x, y, z)"); + c.def("get_orientation", [](const CartesianState &state) -> py::object { + py::object PyQuaternion = py::module_::import("pyquaternion").attr("Quaternion"); + return PyQuaternion(state.get_orientation_coefficients());; + }, "Getter of the orientation attribute as pyquaternion object"); + c.def("get_orientation_coefficients", &CartesianState::get_orientation_coefficients, "Getter of the orientation attribute as quaternion coefficients (w, x, y, z)"); c.def("get_pose", &CartesianState::get_pose, "Getter of a 7d pose vector from position and orientation coefficients"); c.def("get_transformation_matrix", &CartesianState::get_transformation_matrix, "Getter of a 4x4 transformation matrix of the pose"); @@ -83,8 +87,19 @@ void cartesian_state(py::module_& m) { c.def("set_position", py::overload_cast(&CartesianState::set_position), "Setter of the position"); c.def("set_position", py::overload_cast&>(&CartesianState::set_position), "Setter of the position from a list"); c.def("set_position", py::overload_cast(&CartesianState::set_position), "Setter of the position from three scalar coordinates", "x"_a, "y"_a, "z"_a); - c.def("set_orientation", py::overload_cast(&CartesianState::set_orientation), "Setter of the orientation from a 4d vector of quaternion coeffiecients (w, x, y, z)"); - c.def("set_orientation", py::overload_cast&>(&CartesianState::set_orientation), "Setter of the orientation from a 4d list of quaternion coeffiecients (w, x, y, z)"); + c.def("set_orientation", [](CartesianState &state, const py::object& quaternion) { + py::object PyQuaternion = py::module_::import("pyquaternion").attr("Quaternion"); + if (py::isinstance(quaternion)) { + state.set_orientation(py::cast>(quaternion)); + } else if (py::isinstance(quaternion)) { + state.set_orientation(py::cast(quaternion)); + } else if (py::isinstance(quaternion, PyQuaternion)) { + state.set_orientation(py::cast(quaternion.attr("elements"))); + } else { + throw std::invalid_argument("Type of input argument quaternion is not supported. " + "Supported types are: pyquaternion.Quaternion, numpy.array, list"); + } + }, "Setter of the orientation attribute from a pyquaternion.Quaternion, numpy.array(w, x, y, z), or list(w, x, y, z)"); c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz"); c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz"); @@ -252,6 +267,7 @@ void cartesian_twist(py::module_& m) { c.def(std::string("get_" + attr).c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class."); c.def(std::string("set_" + attr).c_str(), [](const CartesianTwist& twist) -> CartesianTwist { return twist; }, "Deleted method from parent class."); } + c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianTwist&) -> void {}, "Deleted method from parent class."); c.def(py::self += py::self); c.def(py::self + py::self); @@ -325,6 +341,7 @@ void cartesian_acceleration(py::module_& m) { c.def(std::string("get_" + attr).c_str(), [](const CartesianAcceleration&) -> void {}, "Deleted method from parent class."); c.def(std::string("set_" + attr).c_str(), [](const CartesianAcceleration& acceleration) -> CartesianAcceleration { return acceleration; }, "Deleted method from parent class."); } + c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianAcceleration&) -> void {}, "Deleted method from parent class."); c.def(py::self += py::self); c.def(py::self + py::self); @@ -396,6 +413,7 @@ void cartesian_wrench(py::module_& m) { c.def(std::string("get_" + attr).c_str(), [](const CartesianWrench&) -> void {}, "Deleted method from parent class."); c.def(std::string("set_" + attr).c_str(), [](const CartesianWrench& wrench) -> CartesianWrench { return wrench; }, "Deleted method from parent class."); } + c.def(std::string("get_orientation_coefficients").c_str(), [](const CartesianWrench&) -> void {}, "Deleted method from parent class."); c.def(py::self += py::self); c.def(py::self + py::self); diff --git a/python/test/state_representation/space/cartesian/test_cartesian_pose.py b/python/test/state_representation/space/cartesian/test_cartesian_pose.py index 4274a8980..4b5ecec0f 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_pose.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_pose.py @@ -15,15 +15,16 @@ def test_constructors(self): CartesianPose("A", "B") A = CartesianPose("A", np.array([1, 2, 3]), "B") self.assert_np_array_equal(A.get_position(), [1, 2, 3]) - self.assert_np_array_equal(A.get_orientation(), [1, 0, 0, 0]) + self.assert_np_array_equal(A.get_orientation().elements, [1, 0, 0, 0]) + self.assert_np_array_equal(A.get_orientation_coefficients(), [1, 0, 0, 0]) A = CartesianPose("A", np.array([1, 2, 3, 4]), "B") self.assert_np_array_equal(A.get_position(), [0, 0, 0]) - self.assert_np_array_equal(A.get_orientation(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4])) + self.assert_np_array_equal(A.get_orientation_coefficients(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4])) A = CartesianPose("A", np.array([1, 2, 3]), np.array([1, 2, 3, 4]), "B") self.assert_np_array_equal(A.get_position(), [1, 2, 3]) - self.assert_np_array_equal(A.get_orientation(), [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4])) + self.assert_np_array_equal(A.get_orientation().elements, [1, 2, 3, 4] / np.linalg.norm([1, 2, 3, 4])) def test_copy(self): state = CartesianPose().Random("test") 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 d00e581e3..77b6dfdd8 100755 --- a/python/test/state_representation/space/cartesian/test_cartesian_state.py +++ b/python/test/state_representation/space/cartesian/test_cartesian_state.py @@ -2,6 +2,7 @@ import copy import numpy as np +from pyquaternion.quaternion import Quaternion from numpy.testing import assert_array_equal, assert_array_almost_equal from state_representation import State, CartesianState, StateType, CartesianStateVariable @@ -25,6 +26,7 @@ 'get_linear_velocity', 'get_name', 'get_orientation', + 'get_orientation_coefficients', 'get_pose', 'get_position', 'get_reference_frame', @@ -128,7 +130,7 @@ def test_identity_initialization(self): self.assertTrue(isinstance(identity, State)) self.assertFalse(identity.is_empty()) self.assertAlmostEqual(np.linalg.norm(identity.get_position()), 0) - self.assertAlmostEqual(np.linalg.norm(identity.get_orientation()), 1) + self.assertAlmostEqual(identity.get_orientation().norm, 1) self.assertAlmostEqual(identity.get_orientation()[0], 1) self.assertAlmostEqual(np.linalg.norm(identity.get_twist()), 0) self.assertAlmostEqual(np.linalg.norm(identity.get_acceleration()), 0) @@ -139,7 +141,7 @@ def test_random_initialization(self): self.assertTrue(isinstance(random, State)) self.assertFalse(random.is_empty()) self.assertTrue(np.linalg.norm(random.get_position()) > 0) - self.assertAlmostEqual(np.linalg.norm(random.get_orientation()), 1) + self.assertAlmostEqual(random.get_orientation().norm, 1) [self.assertTrue(random.get_orientation()[i] != 0) for i in range(4)] self.assertTrue(np.linalg.norm(random.get_twist()) > 0) self.assertTrue(np.linalg.norm(random.get_acceleration()) > 0) @@ -188,14 +190,20 @@ def test_get_set_fields(self): with self.assertRaises(RuntimeError): cs.set_position([1., 2., 3., 4.]) - # orientation + # orientation coefficients orientation_vec = np.random.rand(4) orientation_vec = orientation_vec / np.linalg.norm(orientation_vec) cs.set_orientation(orientation_vec) [self.assertAlmostEqual(cs.get_orientation()[i], orientation_vec[i]) for i in range(4)] - # TODO what is an Eigen::Quaternion in Python ? with self.assertRaises(RuntimeError): - cs.set_orientation(position) + cs.set_orientation(orientation_vec[:3]) + + # orientation quaternion + quaternion = Quaternion.random() + cs.set_orientation(quaternion) + assert_array_almost_equal(cs.get_orientation().elements, quaternion.elements) + with self.assertRaises(ValueError): + cs.set_orientation(dict()) matrix = cs.get_transformation_matrix() trans = matrix[:3, 3] @@ -320,7 +328,7 @@ def test_norms(self): norms = cs.norms() self.assertEqual(len(norms), 8) self.assertAlmostEqual(norms[0], np.linalg.norm(cs.get_position())) - self.assertAlmostEqual(norms[1], np.linalg.norm(cs.get_orientation())) + self.assertAlmostEqual(norms[1], cs.get_orientation().norm) self.assertAlmostEqual(norms[2], np.linalg.norm(cs.get_linear_velocity())) self.assertAlmostEqual(norms[3], np.linalg.norm(cs.get_angular_velocity())) self.assertAlmostEqual(norms[4], np.linalg.norm(cs.get_linear_acceleration())) diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index d67adefae..e233d34c6 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.3) +project(control_libraries VERSION 5.2.4) # Build options option(BUILD_TESTING "Build all tests." OFF) From 1b383c8b741c6673ceca3ff77c0a3231f44c3483 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 19 Apr 2022 11:29:37 +0200 Subject: [PATCH 09/17] Add encode/decode option for shared pointer of State (#287) * Add helper to create shared pointer of State * Implement encode/decode shared pointer of State * Update tests * Add rebuild option to dev server script * Update CHANGELOG * Remove inline comment Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> * 5.2.4 -> 5.2.5 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- protocol/clproto_cpp/src/clproto.cpp | 276 +++++++++++++++- .../test/tests/test_cartesian_space_proto.cpp | 174 ++++------ .../test/tests/test_joint_space_proto.cpp | 204 ++++-------- .../clproto_cpp/test/tests/test_messages.cpp | 14 + .../test/tests/test_parameter_proto.cpp | 301 ++++++------------ protocol/dev-server.sh | 5 +- python/setup.py | 2 +- source/CMakeLists.txt | 2 +- .../include/state_representation/State.hpp | 5 + 13 files changed, 518 insertions(+), 472 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 063b08c53..9cca25e9a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,6 +17,7 @@ Release Versions: - Refactor state type (#277, #278, #280, #284) - Refactor dynamical systems factory (#288) - Use pyquaternion in bindings (#283) +- Add encode/decode options for shared pointer of State (#287) ## 5.2.0 diff --git a/VERSION b/VERSION index 73ce95023..462faf748 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.4 +5.2.5 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index eaee01b49..dcd8b40b7 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.2.4 +PROJECT_NUMBER = 5.2.5 # 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 6965b01f0..be8798199 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.2.4) +project(clproto VERSION 5.2.5) set(CMAKE_CXX_STANDARD 17) diff --git a/protocol/clproto_cpp/src/clproto.cpp b/protocol/clproto_cpp/src/clproto.cpp index 37b19be7a..2f6ebcae1 100644 --- a/protocol/clproto_cpp/src/clproto.cpp +++ b/protocol/clproto_cpp/src/clproto.cpp @@ -7,6 +7,7 @@ #include "clproto/decoders.h" #include +#include #include #include #include @@ -19,8 +20,6 @@ #include #include #include -#include -#include #include "state_representation/state_message.pb.h" @@ -1011,6 +1010,279 @@ bool decode(const std::string& msg, Parameter& obj) { return decode_parameter(msg, obj); } +/*----------------------- + * STD::SHARED_PTR + * ---------------------- */ +template<> std::string encode>(const std::shared_ptr& obj); +template<> std::shared_ptr decode(const std::string& msg); +template<> bool decode(const std::string& msg, std::shared_ptr& obj); +template<> std::string encode>(const std::shared_ptr& obj) { + std::string message; + switch (obj->get_type()) { + case StateType::STATE: + message = encode(*obj); + break; + case StateType::SPATIAL_STATE: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::CARTESIAN_STATE: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::CARTESIAN_POSE: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::CARTESIAN_TWIST: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::CARTESIAN_ACCELERATION: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::CARTESIAN_WRENCH: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::JOINT_STATE: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::JOINT_POSITIONS: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::JOINT_VELOCITIES: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::JOINT_ACCELERATIONS: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::JOINT_TORQUES: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::JACOBIAN: + message = encode(*std::dynamic_pointer_cast(obj)); + break; + case StateType::PARAMETER: { + auto param_ptr = std::dynamic_pointer_cast(obj); + switch (param_ptr->get_parameter_type()) { + case ParameterType::BOOL: + message = encode>(*std::dynamic_pointer_cast>(param_ptr)); + break; + case ParameterType::BOOL_ARRAY: + message = encode>>(*std::dynamic_pointer_cast>>(param_ptr)); + break; + case ParameterType::INT: + message = encode>(*std::dynamic_pointer_cast>(param_ptr)); + break; + case ParameterType::INT_ARRAY: + message = encode>>(*std::dynamic_pointer_cast>>(param_ptr)); + break; + case ParameterType::DOUBLE: + message = encode>(*std::dynamic_pointer_cast>(param_ptr)); + break; + case ParameterType::DOUBLE_ARRAY: + message = encode>>(*std::dynamic_pointer_cast>>(param_ptr)); + break; + case ParameterType::STRING: + message = encode>(*std::dynamic_pointer_cast>(param_ptr)); + break; + case ParameterType::STRING_ARRAY: + message = encode>>(*std::dynamic_pointer_cast>>(param_ptr)); + break; + case ParameterType::VECTOR: + message = encode>(*std::dynamic_pointer_cast>(param_ptr)); + break; + case ParameterType::MATRIX: + message = encode>(*std::dynamic_pointer_cast>(param_ptr)); + break; + default: + throw std::invalid_argument("The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported."); + break; + } + break; + } + default: + throw std::invalid_argument("The StateType contained by state " + obj->get_name() + " is unsupported."); + break; + } + return message; +} +template<> std::shared_ptr decode(const std::string& msg) { + std::shared_ptr obj; + switch (check_message_type(msg)) { + case MessageType::STATE_MESSAGE: + obj = make_shared_state(State()); + break; + case MessageType::SPATIAL_STATE_MESSAGE: + obj = make_shared_state(SpatialState()); + break; + case MessageType::CARTESIAN_STATE_MESSAGE: + obj = make_shared_state(CartesianState()); + break; + case MessageType::CARTESIAN_POSE_MESSAGE: + obj = make_shared_state(CartesianPose()); + break; + case MessageType::CARTESIAN_TWIST_MESSAGE: + obj = make_shared_state(CartesianTwist()); + break; + case MessageType::CARTESIAN_ACCELERATION_MESSAGE: + obj = make_shared_state(CartesianAcceleration()); + break; + case MessageType::CARTESIAN_WRENCH_MESSAGE: + obj = make_shared_state(CartesianWrench()); + break; + case MessageType::JOINT_STATE_MESSAGE: + obj = make_shared_state(JointState()); + break; + case MessageType::JOINT_POSITIONS_MESSAGE: + obj = make_shared_state(JointPositions()); + break; + case MessageType::JOINT_VELOCITIES_MESSAGE: + obj = make_shared_state(JointVelocities()); + break; + case MessageType::JOINT_ACCELERATIONS_MESSAGE: + obj = make_shared_state(JointAccelerations()); + break; + case MessageType::JOINT_TORQUES_MESSAGE: + obj = make_shared_state(JointTorques()); + break; + case MessageType::JACOBIAN_MESSAGE: + obj = make_shared_state(Jacobian()); + break; + case MessageType::PARAMETER_MESSAGE: { + switch (check_parameter_message_type(msg)) { + case ParameterMessageType::BOOL: + obj = make_shared_state(Parameter("")); + break; + case ParameterMessageType::BOOL_ARRAY: + obj = make_shared_state(Parameter>("")); + break; + case ParameterMessageType::INT: + obj = make_shared_state(Parameter("")); + break; + case ParameterMessageType::INT_ARRAY: + obj = make_shared_state(Parameter>("")); + break; + case ParameterMessageType::DOUBLE: + obj = make_shared_state(Parameter("")); + break; + case ParameterMessageType::DOUBLE_ARRAY: + obj = make_shared_state(Parameter>("")); + break; + case ParameterMessageType::STRING: + obj = make_shared_state(Parameter("")); + break; + case ParameterMessageType::STRING_ARRAY: + obj = make_shared_state(Parameter>("")); + break; + case ParameterMessageType::VECTOR: + obj = make_shared_state(Parameter("")); + break; + case ParameterMessageType::MATRIX: + obj = make_shared_state(Parameter("")); + break; + default: + throw std::invalid_argument("The ParameterMessageType contained by this message is unsupported."); + break; + } + break; + } + default: + throw std::invalid_argument("The MessageType contained by this message is unsupported."); + break; + } + if (!decode(msg, obj)) { + throw DecodingException("Could not decode the message into a std::shared_ptr"); + } + return obj; +} +template<> bool decode(const std::string& msg, std::shared_ptr& obj) { + try { + switch (obj->get_type()) { + case StateType::STATE: + obj = make_shared_state(decode(msg)); + break; + case StateType::SPATIAL_STATE: + obj = make_shared_state(decode(msg)); + break; + case StateType::CARTESIAN_STATE: + obj = make_shared_state(decode(msg)); + break; + case StateType::CARTESIAN_POSE: + obj = make_shared_state(decode(msg)); + break; + case StateType::CARTESIAN_TWIST: + obj = make_shared_state(decode(msg)); + break; + case StateType::CARTESIAN_ACCELERATION: + obj = make_shared_state(decode(msg)); + break; + case StateType::CARTESIAN_WRENCH: + obj = make_shared_state(decode(msg)); + break; + case StateType::JOINT_STATE: + obj = make_shared_state(decode(msg)); + break; + case StateType::JOINT_POSITIONS: + obj = make_shared_state(decode(msg)); + break; + case StateType::JOINT_VELOCITIES: + obj = make_shared_state(decode(msg)); + break; + case StateType::JOINT_ACCELERATIONS: + obj = make_shared_state(decode(msg)); + break; + case StateType::JOINT_TORQUES: + obj = make_shared_state(decode(msg)); + break; + case StateType::JACOBIAN: + obj = make_shared_state(decode(msg)); + break; + case StateType::PARAMETER: { + auto param_ptr = std::dynamic_pointer_cast(obj); + switch (param_ptr->get_parameter_type()) { + case ParameterType::BOOL: + obj = make_shared_state(decode>(msg)); + break; + case ParameterType::BOOL_ARRAY: + obj = make_shared_state(decode>>(msg)); + break; + case ParameterType::INT: + obj = make_shared_state(decode>(msg)); + break; + case ParameterType::INT_ARRAY: + obj = make_shared_state(decode>>(msg)); + break; + case ParameterType::DOUBLE: + obj = make_shared_state(decode>(msg)); + break; + case ParameterType::DOUBLE_ARRAY: + obj = make_shared_state(decode>>(msg)); + break; + case ParameterType::STRING: + obj = make_shared_state(decode>(msg)); + break; + case ParameterType::STRING_ARRAY: + obj = make_shared_state(decode>>(msg)); + break; + case ParameterType::VECTOR: + obj = make_shared_state(decode>(msg)); + break; + case ParameterType::MATRIX: + obj = make_shared_state(decode>(msg)); + break; + default: + throw std::invalid_argument("The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported."); + break; + } + break; + } + default: + throw std::invalid_argument("The StateType contained by state " + obj->get_name() + " is unsupported."); + break; + } + return true; + } catch (...) { + return false; + } +} + // Generic template code for future types: /* ---------------------- * __TYPE__ diff --git a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp index f9c3ba54f..475e775d0 100644 --- a/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp +++ b/protocol/clproto_cpp/test/tests/test_cartesian_space_proto.cpp @@ -11,158 +11,96 @@ using namespace state_representation; - -TEST(CartesianProtoTest, EncodeDecodeSpatialState) { - auto send_state = SpatialState(StateType::SPATIAL_STATE, "A", "B", false); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE); - - SpatialState recv_state(StateType::STATE); - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_EQ(send_state.is_empty(), recv_state.is_empty()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); -} - -TEST(CartesianProtoTest, EncodeDecodeCartesianState) { - auto send_state = CartesianState::Random("A", "B"); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_STATE_MESSAGE); - - CartesianState recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); - +template +static void test_cart_state_equal(const T& send_state, const T& recv_state) { EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); } -TEST(CartesianProtoTest, EncodeDecodeCartesianPose) { - auto send_state = CartesianPose::Random("A", "B"); +template +static void test_encode_decode_cartesian(const T& send_state, clproto::MessageType type) { std::string msg = clproto::encode(send_state); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_POSE_MESSAGE); + EXPECT_EQ(clproto::check_message_type(msg), type); - CartesianPose recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); + T recv_state; + EXPECT_NO_THROW(clproto::decode(msg)); EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); -} + test_cart_state_equal(send_state, recv_state); -TEST(CartesianProtoTest, EncodeDecodeCartesianTwist) { - auto send_state = CartesianTwist::Random("A", "B"); - std::string msg = clproto::encode(send_state); + auto send_state_ptr = make_shared_state(send_state); + msg = clproto::encode(send_state_ptr); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_TWIST_MESSAGE); + EXPECT_EQ(clproto::check_message_type(msg), type); - CartesianTwist recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); + T recv_state_2; + auto recv_state_ptr = make_shared_state(recv_state_2); + EXPECT_NO_THROW(clproto::decode>(msg)); + EXPECT_TRUE(clproto::decode(msg, recv_state_ptr)); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); + recv_state_2 = *std::dynamic_pointer_cast(recv_state_ptr); + test_cart_state_equal(send_state, recv_state_2); } -TEST(CartesianProtoTest, EncodeDecodeCartesianAcceleration) { - auto send_state = CartesianAcceleration::Random("A", "B"); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_ACCELERATION_MESSAGE); - - CartesianAcceleration recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); +template +static void test_encode_decode_empty_cartesian(const T& state) { + EXPECT_TRUE(state.is_empty()); + std::string msg; + EXPECT_NO_THROW(msg = clproto::encode(state)); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); + T recv_state; + EXPECT_NO_THROW(recv_state = clproto::decode(msg)); + EXPECT_TRUE(recv_state.is_empty()); } -TEST(CartesianProtoTest, EncodeDecodeCartesianWrench) { - auto send_state = CartesianWrench::Random("A", "B"); +TEST(CartesianProtoTest, EncodeDecodeSpatialState) { + auto send_state = SpatialState(StateType::SPATIAL_STATE, "A", "B", false); std::string msg = clproto::encode(send_state); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::CARTESIAN_WRENCH_MESSAGE); + EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE); - CartesianWrench recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); + SpatialState recv_state(StateType::SPATIAL_STATE); + EXPECT_NO_THROW(clproto::decode(msg)); EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); EXPECT_EQ(send_state.get_type(), recv_state.get_type()); + EXPECT_EQ(send_state.is_empty(), recv_state.is_empty()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state.get_reference_frame().c_str()); - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); -} -TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianState) { - CartesianState empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); + std::shared_ptr send_state_ptr = std::make_shared(send_state); + msg = clproto::encode(send_state_ptr); + EXPECT_TRUE(clproto::is_valid(msg)); + EXPECT_TRUE(clproto::check_message_type(msg) == clproto::SPATIAL_STATE_MESSAGE); - CartesianState recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} + SpatialState recv_state_2(StateType::SPATIAL_STATE); + auto recv_state_ptr = make_shared_state(recv_state_2); + EXPECT_NO_THROW(clproto::decode>(msg)); + EXPECT_TRUE(clproto::decode(msg, recv_state_ptr)); -TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianPose) { - CartesianPose empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - CartesianPose recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); + recv_state_2 = *std::dynamic_pointer_cast(recv_state_ptr); + EXPECT_EQ(send_state.get_type(), recv_state_2.get_type()); + EXPECT_EQ(send_state.is_empty(), recv_state_2.is_empty()); + EXPECT_STREQ(send_state.get_name().c_str(), recv_state_2.get_name().c_str()); + EXPECT_STREQ(send_state.get_reference_frame().c_str(), recv_state_2.get_reference_frame().c_str()); } -TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianTwist) { - CartesianTwist empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - CartesianTwist recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); +TEST(CartesianProtoTest, EncodeDecodeRandomCartesian) { + test_encode_decode_cartesian(CartesianState::Random("A", "B"), clproto::CARTESIAN_STATE_MESSAGE); + test_encode_decode_cartesian(CartesianPose::Random("A", "B"), clproto::CARTESIAN_POSE_MESSAGE); + test_encode_decode_cartesian(CartesianTwist::Random("A", "B"), clproto::CARTESIAN_TWIST_MESSAGE); + test_encode_decode_cartesian(CartesianAcceleration::Random("A", "B"), clproto::CARTESIAN_ACCELERATION_MESSAGE); + test_encode_decode_cartesian(CartesianWrench::Random("A", "B"), clproto::CARTESIAN_WRENCH_MESSAGE); } -TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianAcceleration) { - CartesianAcceleration empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - CartesianAcceleration recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); +TEST(CartesianProtoTest, EncodeDecodeEmptyCartesian) { + test_encode_decode_empty_cartesian(CartesianState()); + test_encode_decode_empty_cartesian(CartesianPose()); + test_encode_decode_empty_cartesian(CartesianTwist()); + test_encode_decode_empty_cartesian(CartesianAcceleration()); + test_encode_decode_empty_cartesian(CartesianWrench()); } - -TEST(CartesianProtoTest, EncodeDecodeEmptyCartesianWrench) { - CartesianWrench empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - CartesianWrench recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} \ No newline at end of file diff --git a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp index cfc9ebd55..b2294704f 100644 --- a/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp +++ b/protocol/clproto_cpp/test/tests/test_joint_space_proto.cpp @@ -11,17 +11,18 @@ using namespace state_representation; -TEST(JointProtoTest, EncodeDecodeJacobian) { - auto send_state = Jacobian::Random("robot", {"one", "two", "three"}, "A", "B"); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JACOBIAN_MESSAGE); - - Jacobian recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); +template +static void test_joint_state_equal(const T& send_state, const T& recv_state) { + EXPECT_EQ(send_state.get_type(), recv_state.get_type()); + EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); + ASSERT_EQ(send_state.get_size(), recv_state.get_size()); + for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { + EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str()); + } + EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); +} +static void test_jacobian_equal(const Jacobian& send_state, const Jacobian& recv_state) { EXPECT_EQ(send_state.get_type(), recv_state.get_type()); EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_STREQ(send_state.get_frame().c_str(), recv_state.get_frame().c_str()); @@ -35,105 +36,87 @@ TEST(JointProtoTest, EncodeDecodeJacobian) { EXPECT_NEAR(send_state.data().norm(), recv_state.data().norm(), 1e-5); } -TEST(JointProtoTest, EncodeDecodeJointState) { - std::vector joint_names = {"apple", "orange", "banana", "prune"}; - auto send_state = JointState::Random("zeiss", joint_names); +template +static void test_encode_decode(const T& send_state, clproto::MessageType type) { std::string msg = clproto::encode(send_state); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_STATE_MESSAGE); + EXPECT_EQ(clproto::check_message_type(msg), type); - JointState recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); + T recv_state; + EXPECT_NO_THROW(clproto::decode(msg)); EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - ASSERT_EQ(send_state.get_size(), recv_state.get_size()); - for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { - EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str()); - } - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); -} + test_joint_state_equal(send_state, recv_state); -TEST(JointProtoTest, EncodeDecodeJointPositions) { - auto send_state = JointPositions::Random("robot", 3); - std::string msg = clproto::encode(send_state); + auto send_state_ptr = make_shared_state(send_state); + msg = clproto::encode(send_state_ptr); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_POSITIONS_MESSAGE); + EXPECT_EQ(clproto::check_message_type(msg), type); - JointPositions recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); + T recv_state_2; + auto recv_state_ptr = make_shared_state(recv_state_2); + EXPECT_NO_THROW(clproto::decode>(msg)); + EXPECT_TRUE(clproto::decode(msg, recv_state_ptr)); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - ASSERT_EQ(send_state.get_size(), recv_state.get_size()); - for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { - EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str()); - } - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); + recv_state_2 = *std::dynamic_pointer_cast(recv_state_ptr); + test_joint_state_equal(send_state, recv_state_2); } -TEST(JointProtoTest, EncodeDecodeJointVelocities) { - auto send_state = JointVelocities::Random("robot", 3); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_VELOCITIES_MESSAGE); +template +static void test_encode_decode_empty_joint(const T& state) { + EXPECT_TRUE(state.is_empty()); + std::string msg; + EXPECT_NO_THROW(msg = clproto::encode(state)); - JointVelocities recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); + T recv_state; + EXPECT_NO_THROW(recv_state = clproto::decode(msg)); + EXPECT_TRUE(recv_state.is_empty()); +} - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - ASSERT_EQ(send_state.get_size(), recv_state.get_size()); - for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { - EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str()); - } - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); +TEST(JointProtoTest, EncodeDecodeRandomJoint) { + std::vector joint_names = {"apple", "orange", "banana", "prune"}; + auto send_state = JointState::Random("zeiss", joint_names); + test_encode_decode(send_state, clproto::JOINT_STATE_MESSAGE); + test_encode_decode(JointPositions::Random("robot", 3), clproto::JOINT_POSITIONS_MESSAGE); + test_encode_decode(JointVelocities::Random("robot", 3), clproto::JOINT_VELOCITIES_MESSAGE); + test_encode_decode(JointAccelerations::Random("robot", 3), clproto::JOINT_ACCELERATIONS_MESSAGE); + test_encode_decode(JointTorques::Random("robot", 3), clproto::JOINT_TORQUES_MESSAGE); } -TEST(JointProtoTest, EncodeDecodeJointAccelerations) { - auto send_state = JointAccelerations::Random("robot", 3); +TEST(CartesianProtoTest, EncodeDecodeEmptyJoint) { + test_encode_decode_empty_joint(JointState()); + test_encode_decode_empty_joint(JointPositions()); + test_encode_decode_empty_joint(JointVelocities()); + test_encode_decode_empty_joint(JointAccelerations()); + test_encode_decode_empty_joint(JointTorques()); +} + +TEST(JointProtoTest, EncodeDecodeJacobian) { + auto send_state = Jacobian::Random("robot", {"one", "two", "three"}, "A", "B"); std::string msg = clproto::encode(send_state); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_ACCELERATIONS_MESSAGE); + EXPECT_EQ(clproto::check_message_type(msg), clproto::JACOBIAN_MESSAGE); - JointAccelerations recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); + Jacobian recv_state; + EXPECT_NO_THROW(clproto::decode(msg)); EXPECT_TRUE(clproto::decode(msg, recv_state)); EXPECT_FALSE(recv_state.is_empty()); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - ASSERT_EQ(send_state.get_size(), recv_state.get_size()); - for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { - EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str()); - } - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); -} + test_jacobian_equal(send_state, recv_state); -TEST(JointProtoTest, EncodeDecodeJointTorques) { - auto send_state = JointTorques::Random("robot", 3); - std::string msg = clproto::encode(send_state); + auto send_state_ptr = make_shared_state(send_state); + msg = clproto::encode(send_state_ptr); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::JOINT_TORQUES_MESSAGE); + EXPECT_EQ(clproto::check_message_type(msg), clproto::JACOBIAN_MESSAGE); - JointTorques recv_state; - EXPECT_NO_THROW(clproto::decode(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - EXPECT_FALSE(recv_state.is_empty()); + Jacobian recv_state_2; + auto recv_state_ptr = make_shared_state(recv_state_2); + EXPECT_NO_THROW(clproto::decode>(msg)); + EXPECT_TRUE(clproto::decode(msg, recv_state_ptr)); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - ASSERT_EQ(send_state.get_size(), recv_state.get_size()); - for (std::size_t ind = 0; ind < send_state.get_size(); ++ind) { - EXPECT_STREQ(send_state.get_names().at(ind).c_str(), recv_state.get_names().at(ind).c_str()); - } - EXPECT_NEAR(send_state.dist(recv_state), 0, 1e-5); + recv_state_2 = *std::dynamic_pointer_cast(recv_state_ptr); + test_jacobian_equal(send_state, recv_state_2); } TEST(JointProtoTest, EncodeDecodeEmptyJacobian) { @@ -146,58 +129,3 @@ TEST(JointProtoTest, EncodeDecodeEmptyJacobian) { EXPECT_NO_THROW(recv_state = clproto::decode(msg)); EXPECT_TRUE(recv_state.is_empty()); } - -TEST(JointProtoTest, EncodeDecodeEmptyJointPositions) { - JointPositions empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - JointPositions recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} - -TEST(JointProtoTest, EncodeDecodeEmptyJointState) { - JointState empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - JointState recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} - -TEST(JointProtoTest, EncodeDecodeEmptyJointVelocities) { - JointVelocities empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - JointVelocities recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} - -TEST(JointProtoTest, EncodeDecodeEmptyJointAccelerations) { - JointAccelerations empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - JointAccelerations recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} - -TEST(JointProtoTest, EncodeDecodeEmptyJointTorques) { - JointTorques empty_state; - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - JointTorques recv_state; - EXPECT_NO_THROW(recv_state = clproto::decode(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} diff --git a/protocol/clproto_cpp/test/tests/test_messages.cpp b/protocol/clproto_cpp/test/tests/test_messages.cpp index aac920bbb..9a9625f10 100644 --- a/protocol/clproto_cpp/test/tests/test_messages.cpp +++ b/protocol/clproto_cpp/test/tests/test_messages.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include @@ -25,6 +26,19 @@ TEST(MessageProtoTest, EncodeDecodeState) { recv_state.get_timestamp().time_since_epoch().count()); } +TEST(MessageProtoTest, EncodeDecodeInvalidState) { + auto send_state = Ellipsoid("ellipsoid"); + auto send_state_ptr = make_shared_state(send_state); + EXPECT_THROW(clproto::encode(send_state_ptr), std::invalid_argument); + + auto send_state_2 = State(StateType::STATE, "A", false); + std::string msg = clproto::encode(send_state_2); + + Ellipsoid recv_state; + auto recv_state_ptr = make_shared_state(recv_state); + EXPECT_FALSE(clproto::decode(msg, recv_state_ptr)); +} + TEST(MessageProtoTest, DecodeInvalidString) { std::string dummy_msg = "hello world"; diff --git a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp index 0d3ce1a14..c66d388b7 100644 --- a/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp +++ b/protocol/clproto_cpp/test/tests/test_parameter_proto.cpp @@ -1,242 +1,127 @@ #include #include +#include #include "clproto.h" using namespace state_representation; -TEST(ParameterProtoTest, EncodeDecodeParameterInt) { - int value = 1; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::INT); - - Parameter recv_state(""); - EXPECT_NO_THROW(clproto::decode>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value(), recv_state.get_value()); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); -} - -TEST(ParameterProtoTest, EncodeDecodeParameterIntArray) { - std::vector value = {1, 2, 3}; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::INT_ARRAY); - - Parameter> recv_state(""); - EXPECT_NO_THROW(clproto::decode>>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size()); - for(std::size_t index = 0; index < send_state.get_value().size(); ++index) { - EXPECT_EQ(send_state.get_value().at(index), recv_state.get_value().at(index)); - } - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); -} - -TEST(ParameterProtoTest, EncodeDecodeParameterDouble) { - double value = 1.0; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::DOUBLE); - - Parameter recv_state(""); - EXPECT_NO_THROW(clproto::decode>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value(), recv_state.get_value()); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); -} - -TEST(ParameterProtoTest, EncodeDecodeParameterDoubleArray) { - std::vector value = {1.0, 2.0, 3.0}; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::DOUBLE_ARRAY); - - Parameter> recv_state(""); - EXPECT_NO_THROW(clproto::decode>>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size()); - for(std::size_t index = 0; index < send_state.get_value().size(); ++index) { - EXPECT_EQ(send_state.get_value().at(index), recv_state.get_value().at(index)); - } - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); -} - -TEST(ParameterProtoTest, EncodeDecodeParameterBool) { - bool value = true; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::BOOL); - - Parameter recv_state(""); - EXPECT_NO_THROW(clproto::decode>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - +template +static void test_parameter_equal(const T& send_state, const T& recv_state) { EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); EXPECT_EQ(send_state.get_value(), recv_state.get_value()); EXPECT_EQ(send_state.get_type(), recv_state.get_type()); } -TEST(ParameterProtoTest, EncodeDecodeParameterBoolArray) { - std::vector value = {true, false, true, false}; - - auto send_state = Parameter("A", value); +template +static void test_encode_decode_parameter( + const T& send_state, clproto::MessageType msg_type, clproto::ParameterMessageType param_type +) { std::string msg = clproto::encode(send_state); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::BOOL_ARRAY); + EXPECT_EQ(clproto::check_message_type(msg), msg_type); + EXPECT_EQ(clproto::check_parameter_message_type(msg), param_type); - Parameter> recv_state(""); - EXPECT_NO_THROW(clproto::decode>>(msg)); + T recv_state(""); + EXPECT_NO_THROW(clproto::decode(msg)); EXPECT_TRUE(clproto::decode(msg, recv_state)); + EXPECT_FALSE(recv_state.is_empty()); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size()); - for(std::size_t index = 0; index < send_state.get_value().size(); ++index) { - EXPECT_EQ(send_state.get_value().at(index), recv_state.get_value().at(index)); - } - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); -} - -TEST(ParameterProtoTest, EncodeDecodeParameterString) { - std::string value = "value"; + test_parameter_equal(send_state, recv_state); - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); + auto send_state_ptr = make_shared_state(send_state); + msg = clproto::encode(send_state_ptr); EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::STRING); + EXPECT_EQ(clproto::check_message_type(msg), msg_type); + EXPECT_EQ(clproto::check_parameter_message_type(msg), param_type); - Parameter recv_state(""); - EXPECT_NO_THROW(clproto::decode>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); + T recv_state_2(""); + auto recv_state_ptr = make_shared_state(recv_state_2); + EXPECT_NO_THROW(clproto::decode>(msg)); + EXPECT_TRUE(clproto::decode(msg, recv_state_ptr)); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_STREQ(send_state.get_value().c_str(), recv_state.get_value().c_str()); - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); + recv_state_2 = *std::dynamic_pointer_cast(recv_state_ptr); + test_parameter_equal(send_state, recv_state_2); } -TEST(ParameterProtoTest, EncodeDecodeParameterStringArray) { - std::vector value = {"value 1", "value 2", "value 3"}; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::STRING_ARRAY); - - Parameter> recv_state(""); - EXPECT_NO_THROW(clproto::decode>>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); +template +static void test_encode_decode_empty_parameter(const T& state) { + EXPECT_TRUE(state.is_empty()); + std::string msg; + EXPECT_NO_THROW(msg = clproto::encode(state)); - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size()); - for(std::size_t index = 0; index < send_state.get_value().size(); ++index) { - EXPECT_STREQ(send_state.get_value().at(index).c_str(), recv_state.get_value().at(index).c_str()); - } - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); + T recv_state(""); + EXPECT_NO_THROW(recv_state = clproto::decode(msg)); + EXPECT_TRUE(recv_state.is_empty()); } -TEST(ParameterProtoTest, EncodeDecodeParameterVector) { - Eigen::VectorXd value(5); - value << 1.0, 2.0, 3.0, 4.0, 5.0; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::VECTOR); - - Parameter recv_state(""); - EXPECT_NO_THROW(clproto::decode>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size()); - for(std::size_t index = 0; index < send_state.get_value().size(); ++index) { - EXPECT_EQ(send_state.get_value()(index), recv_state.get_value()(index)); - } - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); +TEST(ParameterProtoTest, EncodeDecodeParameterInt) { + test_encode_decode_parameter( + Parameter("A", 1), clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::INT + ); + test_encode_decode_parameter( + Parameter>("A", {1, 2, 3}), clproto::MessageType::PARAMETER_MESSAGE, + clproto::ParameterMessageType::INT_ARRAY + ); + test_encode_decode_parameter( + Parameter("A", 1.0), clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::DOUBLE + ); + test_encode_decode_parameter( + Parameter>("A", {1.0, 2.0, 3.0}), clproto::MessageType::PARAMETER_MESSAGE, + clproto::ParameterMessageType::DOUBLE_ARRAY + ); + test_encode_decode_parameter( + Parameter("A", true), clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::BOOL + ); + test_encode_decode_parameter( + Parameter>("A", {true, false, true, false}), clproto::MessageType::PARAMETER_MESSAGE, + clproto::ParameterMessageType::BOOL_ARRAY + ); + test_encode_decode_parameter( + Parameter("A", "value"), clproto::MessageType::PARAMETER_MESSAGE, + clproto::ParameterMessageType::STRING + ); + test_encode_decode_parameter( + Parameter>("A", {"value 1", "value 2", "value 3"}), + clproto::MessageType::PARAMETER_MESSAGE, clproto::ParameterMessageType::STRING_ARRAY + ); + Eigen::VectorXd vector_value(5); + vector_value << 1.0, 2.0, 3.0, 4.0, 5.0; + test_encode_decode_parameter( + Parameter("A", vector_value), clproto::MessageType::PARAMETER_MESSAGE, + clproto::ParameterMessageType::VECTOR + ); + Eigen::MatrixXd matrix_value(2, 3); + matrix_value << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + test_encode_decode_parameter( + Parameter("A", matrix_value), clproto::MessageType::PARAMETER_MESSAGE, + clproto::ParameterMessageType::MATRIX + ); } -TEST(ParameterProtoTest, EncodeDecodeParameterMatrix) { - Eigen::MatrixXd value(2, 3); - value << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; - - auto send_state = Parameter("A", value); - std::string msg = clproto::encode(send_state); - EXPECT_TRUE(clproto::is_valid(msg)); - EXPECT_TRUE(clproto::check_message_type(msg) == clproto::MessageType::PARAMETER_MESSAGE); - EXPECT_TRUE(clproto::check_parameter_message_type(msg) == clproto::ParameterMessageType::MATRIX); - - Parameter recv_state(""); - EXPECT_NO_THROW(clproto::decode>(msg)); - EXPECT_TRUE(clproto::decode(msg, recv_state)); - - EXPECT_STREQ(send_state.get_name().c_str(), recv_state.get_name().c_str()); - EXPECT_EQ(send_state.get_value().size(), recv_state.get_value().size()); - EXPECT_EQ(send_state.get_value().rows(), recv_state.get_value().rows()); - EXPECT_EQ(send_state.get_value().cols(), recv_state.get_value().cols()); - for(std::size_t index = 0; index < send_state.get_value().size(); ++index) { - EXPECT_EQ(send_state.get_value()(index), recv_state.get_value()(index)); - } - EXPECT_EQ(send_state.get_type(), recv_state.get_type()); +TEST(ParameterProtoTest, EncodeDecodeEmptyParameter) { + test_encode_decode_empty_parameter(Parameter("")); + test_encode_decode_empty_parameter(Parameter>("")); + test_encode_decode_empty_parameter(Parameter("")); + test_encode_decode_empty_parameter(Parameter>("")); + test_encode_decode_empty_parameter(Parameter("")); + test_encode_decode_empty_parameter(Parameter>("")); + test_encode_decode_empty_parameter(Parameter("")); + test_encode_decode_empty_parameter(Parameter>("")); + test_encode_decode_empty_parameter(Parameter("")); + test_encode_decode_empty_parameter(Parameter("")); } -TEST(ParameterProtoTest, EncodeDecodeEmptyParameterDouble) { - Parameter empty_state(""); - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - Parameter recv_state(""); - EXPECT_NO_THROW(recv_state = clproto::decode>(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} +TEST(MessageProtoTest, EncodeDecodeInvalidParameter) { + auto send_state = Parameter("state", CartesianState::Random("state")); + auto send_state_ptr = make_shared_state(send_state); + EXPECT_THROW(clproto::encode(send_state_ptr), std::invalid_argument); -TEST(ParameterProtoTest, EncodeDecodeEmptyParameterBool) { - Parameter empty_state(""); - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); + auto send_state_2 = State(StateType::STATE, "A", false); + std::string msg = clproto::encode(send_state_2); - Parameter recv_state(""); - EXPECT_NO_THROW(recv_state = clproto::decode>(msg)); - EXPECT_TRUE(recv_state.is_empty()); + Parameter recv_state(""); + auto recv_state_ptr = make_shared_state(recv_state); + EXPECT_FALSE(clproto::decode(msg, recv_state_ptr)); } - -TEST(ParameterProtoTest, EncodeDecodeEmptyParameterString) { - Parameter empty_state(""); - EXPECT_TRUE(empty_state.is_empty()); - std::string msg; - EXPECT_NO_THROW(msg = clproto::encode(empty_state)); - - Parameter recv_state(""); - EXPECT_NO_THROW(recv_state = clproto::decode>(msg)); - EXPECT_TRUE(recv_state.is_empty()); -} \ No newline at end of file diff --git a/protocol/dev-server.sh b/protocol/dev-server.sh index 1d2164665..5270534e9 100755 --- a/protocol/dev-server.sh +++ b/protocol/dev-server.sh @@ -7,7 +7,7 @@ BRANCH=$(git branch --show-current) SSH_PORT=2244 SSH_KEY_FILE="${HOME}/.ssh/id_rsa.pub" -HELP_MESSAGE="Usage: ./dev-server.sh [-b ] [-p ] [-k ] +HELP_MESSAGE="Usage: ./dev-server.sh [-b ] [-p ] [-k ] [-r] Build and run a docker container as an SSH toolchain server for remote development. @@ -32,13 +32,16 @@ Options: -k, --key-file [path] Specify the path of the RSA public key file. (default: ${SSH_KEY_FILE}) + -r, --rebuild Rebuild the image with the --no-cache option. -h, --help Show this help message." +BUILD_FLAGS=() while [ "$#" -gt 0 ]; do case "$1" in -p|--port) SSH_PORT=$2; shift 2;; -k|--key-file) SSH_KEY_FILE=$2; shift 2;; -h|--help) echo "${HELP_MESSAGE}"; exit 0;; + -r|--rebuild) BUILD_FLAGS+=(--no-cache); shift 1;; *) echo 'Error in command line parsing' >&2 echo -e "\n${HELP_MESSAGE}" exit 1 diff --git a/python/setup.py b/python/setup.py index 3f81e4cd0..27d7ba607 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.2.4" +__version__ = "5.2.5" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index e233d34c6..e87d2a4f4 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.4) +project(control_libraries VERSION 5.2.5) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp index 912bff341..49a32f7fd 100644 --- a/source/state_representation/include/state_representation/State.hpp +++ b/source/state_representation/include/state_representation/State.hpp @@ -186,4 +186,9 @@ inline bool State::is_deprecated(const std::chrono::duration return ((std::chrono::steady_clock::now() - this->timestamp_) > time_delay); } +template +std::shared_ptr make_shared_state(const T& state) { + return std::make_shared(state); +} + }// namespace state_representation From 14db4280a6ff361e9f89da232ed761ea19b3ac53 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 19 Apr 2022 15:50:54 +0200 Subject: [PATCH 10/17] Mark Parameter getters const (#289) * To allow const class methods that get parameters or values, or to allow getting values on a const ParameterInterface, mark all getters as const and use const_pointer_cast on shared_from_this * Changelog * 5.2.5 -> 5.2.6 --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 2 +- source/CMakeLists.txt | 2 +- .../state_representation/parameters/Parameter.hpp | 4 ++-- .../parameters/ParameterInterface.hpp | 10 +++++----- 8 files changed, 13 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9cca25e9a..112ba6b62 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ Release Versions: - Refactor dynamical systems factory (#288) - Use pyquaternion in bindings (#283) - Add encode/decode options for shared pointer of State (#287) +- Mark Parameter getters as const (#289) ## 5.2.0 diff --git a/VERSION b/VERSION index 462faf748..a944d7e61 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.5 +5.2.6 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index dcd8b40b7..a0e3af217 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.2.5 +PROJECT_NUMBER = 5.2.6 # 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 be8798199..2f26db941 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.2.5) +project(clproto VERSION 5.2.6) set(CMAKE_CXX_STANDARD 17) diff --git a/python/setup.py b/python/setup.py index 27d7ba607..60b6af845 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.2.5" +__version__ = "5.2.6" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index e87d2a4f4..1d0c5ad1c 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.5) +project(control_libraries VERSION 5.2.6) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/state_representation/include/state_representation/parameters/Parameter.hpp b/source/state_representation/include/state_representation/parameters/Parameter.hpp index 2a419ac2d..82057fd3a 100644 --- a/source/state_representation/include/state_representation/parameters/Parameter.hpp +++ b/source/state_representation/include/state_representation/parameters/Parameter.hpp @@ -49,7 +49,7 @@ class Parameter : public ParameterInterface { * @return The value attribute */ template - U get_value(); + U get_value() const; /** * @brief Getter of the value attribute. @@ -94,7 +94,7 @@ Parameter& Parameter::operator=(const Parameter& parameter) { template template -U Parameter::get_value() { +U Parameter::get_value() const { return static_cast(this->value_); } diff --git a/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp b/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp index 72d0dc844..62d28b854 100644 --- a/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp +++ b/source/state_representation/include/state_representation/parameters/ParameterInterface.hpp @@ -59,7 +59,7 @@ class ParameterInterface : public State { * if downcasting failed and validate_pointer was set to false. */ template - std::shared_ptr> get_parameter(bool validate_pointer = true); + std::shared_ptr> get_parameter(bool validate_pointer = true) const; /** * @brief Get the parameter value of a derived Parameter instance through the ParameterInterface pointer. @@ -70,7 +70,7 @@ class ParameterInterface : public State { * @return The value contained in the underlying Parameter instance */ template - T get_parameter_value(); + T get_parameter_value() const; /** * @brief Set the parameter value of a derived Parameter instance through the ParameterInterface pointer. @@ -103,10 +103,10 @@ class ParameterInterface : public State { }; template -inline std::shared_ptr> ParameterInterface::get_parameter(bool validate_pointer) { +inline std::shared_ptr> ParameterInterface::get_parameter(bool validate_pointer) const { std::shared_ptr> parameter_ptr; try { - parameter_ptr = std::dynamic_pointer_cast>(shared_from_this()); + parameter_ptr = std::dynamic_pointer_cast>(std::const_pointer_cast(shared_from_this())); } catch (const std::exception&) { if (validate_pointer) { throw exceptions::InvalidPointerException( @@ -123,7 +123,7 @@ inline std::shared_ptr> ParameterInterface::get_parameter(bool vali } template -inline T ParameterInterface::get_parameter_value() { +inline T ParameterInterface::get_parameter_value() const { return get_parameter(true)->get_value(); } From c56d2dfc0092118972d4201c7119b86e1af9e5c2 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 26 Apr 2022 16:48:51 +0200 Subject: [PATCH 11/17] Throw exception upon parameter validation in controllers (#290) * Throw exception upon parameter validation in controllers * 5.2.6 -> 5.2.7 * Update CHANGELOG * Revert change in ParameterMap --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 2 +- source/CMakeLists.txt | 2 +- .../controllers/include/controllers/impedance/Impedance.hpp | 4 ++++ source/controllers/src/impedance/CompliantTwist.cpp | 4 ++++ source/state_representation/src/parameters/ParameterMap.cpp | 5 ++--- 9 files changed, 16 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 112ba6b62..6d57ee050 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ Release Versions: - Use pyquaternion in bindings (#283) - Add encode/decode options for shared pointer of State (#287) - Mark Parameter getters as const (#289) +- Throw exception upon parameter validation in controllers (#290) ## 5.2.0 diff --git a/VERSION b/VERSION index a944d7e61..32a406815 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.6 +5.2.7 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index a0e3af217..59ed648f4 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.2.6 +PROJECT_NUMBER = 5.2.7 # 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 2f26db941..8ccb35998 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.2.6) +project(clproto VERSION 5.2.7) set(CMAKE_CXX_STANDARD 17) diff --git a/python/setup.py b/python/setup.py index 60b6af845..3fdcfaa16 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.2.6" +__version__ = "5.2.7" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 1d0c5ad1c..c1502dc1b 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.6) +project(control_libraries VERSION 5.2.7) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/controllers/include/controllers/impedance/Impedance.hpp b/source/controllers/include/controllers/impedance/Impedance.hpp index 30a1287d0..5cd033a3c 100644 --- a/source/controllers/include/controllers/impedance/Impedance.hpp +++ b/source/controllers/include/controllers/impedance/Impedance.hpp @@ -118,6 +118,10 @@ void Impedance::validate_and_set_parameter( } else if (parameter->get_name() == "force_limit") { auto limit_matrix = this->gain_matrix_from_parameter(parameter); this->force_limit_->set_value(limit_matrix.diagonal()); + } else { + throw state_representation::exceptions::InvalidParameterException( + "No parameter with name '" + parameter->get_name() + "' found" + ); } } diff --git a/source/controllers/src/impedance/CompliantTwist.cpp b/source/controllers/src/impedance/CompliantTwist.cpp index 9e45f0792..fe8ed480e 100644 --- a/source/controllers/src/impedance/CompliantTwist.cpp +++ b/source/controllers/src/impedance/CompliantTwist.cpp @@ -92,6 +92,10 @@ void CompliantTwist::validate_and_set_parameter( this->set_angular_stiffness(value); } else if (parameter->get_name() == "angular_damping") { this->set_angular_damping(value); + } else { + throw state_representation::exceptions::InvalidParameterException( + "No parameter with name '" + parameter->get_name() + "' found" + ); } } diff --git a/source/state_representation/src/parameters/ParameterMap.cpp b/source/state_representation/src/parameters/ParameterMap.cpp index baae32c07..68f7a311e 100644 --- a/source/state_representation/src/parameters/ParameterMap.cpp +++ b/source/state_representation/src/parameters/ParameterMap.cpp @@ -52,9 +52,8 @@ void ParameterMap::assert_parameter_valid(const std::shared_ptr& parameter) { - this->parameters_[parameter->get_name()] = parameter; +void ParameterMap::validate_and_set_parameter(const std::shared_ptr& parameter) { + this->parameters_.insert_or_assign(parameter->get_name(), parameter); } } \ No newline at end of file From 2c7142db8848469b7e517fc37d517fe44459b528 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 24 May 2022 11:50:07 +0200 Subject: [PATCH 12/17] Add CartesianState attribute setters from std vector and coefficients (#291) * Add setters with coefficients and std vector for CartesianState * Update unittests * Add another test * Add the bindings, except set_orientation from 4 coefficients * 5.2.7 -> 5.2.8 * Update CHANGELOG * Correct README --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 2 +- .../bind_cartesian_space.cpp | 43 ++- source/CMakeLists.txt | 2 +- .../space/cartesian/CartesianAcceleration.hpp | 17 +- .../space/cartesian/CartesianPose.hpp | 15 ++ .../space/cartesian/CartesianState.hpp | 232 ++++++---------- .../space/cartesian/CartesianTwist.hpp | 11 + .../space/cartesian/CartesianWrench.hpp | 11 + .../src/space/cartesian/CartesianState.cpp | 248 +++++++++++++++++- .../space/cartesian/test_cartesian_state.cpp | 143 +++++++--- 14 files changed, 518 insertions(+), 213 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6d57ee050..ffb9e10d2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ Release Versions: - Add encode/decode options for shared pointer of State (#287) - Mark Parameter getters as const (#289) - Throw exception upon parameter validation in controllers (#290) +- Add CartesianState attribute setters from std vector and coefficients (#291) ## 5.2.0 diff --git a/VERSION b/VERSION index 32a406815..614a37bb3 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.7 +5.2.8 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 59ed648f4..260639a82 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.2.7 +PROJECT_NUMBER = 5.2.8 # 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 8ccb35998..f9f2f588d 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.2.7) +project(clproto VERSION 5.2.8) set(CMAKE_CXX_STANDARD 17) diff --git a/python/setup.py b/python/setup.py index 3fdcfaa16..7de141ea3 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.2.7" +__version__ = "5.2.8" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/python/source/state_representation/bind_cartesian_space.cpp b/python/source/state_representation/bind_cartesian_space.cpp index d50d1c3c8..6d33c98a8 100644 --- a/python/source/state_representation/bind_cartesian_space.cpp +++ b/python/source/state_representation/bind_cartesian_space.cpp @@ -100,20 +100,35 @@ void cartesian_state(py::module_& m) { "Supported types are: pyquaternion.Quaternion, numpy.array, list"); } }, "Setter of the orientation attribute from a pyquaternion.Quaternion, numpy.array(w, x, y, z), or list(w, x, y, z)"); - c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz"); - c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz"); - - c.def("set_linear_velocity", &CartesianState::set_linear_velocity, "Setter of the linear velocity attribute"); - c.def("set_angular_velocity", &CartesianState::set_angular_velocity, "Setter of the angular velocity attribute"); - c.def("set_twist", &CartesianState::set_twist, "Setter of the linear and angular velocities from a 6d twist vector"); - - c.def("set_linear_acceleration", &CartesianState::set_linear_acceleration, "Setter of the linear acceleration attribute"); - c.def("set_angular_acceleration", &CartesianState::set_angular_acceleration, "Setter of the angular acceleration attribute"); - c.def("set_acceleration", &CartesianState::set_acceleration, "Setter of the linear and angular acceleration from a 6d acceleration vector"); - - c.def("set_force", &CartesianState::set_force, "Setter of the force attribute"); - c.def("set_torque", &CartesianState::set_torque, "Setter of the torque attribute"); - c.def("set_wrench", &CartesianState::set_wrench, "Setter of the force and torque from a 6d wrench vector"); + c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d vector of position and orientation coefficients (x, y, z, qw, qx, qy, qz)"); + c.def("set_pose", py::overload_cast&>(&CartesianState::set_pose), "Setter of the pose from a 7d list of position and orientation coefficients (x, y, z, qw, qx, qy, qz)"); + + c.def("set_linear_velocity", py::overload_cast(&CartesianState::set_linear_velocity), "Setter of the linear velocity attribute"); + c.def("set_linear_velocity", py::overload_cast&>(&CartesianState::set_linear_velocity), "Setter of the linear velocity from a list"); + c.def("set_linear_velocity", py::overload_cast(&CartesianState::set_linear_velocity), "Setter of the linear velocity from three scalar coordinates", "x"_a, "y"_a, "z"_a); + c.def("set_angular_velocity", py::overload_cast(&CartesianState::set_angular_velocity), "Setter of the angular velocity attribute"); + c.def("set_angular_velocity", py::overload_cast&>(&CartesianState::set_angular_velocity), "Setter of the angular velocity from a list"); + c.def("set_angular_velocity", py::overload_cast(&CartesianState::set_angular_velocity), "Setter of the angular velocity from three scalar coordinates", "x"_a, "y"_a, "z"_a); + c.def("set_twist", py::overload_cast&>(&CartesianState::set_twist), "Setter of the linear and angular velocities from a 6d twist vector"); + c.def("set_twist", py::overload_cast&>(&CartesianState::set_twist), "Setter of the linear and angular velocities from a list"); + + c.def("set_linear_acceleration", py::overload_cast(&CartesianState::set_linear_acceleration), "Setter of the linear acceleration attribute"); + c.def("set_linear_acceleration", py::overload_cast&>(&CartesianState::set_linear_acceleration), "Setter of the linear acceleration from a list"); + c.def("set_linear_acceleration", py::overload_cast(&CartesianState::set_linear_acceleration), "Setter of the linear acceleration from three scalar coordinates", "x"_a, "y"_a, "z"_a); + c.def("set_angular_acceleration", py::overload_cast(&CartesianState::set_angular_acceleration), "Setter of the angular acceleration attribute"); + c.def("set_angular_acceleration", py::overload_cast&>(&CartesianState::set_angular_acceleration), "Setter of the angular acceleration from a list"); + c.def("set_angular_acceleration", py::overload_cast(&CartesianState::set_angular_acceleration), "Setter of the angular acceleration from three scalar coordinates", "x"_a, "y"_a, "z"_a); + c.def("set_acceleration", py::overload_cast&>(&CartesianState::set_acceleration), "Setter of the linear and angular accelerations from a 6d acceleration vector"); + c.def("set_acceleration", py::overload_cast&>(&CartesianState::set_acceleration), "Setter of the linear and angular accelerations from a list"); + + c.def("set_force", py::overload_cast(&CartesianState::set_force), "Setter of the force attribute"); + c.def("set_force", py::overload_cast&>(&CartesianState::set_force), "Setter of the force from a list"); + c.def("set_force", py::overload_cast(&CartesianState::set_force), "Setter of the force from three scalar coordinates", "x"_a, "y"_a, "z"_a); + c.def("set_torque", py::overload_cast(&CartesianState::set_torque), "Setter of the torque attribute"); + c.def("set_torque", py::overload_cast&>(&CartesianState::set_torque), "Setter of the torque from a list"); + c.def("set_torque", py::overload_cast(&CartesianState::set_torque), "Setter of the torque from three scalar coordinates", "x"_a, "y"_a, "z"_a); + c.def("set_wrench", py::overload_cast&>(&CartesianState::set_wrench), "Setter of the force and torque from a 6d wrench vector"); + c.def("set_wrench", py::overload_cast&>(&CartesianState::set_wrench), "Setter of the force and torque velocities from a list"); c.def("set_zero", &CartesianState::set_zero, "Set the CartesianState to a zero value"); c.def("clamp_state_variable", &CartesianState::clamp_state_variable, "Clamp inplace the magnitude of the a specific state variable (velocity, acceleration or force)", "max_value"_a, "state_variable_type"_a, "noise_ratio"_a=double(0)); diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index c1502dc1b..76c16bede 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.7) +project(control_libraries VERSION 5.2.8) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp index 44260ec9c..47f58810d 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianAcceleration.hpp @@ -33,15 +33,26 @@ class CartesianAcceleration : public CartesianState { void set_orientation(const Eigen::Quaterniond& orientation) = delete; void set_orientation(const Eigen::Vector4d& orientation) = delete; void set_orientation(const std::vector& orientation) = delete; + void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete; void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete; void set_pose(const Eigen::Matrix& pose) = delete; void set_pose(const std::vector& pose) = delete; - void set_linear_velocity(const Eigen::Vector3d& linear_acceleration) = delete; - void set_angular_velocity(const Eigen::Vector3d& angular_acceleration) = delete; - void set_twist(const Eigen::Matrix& acceleration) = delete; + void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete; + void set_linear_velocity(const std::vector& linear_velocity) = delete; + void set_linear_velocity(const double& x, const double& y, const double& z) = delete; + void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete; + void set_angular_velocity(const std::vector& angular_velocity) = delete; + void set_angular_velocity(const double& x, const double& y, const double& z) = delete; + void set_twist(const Eigen::Matrix& twist) = delete; + void set_twist(const std::vector& twist) = delete; void set_force(const Eigen::Vector3d& force) = delete; + void set_force(const std::vector& force) = delete; + void set_force(const double& x, const double& y, const double& z) = delete; void set_torque(const Eigen::Vector3d& torque) = delete; + void set_torque(const std::vector& torque) = delete; + void set_torque(const double& x, const double& y, const double& z) = delete; void set_wrench(const Eigen::Matrix& wrench) = delete; + void set_wrench(const std::vector& wrench) = delete; CartesianState operator*=(const CartesianState& state) = delete; CartesianState operator*(const CartesianState& state) = delete; friend CartesianState operator*=(const CartesianState& state, const CartesianAcceleration& acceleration) = delete; diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp index 43152bc1d..c718c81d7 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianPose.hpp @@ -30,14 +30,29 @@ class CartesianPose : public CartesianState { const Eigen::Vector3d& get_torque() const = delete; Eigen::Matrix get_wrench() const = delete; void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete; + void set_linear_velocity(const std::vector& linear_velocity) = delete; + void set_linear_velocity(const double& x, const double& y, const double& z) = delete; void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete; + void set_angular_velocity(const std::vector& angular_velocity) = delete; + void set_angular_velocity(const double& x, const double& y, const double& z) = delete; void set_twist(const Eigen::Matrix& twist) = delete; + void set_twist(const std::vector& twist) = delete; void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete; + void set_linear_acceleration(const std::vector& linear_acceleration) = delete; + void set_linear_acceleration(const double& x, const double& y, const double& z) = delete; void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete; + void set_angular_acceleration(const std::vector& angular_acceleration) = delete; + void set_angular_acceleration(const double& x, const double& y, const double& z) = delete; void set_acceleration(const Eigen::Matrix& acceleration) = delete; + void set_acceleration(const std::vector& acceleration) = delete; void set_force(const Eigen::Vector3d& force) = delete; + void set_force(const std::vector& force) = delete; + void set_force(const double& x, const double& y, const double& z) = delete; void set_torque(const Eigen::Vector3d& torque) = delete; + void set_torque(const std::vector& torque) = delete; + void set_torque(const double& x, const double& y, const double& z) = delete; void set_wrench(const Eigen::Matrix& wrench) = delete; + void set_wrench(const std::vector& wrench) = delete; CartesianState operator*=(const CartesianState& state) = delete; friend CartesianState operator*=(const CartesianState& state, const CartesianPose& pose) = delete; 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 31abc95ab..3e67ce2f2 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp @@ -257,6 +257,11 @@ class CartesianState : public SpatialState { */ void set_orientation(const std::vector& orientation); + /** + * @brief Setter of the orientation from four scalar coefficients (w, x, y, z) + */ + void set_orientation(const double& w, const double& x, const double& y, const double& z); + /** * @brief Setter of the pose from both position and orientation * @param position the position @@ -283,46 +288,121 @@ class CartesianState : public SpatialState { */ void set_linear_velocity(const Eigen::Vector3d& linear_velocity); + /** + * @brief Setter of the linear velocity from a std vector + */ + void set_linear_velocity(const std::vector& linear_velocity); + + /** + * @brief Setter of the linear velocity from three scalar coordinates + */ + void set_linear_velocity(const double& x, const double& y, const double& z); + /** * @brief Setter of the angular velocity attribute */ void set_angular_velocity(const Eigen::Vector3d& angular_velocity); + /** + * @brief Setter of the angular velocity from a std vector + */ + void set_angular_velocity(const std::vector& angular_velocity); + + /** + * @brief Setter of the angular velocity from three scalar coordinates + */ + void set_angular_velocity(const double& x, const double& y, const double& z); + /** * @brief Setter of the linear and angular velocities from a 6d twist vector */ void set_twist(const Eigen::Matrix& twist); + /** + * @brief Setter of the linear and angular velocities from a std vector + */ + void set_twist(const std::vector& twist); + /** * @brief Setter of the linear acceleration attribute */ void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration); + /** + * @brief Setter of the linear acceleration from a std vector + */ + void set_linear_acceleration(const std::vector& linear_acceleration); + + /** + * @brief Setter of the linear acceleration from three scalar coordinates + */ + void set_linear_acceleration(const double& x, const double& y, const double& z); + /** * @brief Setter of the angular velocity attribute */ void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration); + /** + * @brief Setter of the angular acceleration from a std vector + */ + void set_angular_acceleration(const std::vector& angular_acceleration); + + /** + * @brief Setter of the angular acceleration from three scalar coordinates + */ + void set_angular_acceleration(const double& x, const double& y, const double& z); + /** * @brief Setter of the linear and angular acceleration from a 6d acceleration vector */ void set_acceleration(const Eigen::Matrix& acceleration); + /** + * @brief Setter of the linear and angular acceleration from a std vector + */ + void set_acceleration(const std::vector& acceleration); + /** * @brief Setter of the force attribute */ void set_force(const Eigen::Vector3d& force); /** - * @brief Setter of the force attribute + * @brief Setter of the force from a std vector + */ + void set_force(const std::vector& force); + + /** + * @brief Setter of the force from three scalar coordinates + */ + void set_force(const double& x, const double& y, const double& z); + + /** + * @brief Setter of the torque attribute */ void set_torque(const Eigen::Vector3d& torque); + /** + * @brief Setter of the torque from a std vector + */ + void set_torque(const std::vector& torque); + + /** + * @brief Setter of the torque from three scalar coordinates + */ + void set_torque(const double& x, const double& y, const double& z); + /** * @brief Setter of the force and torque from a 6d wrench vector */ void set_wrench(const Eigen::Matrix& wrench); + /** + * @brief Setter of the force and torque from a std vector + */ + void set_wrench(const std::vector& wrench); + /** * @brief Initialize the CartesianState to a zero value */ @@ -535,74 +615,6 @@ inline CartesianState& CartesianState::operator=(const CartesianState& state) { return *this; } -inline const Eigen::Vector3d& CartesianState::get_position() const { - return this->position_; -} - -inline const Eigen::Quaterniond& CartesianState::get_orientation() const { - return this->orientation_; -} - -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()); -} - -inline Eigen::Matrix CartesianState::get_pose() const { - Eigen::Matrix pose; - pose << this->get_position(), this->get_orientation_coefficients(); - return pose; -} - -inline Eigen::Matrix4d CartesianState::get_transformation_matrix() const { - Eigen::Matrix4d pose; - pose << this->orientation_.toRotationMatrix(), this->position_, 0., 0., 0., 1; - return pose; -} - -inline const Eigen::Vector3d& CartesianState::get_linear_velocity() const { - return this->linear_velocity_; -} - -inline const Eigen::Vector3d& CartesianState::get_angular_velocity() const { - return this->angular_velocity_; -} - -inline Eigen::Matrix CartesianState::get_twist() const { - Eigen::Matrix twist; - twist << this->get_linear_velocity(), this->get_angular_velocity(); - return twist; -} - -inline const Eigen::Vector3d& CartesianState::get_linear_acceleration() const { - return this->linear_acceleration_; -} - -inline const Eigen::Vector3d& CartesianState::get_angular_acceleration() const { - return this->angular_acceleration_; -} - -inline Eigen::Matrix CartesianState::get_acceleration() const { - Eigen::Matrix acceleration; - acceleration << this->get_linear_acceleration(), this->get_angular_acceleration(); - return acceleration; -} - -inline const Eigen::Vector3d& CartesianState::get_force() const { - return this->force_; -} - -inline const Eigen::Vector3d& CartesianState::get_torque() const { - return this->torque_; -} - -inline Eigen::Matrix CartesianState::get_wrench() const { - Eigen::Matrix wrench; - wrench << this->get_force(), this->get_torque(); - return wrench; -} - inline Eigen::VectorXd CartesianState::get_state_variable(const CartesianStateVariable& state_variable_type) const { switch (state_variable_type) { case CartesianStateVariable::POSITION: @@ -682,88 +694,6 @@ inline void CartesianState::set_state_variable( this->set_state_variable(angular_state_variable, new_value.tail(3)); } -inline void CartesianState::set_position(const Eigen::Vector3d& position) { - this->set_state_variable(this->position_, position); -} - -inline void CartesianState::set_position(const std::vector& position) { - this->set_state_variable(this->position_, position); -} - -inline void CartesianState::set_position(const double& x, const double& y, const double& z) { - this->set_position(Eigen::Vector3d(x, y, z)); -} - -inline void CartesianState::set_orientation(const Eigen::Quaterniond& orientation) { - this->set_filled(); - this->orientation_ = orientation.normalized(); -} - -inline void CartesianState::set_orientation(const Eigen::Vector4d& orientation) { - this->set_orientation(Eigen::Quaterniond(orientation(0), orientation(1), orientation(2), orientation(3))); -} - -inline void CartesianState::set_orientation(const std::vector& orientation) { - if (orientation.size() != 4) { - throw exceptions::IncompatibleSizeException("The input vector is not of size 4 required for orientation"); - } - this->set_orientation(Eigen::Vector4d::Map(orientation.data(), orientation.size())); -} - -inline void CartesianState::set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) { - this->set_position(position); - this->set_orientation(orientation); -} - -inline void CartesianState::set_pose(const Eigen::Matrix& pose) { - this->set_position(pose.head(3)); - this->set_orientation(pose.tail(4)); -} - -inline void CartesianState::set_pose(const std::vector& pose) { - if (pose.size() != 7) { - throw exceptions::IncompatibleSizeException("The input vector is not of size 7 required for pose"); - } - this->set_position(std::vector(pose.begin(), pose.begin() + 3)); - this->set_orientation(std::vector(pose.begin() + 3, pose.end())); -} - -inline void CartesianState::set_linear_velocity(const Eigen::Vector3d& linear_velocity) { - this->set_state_variable(this->linear_velocity_, linear_velocity); -} - -inline void CartesianState::set_angular_velocity(const Eigen::Vector3d& angular_velocity) { - this->set_state_variable(this->angular_velocity_, angular_velocity); -} - -inline void CartesianState::set_twist(const Eigen::Matrix& twist) { - this->set_state_variable(this->linear_velocity_, this->angular_velocity_, twist); -} - -inline void CartesianState::set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) { - this->set_state_variable(this->linear_acceleration_, linear_acceleration); -} - -inline void CartesianState::set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) { - this->set_state_variable(this->angular_acceleration_, angular_acceleration); -} - -inline void CartesianState::set_acceleration(const Eigen::Matrix& acceleration) { - this->set_state_variable(this->linear_acceleration_, this->angular_acceleration_, acceleration); -} - -inline void CartesianState::set_force(const Eigen::Vector3d& force) { - this->set_state_variable(this->force_, force); -} - -inline void CartesianState::set_torque(const Eigen::Vector3d& torque) { - this->set_state_variable(this->torque_, torque); -} - -inline void CartesianState::set_wrench(const Eigen::Matrix& wrench) { - this->set_state_variable(this->force_, this->torque_, wrench); -} - inline void CartesianState::set_state_variable( const Eigen::VectorXd& new_value, const CartesianStateVariable& state_variable_type ) { diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp index 064c49b25..f0943406c 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianTwist.hpp @@ -35,15 +35,26 @@ class CartesianTwist : public CartesianState { void set_orientation(const Eigen::Quaterniond& orientation) = delete; void set_orientation(const Eigen::Vector4d& orientation) = delete; void set_orientation(const std::vector& orientation) = delete; + void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete; void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete; void set_pose(const Eigen::Matrix& pose) = delete; void set_pose(const std::vector& pose) = delete; void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete; + void set_linear_acceleration(const std::vector& linear_acceleration) = delete; + void set_linear_acceleration(const double& x, const double& y, const double& z) = delete; void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete; + void set_angular_acceleration(const std::vector& angular_acceleration) = delete; + void set_angular_acceleration(const double& x, const double& y, const double& z) = delete; void set_acceleration(const Eigen::Matrix& acceleration) = delete; + void set_acceleration(const std::vector& acceleration) = delete; void set_force(const Eigen::Vector3d& force) = delete; + void set_force(const std::vector& force) = delete; + void set_force(const double& x, const double& y, const double& z) = delete; void set_torque(const Eigen::Vector3d& torque) = delete; + void set_torque(const std::vector& torque) = delete; + void set_torque(const double& x, const double& y, const double& z) = delete; void set_wrench(const Eigen::Matrix& wrench) = delete; + void set_wrench(const std::vector& wrench) = delete; CartesianState operator*=(const CartesianState& state) = delete; CartesianState operator*(const CartesianState& state) = delete; friend CartesianState operator*=(const CartesianState& state, const CartesianTwist& twist) = delete; diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp index 92fc01ef2..862eec2f2 100644 --- a/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp +++ b/source/state_representation/include/state_representation/space/cartesian/CartesianWrench.hpp @@ -34,15 +34,26 @@ class CartesianWrench : public CartesianState { void set_orientation(const Eigen::Quaterniond& orientation) = delete; void set_orientation(const Eigen::Vector4d& orientation) = delete; void set_orientation(const std::vector& orientation) = delete; + void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete; void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete; void set_pose(const Eigen::Matrix& pose) = delete; void set_pose(const std::vector& pose) = delete; void set_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete; + void set_linear_velocity(const std::vector& linear_velocity) = delete; + void set_linear_velocity(const double& x, const double& y, const double& z) = delete; void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete; + void set_angular_velocity(const std::vector& angular_velocity) = delete; + void set_angular_velocity(const double& x, const double& y, const double& z) = delete; void set_twist(const Eigen::Matrix& twist) = delete; + void set_twist(const std::vector& twist) = delete; void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete; + void set_linear_acceleration(const std::vector& linear_acceleration) = delete; + void set_linear_acceleration(const double& x, const double& y, const double& z) = delete; void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete; + void set_angular_acceleration(const std::vector& angular_acceleration) = delete; + void set_angular_acceleration(const double& x, const double& y, const double& z) = delete; void set_acceleration(const Eigen::Matrix& acceleration) = delete; + void set_acceleration(const std::vector& acceleration) = delete; CartesianState operator*=(const CartesianState& state) = delete; CartesianState operator*(const CartesianState& state) = delete; friend CartesianState operator*=(const CartesianState& state, const CartesianWrench& wrench) = delete; diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp index b4319c3b9..2376608bb 100644 --- a/source/state_representation/src/space/cartesian/CartesianState.cpp +++ b/source/state_representation/src/space/cartesian/CartesianState.cpp @@ -45,6 +45,232 @@ CartesianState CartesianState::Random(const std::string& name, const std::string return random; } +const Eigen::Vector3d& CartesianState::get_position() const { + return this->position_; +} + +const Eigen::Quaterniond& CartesianState::get_orientation() const { + return this->orientation_; +} + +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()); +} + +Eigen::Matrix CartesianState::get_pose() const { + Eigen::Matrix pose; + pose << this->get_position(), this->get_orientation_coefficients(); + return pose; +} + +Eigen::Matrix4d CartesianState::get_transformation_matrix() const { + Eigen::Matrix4d pose; + pose << this->orientation_.toRotationMatrix(), this->position_, 0., 0., 0., 1; + return pose; +} + +const Eigen::Vector3d& CartesianState::get_linear_velocity() const { + return this->linear_velocity_; +} + +const Eigen::Vector3d& CartesianState::get_angular_velocity() const { + return this->angular_velocity_; +} + +Eigen::Matrix CartesianState::get_twist() const { + Eigen::Matrix twist; + twist << this->get_linear_velocity(), this->get_angular_velocity(); + return twist; +} + +const Eigen::Vector3d& CartesianState::get_linear_acceleration() const { + return this->linear_acceleration_; +} + +const Eigen::Vector3d& CartesianState::get_angular_acceleration() const { + return this->angular_acceleration_; +} + +Eigen::Matrix CartesianState::get_acceleration() const { + Eigen::Matrix acceleration; + acceleration << this->get_linear_acceleration(), this->get_angular_acceleration(); + return acceleration; +} + +const Eigen::Vector3d& CartesianState::get_force() const { + return this->force_; +} + +const Eigen::Vector3d& CartesianState::get_torque() const { + return this->torque_; +} + +Eigen::Matrix CartesianState::get_wrench() const { + Eigen::Matrix wrench; + wrench << this->get_force(), this->get_torque(); + return wrench; +} + +void CartesianState::set_position(const Eigen::Vector3d& position) { + this->set_state_variable(this->position_, position); +} + +void CartesianState::set_position(const std::vector& position) { + this->set_state_variable(this->position_, position); +} + +void CartesianState::set_position(const double& x, const double& y, const double& z) { + this->set_position(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_orientation(const Eigen::Quaterniond& orientation) { + this->set_filled(); + this->orientation_ = orientation.normalized(); +} + +void CartesianState::set_orientation(const Eigen::Vector4d& orientation) { + this->set_orientation(Eigen::Quaterniond(orientation(0), orientation(1), orientation(2), orientation(3))); +} + +void CartesianState::set_orientation(const std::vector& orientation) { + if (orientation.size() != 4) { + throw exceptions::IncompatibleSizeException("The input vector is not of size 4 required for orientation"); + } + this->set_orientation(Eigen::Vector4d::Map(orientation.data(), orientation.size())); +} + +void CartesianState::set_orientation(const double& w, const double& x, const double& y, const double& z) { + this->set_orientation(Eigen::Vector4d(w, x, y, z)); +} + +void CartesianState::set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) { + this->set_position(position); + this->set_orientation(orientation); +} + +void CartesianState::set_pose(const Eigen::Matrix& pose) { + this->set_position(pose.head(3)); + this->set_orientation(pose.tail(4)); +} + +void CartesianState::set_pose(const std::vector& pose) { + if (pose.size() != 7) { + throw exceptions::IncompatibleSizeException("The input vector is not of size 7 required for pose"); + } + this->set_position(std::vector(pose.begin(), pose.begin() + 3)); + this->set_orientation(std::vector(pose.begin() + 3, pose.end())); +} + +void CartesianState::set_linear_velocity(const Eigen::Vector3d& linear_velocity) { + this->set_state_variable(this->linear_velocity_, linear_velocity); +} + +void CartesianState::set_linear_velocity(const std::vector& linear_velocity) { + this->set_state_variable(this->linear_velocity_, linear_velocity); +} + +void CartesianState::set_linear_velocity(const double& x, const double& y, const double& z) { + this->set_linear_velocity(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_angular_velocity(const Eigen::Vector3d& angular_velocity) { + this->set_state_variable(this->angular_velocity_, angular_velocity); +} + +void CartesianState::set_angular_velocity(const std::vector& angular_velocity) { + this->set_state_variable(this->angular_velocity_, angular_velocity); +} + +void CartesianState::set_angular_velocity(const double& x, const double& y, const double& z) { + this->set_angular_velocity(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_twist(const Eigen::Matrix& twist) { + this->set_state_variable(this->linear_velocity_, this->angular_velocity_, twist); +} + +void CartesianState::set_twist(const std::vector& twist) { + if (twist.size() != 6) { + throw exceptions::IncompatibleSizeException("The input vector is not of size 6 required for twist"); + } + this->set_linear_velocity(std::vector(twist.begin(), twist.begin() + 3)); + this->set_angular_velocity(std::vector(twist.begin() + 3, twist.end())); +} + +void CartesianState::set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) { + this->set_state_variable(this->linear_acceleration_, linear_acceleration); +} + +void CartesianState::set_linear_acceleration(const std::vector& linear_acceleration) { + this->set_state_variable(this->linear_acceleration_, linear_acceleration); +} + +void CartesianState::set_linear_acceleration(const double& x, const double& y, const double& z) { + this->set_linear_acceleration(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) { + this->set_state_variable(this->angular_acceleration_, angular_acceleration); +} + +void CartesianState::set_angular_acceleration(const std::vector& angular_acceleration) { + this->set_state_variable(this->angular_acceleration_, angular_acceleration); +} + +void CartesianState::set_angular_acceleration(const double& x, const double& y, const double& z) { + this->set_angular_acceleration(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_acceleration(const Eigen::Matrix& acceleration) { + this->set_state_variable(this->linear_acceleration_, this->angular_acceleration_, acceleration); +} + +void CartesianState::set_acceleration(const std::vector& acceleration) { + if (acceleration.size() != 6) { + throw exceptions::IncompatibleSizeException("The input vector is not of size 6 required for acceleration"); + } + this->set_linear_acceleration(std::vector(acceleration.begin(), acceleration.begin() + 3)); + this->set_angular_acceleration(std::vector(acceleration.begin() + 3, acceleration.end())); +} + +void CartesianState::set_force(const Eigen::Vector3d& force) { + this->set_state_variable(this->force_, force); +} + +void CartesianState::set_force(const std::vector& force) { + this->set_state_variable(this->force_, force); +} + +void CartesianState::set_force(const double& x, const double& y, const double& z) { + this->set_force(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_torque(const Eigen::Vector3d& torque) { + this->set_state_variable(this->torque_, torque); +} + +void CartesianState::set_torque(const std::vector& torque) { + this->set_state_variable(this->torque_, torque); +} + +void CartesianState::set_torque(const double& x, const double& y, const double& z) { + this->set_torque(Eigen::Vector3d(x, y, z)); +} + +void CartesianState::set_wrench(const Eigen::Matrix& wrench) { + this->set_state_variable(this->force_, this->torque_, wrench); +} + +void CartesianState::set_wrench(const std::vector& wrench) { + if (wrench.size() != 6) { + throw exceptions::IncompatibleSizeException("The input vector is not of size 6 required for wrench"); + } + this->set_force(std::vector(wrench.begin(), wrench.begin() + 3)); + this->set_torque(std::vector(wrench.begin() + 3, wrench.end())); +} + CartesianState& CartesianState::operator*=(double lambda) { // sanity check if (this->is_empty()) { @@ -125,8 +351,9 @@ CartesianState& CartesianState::operator*=(const CartesianState& state) { Eigen::Vector3d f_alpha_b = this->get_angular_acceleration(); // intermediate variables for b_S_c Eigen::Vector3d b_P_c = state.get_position(); - Eigen::Quaterniond b_R_c = (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() - : Eigen::Quaterniond(-state.get_orientation().coeffs()); + Eigen::Quaterniond b_R_c = + (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() : Eigen::Quaterniond( + -state.get_orientation().coeffs()); Eigen::Vector3d b_v_c = state.get_linear_velocity(); Eigen::Vector3d b_omega_c = state.get_angular_velocity(); Eigen::Vector3d b_a_c = state.get_linear_acceleration(); @@ -138,10 +365,9 @@ CartesianState& CartesianState::operator*=(const CartesianState& state) { this->set_linear_velocity(f_v_b + f_R_b * b_v_c + f_omega_b.cross(f_R_b * b_P_c)); this->set_angular_velocity(f_omega_b + f_R_b * b_omega_c); // acceleration - this->set_linear_acceleration(f_a_b + f_R_b * b_a_c - + f_alpha_b.cross(f_R_b * b_P_c) - + 2 * f_omega_b.cross(f_R_b * b_v_c) - + f_omega_b.cross(f_omega_b.cross(f_R_b * b_P_c))); + this->set_linear_acceleration( + f_a_b + f_R_b * b_a_c + f_alpha_b.cross(f_R_b * b_P_c) + 2 * f_omega_b.cross(f_R_b * b_v_c) + + f_omega_b.cross(f_omega_b.cross(f_R_b * b_P_c))); this->set_angular_acceleration(f_alpha_b + f_R_b * b_alpha_c + f_omega_b.cross(f_R_b * b_omega_c)); // wrench //TODO @@ -168,8 +394,9 @@ CartesianState& CartesianState::operator+=(const CartesianState& state) { // operation on pose this->set_position(this->get_position() + state.get_position()); // specific operation on quaternion using Hamilton product - Eigen::Quaterniond orientation = (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() - : Eigen::Quaterniond(-state.get_orientation().coeffs()); + Eigen::Quaterniond orientation = + (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() : Eigen::Quaterniond( + -state.get_orientation().coeffs()); this->set_orientation(this->get_orientation() * orientation); // operation on twist this->set_twist(this->get_twist() + state.get_twist()); @@ -200,8 +427,9 @@ CartesianState& CartesianState::operator-=(const CartesianState& state) { // operation on pose this->set_position(this->get_position() - state.get_position()); // specific operation on quaternion using Hamilton product - Eigen::Quaterniond orientation = (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() - : Eigen::Quaterniond(-state.get_orientation().coeffs()); + Eigen::Quaterniond orientation = + (this->get_orientation().dot(state.get_orientation()) > 0) ? state.get_orientation() : Eigen::Quaterniond( + -state.get_orientation().coeffs()); this->set_orientation(this->get_orientation() * orientation.conjugate()); // operation on twist this->set_twist(this->get_twist() - state.get_twist()); 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 26646af30..6999744bf 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 @@ -104,6 +104,8 @@ TEST(CartesianStateTest, CopyConstructor) { TEST(CartesianStateTest, GetSetFields) { CartesianState cs("test"); + static Eigen::Vector3d data; + static std::vector std_data(3); // name cs.set_name("robot"); @@ -113,10 +115,10 @@ TEST(CartesianStateTest, GetSetFields) { EXPECT_EQ(cs.get_reference_frame(), "base"); // position - std::vector position{1, 2, 3}; - cs.set_position(position); - for (std::size_t i = 0; i < position.size(); ++i) { - EXPECT_FLOAT_EQ(cs.get_position()(i), position.at(i)); + std_data = {1, 2, 3}; + cs.set_position(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_position()(i), std_data.at(i)); } cs.set_position(1.1, 2.2, 3.3); EXPECT_TRUE(Eigen::Vector3d(1.1, 2.2, 3.3).isApprox(cs.get_position())); @@ -133,7 +135,10 @@ TEST(CartesianStateTest, GetSetFields) { orientation{random_orientation.w(), random_orientation.x(), random_orientation.y(), random_orientation.z()}; cs.set_orientation(orientation); EXPECT_TRUE(random_orientation.coeffs().isApprox(cs.get_orientation().coeffs())); - EXPECT_THROW(cs.set_orientation(position), exceptions::IncompatibleSizeException); + random_orientation = Eigen::Quaterniond::UnitRandom(); + cs.set_orientation(random_orientation.w(), random_orientation.x(), random_orientation.y(), random_orientation.z()); + EXPECT_TRUE(random_orientation.coeffs().isApprox(cs.get_orientation().coeffs())); + EXPECT_THROW(cs.set_orientation(std_data), exceptions::IncompatibleSizeException); auto matrix = cs.get_transformation_matrix(); Eigen::Vector3d trans = matrix.topRightCorner<3, 1>(); @@ -146,38 +151,116 @@ TEST(CartesianStateTest, GetSetFields) { // pose EXPECT_THROW(cs.set_pose(std::vector(8)), exceptions::IncompatibleSizeException); + // linear velocity + data = Eigen::Vector3d::Random(); + cs.set_linear_velocity(data); + EXPECT_TRUE(cs.get_linear_velocity().isApprox(data)); + std_data = {2, 3, 4}; + cs.set_linear_velocity(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_linear_velocity()(i), std_data.at(i)); + } + cs.set_linear_velocity(2.1, 3.2, 4.3); + EXPECT_TRUE(Eigen::Vector3d(2.1, 3.2, 4.3).isApprox(cs.get_linear_velocity())); + EXPECT_THROW(cs.set_linear_velocity(std::vector(4)), exceptions::IncompatibleSizeException); + + // angular velocity + data = Eigen::Vector3d::Random(); + cs.set_angular_velocity(data); + EXPECT_TRUE(cs.get_angular_velocity().isApprox(data)); + std_data = {3, 4, 5}; + cs.set_angular_velocity(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_angular_velocity()(i), std_data.at(i)); + } + cs.set_angular_velocity(3.1, 4.2, 5.3); + EXPECT_TRUE(Eigen::Vector3d(3.1, 4.2, 5.3).isApprox(cs.get_angular_velocity())); + EXPECT_THROW(cs.set_angular_velocity(std::vector(4)), exceptions::IncompatibleSizeException); + // twist - Eigen::Vector3d linear_velocity = Eigen::Vector3d::Random(); - cs.set_linear_velocity(linear_velocity); - EXPECT_TRUE(cs.get_linear_velocity().isApprox(linear_velocity)); - Eigen::Vector3d angular_velocity = Eigen::Vector3d::Random(); - cs.set_angular_velocity(angular_velocity); - EXPECT_TRUE(cs.get_angular_velocity().isApprox(angular_velocity)); - Eigen::VectorXd twist = Eigen::VectorXd::Random(6); + Eigen::VectorXd twist_eigen = Eigen::VectorXd::Random(6); + cs.set_twist(twist_eigen); + EXPECT_TRUE(cs.get_twist().isApprox(twist_eigen)); + std::vector twist{4, 5, 6, 7, 8, 9}; cs.set_twist(twist); - EXPECT_TRUE(cs.get_twist().isApprox(twist)); + for (std::size_t i = 0; i < twist.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_twist()(i), twist.at(i)); + } + EXPECT_THROW(cs.set_twist(std::vector(4)), exceptions::IncompatibleSizeException); + + // linear acceleration + data = Eigen::Vector3d::Random(); + cs.set_linear_acceleration(data); + EXPECT_TRUE(cs.get_linear_acceleration().isApprox(data)); + std_data = {5, 6, 7}; + cs.set_linear_acceleration(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_linear_acceleration()(i), std_data.at(i)); + } + cs.set_linear_acceleration(5.1, 6.2, 7.3); + EXPECT_TRUE(Eigen::Vector3d(5.1, 6.2, 7.3).isApprox(cs.get_linear_acceleration())); + EXPECT_THROW(cs.set_linear_acceleration(std::vector(4)), exceptions::IncompatibleSizeException); + + // angular acceleration + data = Eigen::Vector3d::Random(); + cs.set_angular_acceleration(data); + EXPECT_TRUE(cs.get_angular_acceleration().isApprox(data)); + std_data = {6, 7, 8}; + cs.set_angular_acceleration(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_angular_acceleration()(i), std_data.at(i)); + } + cs.set_angular_acceleration(6.1, 7.2, 8.3); + EXPECT_TRUE(Eigen::Vector3d(6.1, 7.2, 8.3).isApprox(cs.get_angular_acceleration())); + EXPECT_THROW(cs.set_angular_acceleration(std::vector(4)), exceptions::IncompatibleSizeException); // acceleration - Eigen::Vector3d linear_acceleration = Eigen::Vector3d::Random(); - cs.set_linear_acceleration(linear_acceleration); - EXPECT_TRUE(cs.get_linear_acceleration().isApprox(linear_acceleration)); - Eigen::Vector3d angular_acceleration = Eigen::Vector3d::Random(); - cs.set_angular_acceleration(angular_acceleration); - EXPECT_TRUE(cs.get_angular_acceleration().isApprox(angular_acceleration)); - Eigen::VectorXd accerlerations = Eigen::VectorXd::Random(6); - cs.set_acceleration(accerlerations); - EXPECT_TRUE(cs.get_acceleration().isApprox(accerlerations)); + Eigen::VectorXd acceleration_eigen = Eigen::VectorXd::Random(6); + cs.set_acceleration(acceleration_eigen); + EXPECT_TRUE(cs.get_acceleration().isApprox(acceleration_eigen)); + std::vector acceleration{7, 8, 9, 10, 11, 12}; + cs.set_acceleration(acceleration); + for (std::size_t i = 0; i < acceleration.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_acceleration()(i), acceleration.at(i)); + } + EXPECT_THROW(cs.set_acceleration(std::vector(4)), exceptions::IncompatibleSizeException); + + // force + data = Eigen::Vector3d::Random(); + cs.set_force(data); + EXPECT_TRUE(cs.get_force().isApprox(data)); + std_data = {8, 9, 10}; + cs.set_force(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_force()(i), std_data.at(i)); + } + cs.set_force(8.1, 9.2, 10.3); + EXPECT_TRUE(Eigen::Vector3d(8.1, 9.2, 10.3).isApprox(cs.get_force())); + EXPECT_THROW(cs.set_force(std::vector(4)), exceptions::IncompatibleSizeException); + + // torque + data = Eigen::Vector3d::Random(); + cs.set_torque(data); + EXPECT_TRUE(cs.get_torque().isApprox(data)); + std_data = {9, 10, 11}; + cs.set_torque(std_data); + for (std::size_t i = 0; i < std_data.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_torque()(i), std_data.at(i)); + } + cs.set_torque(9.1, 10.2, 11.3); + EXPECT_TRUE(Eigen::Vector3d(9.1, 10.2, 11.3).isApprox(cs.get_torque())); + EXPECT_THROW(cs.set_torque(std::vector(4)), exceptions::IncompatibleSizeException); // wrench - Eigen::Vector3d force = Eigen::Vector3d::Random(); - cs.set_force(force); - EXPECT_TRUE(cs.get_force().isApprox(force)); - Eigen::Vector3d torque = Eigen::Vector3d::Random(); - cs.set_torque(torque); - EXPECT_TRUE(cs.get_torque().isApprox(torque)); - Eigen::VectorXd wrench = Eigen::VectorXd::Random(6); + Eigen::VectorXd wrench_eigen = Eigen::VectorXd::Random(6); + cs.set_wrench(wrench_eigen); + EXPECT_TRUE(cs.get_wrench().isApprox(wrench_eigen)); + std::vector wrench{10, 11, 12, 13, 14, 15}; cs.set_wrench(wrench); - EXPECT_TRUE(cs.get_wrench().isApprox(wrench)); + for (std::size_t i = 0; i < wrench.size(); ++i) { + EXPECT_FLOAT_EQ(cs.get_wrench()(i), wrench.at(i)); + } + EXPECT_THROW(cs.set_wrench(std::vector(4)), exceptions::IncompatibleSizeException); cs.set_zero(); EXPECT_FLOAT_EQ(cs.data().norm(), 1); From e3543ca9cfd176a6b9b71b89942745de70be6228 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 7 Jun 2022 11:04:21 +0200 Subject: [PATCH 13/17] Add CMake config for clproto (#292) * Add CMake config analogue to core libraries * Minor simplifications * 5.2.8 -> 5.2.9 * Update CHANGELOG --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/Dockerfile.protocol | 3 +- protocol/clproto_cpp/CMakeLists.txt | 119 +++++++++++++++++++-------- protocol/clproto_cpp/Config.cmake.in | 5 ++ python/setup.py | 2 +- source/CMakeLists.txt | 2 +- 8 files changed, 97 insertions(+), 39 deletions(-) create mode 100644 protocol/clproto_cpp/Config.cmake.in diff --git a/CHANGELOG.md b/CHANGELOG.md index ffb9e10d2..a32a823d2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ Release Versions: - Mark Parameter getters as const (#289) - Throw exception upon parameter validation in controllers (#290) - Add CartesianState attribute setters from std vector and coefficients (#291) +- Add CMake config for clproto (#292) ## 5.2.0 diff --git a/VERSION b/VERSION index 614a37bb3..879fd9751 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.8 +5.2.9 diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 260639a82..0fb847912 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.2.8 +PROJECT_NUMBER = 5.2.9 # 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/Dockerfile.protocol b/protocol/Dockerfile.protocol index a13d3336c..4b65830f0 100644 --- a/protocol/Dockerfile.protocol +++ b/protocol/Dockerfile.protocol @@ -16,8 +16,7 @@ FROM dependencies as build ARG BUILD_TESTING=ON WORKDIR /tmp/protocol/clproto_cpp/build -RUN cmake -DBUILD_TESTING="${BUILD_TESTING}" .. \ - && make -j all +RUN cmake -DBUILD_TESTING="${BUILD_TESTING}" .. && make -j all FROM build as testing diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index f9f2f588d..5182b204e 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/protocol/clproto_cpp/CMakeLists.txt @@ -1,29 +1,44 @@ -cmake_minimum_required(VERSION 3.9) -project(clproto VERSION 5.2.8) +cmake_minimum_required(VERSION 3.15) -set(CMAKE_CXX_STANDARD 17) +project(clproto VERSION 5.2.9) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) include(FindProtobuf) -find_package(Protobuf REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(control_libraries ${PROJECT_VERSION} REQUIRED state_representation) +find_package(Protobuf 3.17 REQUIRED) +find_package(control_libraries ${PROJECT_VERSION} REQUIRED COMPONENTS state_representation) if(BUILD_TESTING) - enable_testing() - find_package(GTest REQUIRED) - if (APPLE) - add_definitions(-DGTEST_USE_OWN_TR1_TUPLE) - add_definitions(-D__GLIBCXX__) - endif (APPLE) + enable_testing() + find_package(GTest REQUIRED) + if (APPLE) + add_definitions(-DGTEST_USE_OWN_TR1_TUPLE) + add_definitions(-D__GLIBCXX__) + endif (APPLE) else() - find_package(GTest QUIET) + find_package(GTest QUIET) endif() -set(PROTOBUF_DIR ${PROJECT_SOURCE_DIR}/../protobuf) +set(PROTOBUF_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../protobuf) set(PROTOBUF_BINDINGS_DIR ${PROTOBUF_DIR}/bindings/cpp) add_custom_target(generate_proto_bindings COMMAND make cpp_bindings - WORKING_DIRECTORY ${PROTOBUF_DIR} + WORKING_DIRECTORY ${PROTOBUF_DIR} ) file(GLOB_RECURSE GENERATED_PROTO_BINDINGS "${PROTOBUF_BINDINGS_DIR}/*.pb.cc" "${PROTOBUF_BINDINGS_DIR}/*.pb.h") @@ -33,29 +48,67 @@ target_include_directories(${PROJECT_NAME}_bindings PUBLIC ${PROTOBUF_BINDINGS_D set_property(TARGET ${PROJECT_NAME}_bindings PROPERTY POSITION_INDEPENDENT_CODE ON) add_library(${PROJECT_NAME} SHARED - ${PROJECT_SOURCE_DIR}/src/clproto.cpp - ${PROJECT_SOURCE_DIR}/src/decoders.cpp - ${PROJECT_SOURCE_DIR}/src/encoders.cpp + ${PROJECT_SOURCE_DIR}/src/clproto.cpp + ${PROJECT_SOURCE_DIR}/src/decoders.cpp + ${PROJECT_SOURCE_DIR}/src/encoders.cpp +) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ ) -target_include_directories(${PROJECT_NAME} PUBLIC ${PROJECT_SOURCE_DIR}/include) -set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${PROJECT_SOURCE_DIR}/include/clproto.h) +set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/include/clproto.h) target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} state_representation PRIVATE ${PROJECT_NAME}_bindings) +# install the target and create export-set install(TARGETS ${PROJECT_NAME} - PUBLIC_HEADER DESTINATION include - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib + EXPORT ${PROJECT_NAME}_targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} +) + +install(TARGETS ${PROJECT_NAME} + PUBLIC_HEADER DESTINATION include + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} +) + +# generate and install export file +install(EXPORT ${PROJECT_NAME}_targets + FILE ${PROJECT_NAME}_targets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} ) if (BUILD_TESTING) - file(GLOB_RECURSE TEST_SOURCES test/ test_*.cpp) - add_executable(test_${PROJECT_NAME} ${TEST_SOURCES}) - target_link_libraries(test_${PROJECT_NAME} - protobuf - ${PROJECT_NAME} - state_representation - ${GTEST_LIBRARIES} - pthread - ) - add_test(NAME test_${PROJECT_NAME} COMMAND test_${PROJECT_NAME}) + file(GLOB_RECURSE TEST_SOURCES test/ test_*.cpp) + add_executable(test_${PROJECT_NAME} ${TEST_SOURCES}) + target_link_libraries(test_${PROJECT_NAME} + protobuf + ${PROJECT_NAME} + ${GTEST_LIBRARIES} + pthread + ) + add_test(NAME test_${PROJECT_NAME} COMMAND test_${PROJECT_NAME}) endif () + +# generate the version file for the config file +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION "${PROJECT_VERSION}" + COMPATIBILITY SameMajorVersion +) + +# create config file +configure_package_config_file(${CMAKE_CURRENT_SOURCE_DIR}/Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +# install config files +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) diff --git a/protocol/clproto_cpp/Config.cmake.in b/protocol/clproto_cpp/Config.cmake.in new file mode 100644 index 000000000..2f7c7d738 --- /dev/null +++ b/protocol/clproto_cpp/Config.cmake.in @@ -0,0 +1,5 @@ +@PACKAGE_INIT@ + +if(NOT TARGET @PROJECT_NAME@) + include("${CMAKE_CURRENT_LIST_DIR}/clproto_targets.cmake") +endif() \ No newline at end of file diff --git a/python/setup.py b/python/setup.py index 7de141ea3..ea1d9d4b6 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.2.8" +__version__ = "5.2.9" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 76c16bede..23df45dc7 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 5.2.8) +project(control_libraries VERSION 5.2.9) # Build options option(BUILD_TESTING "Build all tests." OFF) From b30ab2539dfcff1ed66dd0473e0ec624eb8dbaa8 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 7 Jun 2022 15:41:32 +0200 Subject: [PATCH 14/17] Update and extend demos with Python examples and migrate ROS demos away (#293) * Remove ROS demos * Move control loop demos content to demos directory * Update cpp scripts and add corresponding python scripts * Finalize updated demos with CMakeLists and new Dockerfile * 5.2.9 -> 5.2.10 * Update CHANGELOG * Correct PR number --- CHANGELOG.md | 1 + VERSION | 2 +- .../CMakeLists.txt | 7 +- demos/Dockerfile | 16 ++ demos/README.md | 94 +++++++++++ demos/control_loop_examples/Dockerfile | 12 -- demos/control_loop_examples/README.md | 107 ------------- .../robot_kinematics_control_loop.cpp | 11 +- .../task_space_control_loop.cpp | 9 +- .../scripts => }/fixtures/panda_arm.urdf | 0 .../robot_kinematics_control_loop.py | 75 +++++++++ .../python_scripts/task_space_control_loop.py | 44 ++++++ demos/ros2_examples/.dockerignore | 1 - demos/ros2_examples/.gitignore | 1 - demos/ros2_examples/CMakeLists.txt | 46 ------ demos/ros2_examples/Dockerfile | 54 ------- demos/ros2_examples/README.md | 33 ---- demos/ros2_examples/config/entrypoint.sh | 10 -- demos/ros2_examples/config/update_bashrc | 7 - .../launch/task_space_control_loop.launch.py | 20 --- demos/ros2_examples/package.xml | 20 --- demos/ros2_examples/run-demo.sh | 74 --------- demos/ros2_examples/rviz/config_file.rviz | 148 ------------------ .../scripts/fixtures/panda_arm.urdf | 134 ---------------- .../scripts/task_space_control_loop.cpp | 105 ------------- demos/ros_examples/.dockerignore | 1 - demos/ros_examples/.gitignore | 1 - demos/ros_examples/CMakeLists.txt | 61 -------- demos/ros_examples/Dockerfile | 54 ------- demos/ros_examples/README.md | 63 -------- demos/ros_examples/config/entrypoint.sh | 10 -- demos/ros_examples/config/update_bashrc | 6 - .../joint_space_velocity_control.launch | 11 -- .../launch/task_space_control_loop.launch | 4 - demos/ros_examples/package.xml | 14 -- demos/ros_examples/run-demo.sh | 65 -------- demos/ros_examples/rviz/config_file.rviz | 145 ----------------- .../scripts/fixtures/panda_arm.urdf | 134 ---------------- .../scripts/joint_space_velocity_control.cpp | 117 -------------- .../scripts/task_space_control_loop.cpp | 64 -------- demos/{control_loop_examples => }/run-demo.sh | 25 ++- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/setup.py | 2 +- source/CMakeLists.txt | 2 +- 45 files changed, 254 insertions(+), 1560 deletions(-) rename demos/{control_loop_examples => }/CMakeLists.txt (79%) create mode 100644 demos/Dockerfile create mode 100644 demos/README.md delete mode 100644 demos/control_loop_examples/Dockerfile delete mode 100644 demos/control_loop_examples/README.md rename demos/{control_loop_examples/scripts => cpp_scripts}/robot_kinematics_control_loop.cpp (94%) rename demos/{control_loop_examples/scripts => cpp_scripts}/task_space_control_loop.cpp (86%) rename demos/{control_loop_examples/scripts => }/fixtures/panda_arm.urdf (100%) create mode 100644 demos/python_scripts/robot_kinematics_control_loop.py create mode 100644 demos/python_scripts/task_space_control_loop.py delete mode 100644 demos/ros2_examples/.dockerignore delete mode 100644 demos/ros2_examples/.gitignore delete mode 100644 demos/ros2_examples/CMakeLists.txt delete mode 100644 demos/ros2_examples/Dockerfile delete mode 100644 demos/ros2_examples/README.md delete mode 100644 demos/ros2_examples/config/entrypoint.sh delete mode 100644 demos/ros2_examples/config/update_bashrc delete mode 100644 demos/ros2_examples/launch/task_space_control_loop.launch.py delete mode 100644 demos/ros2_examples/package.xml delete mode 100755 demos/ros2_examples/run-demo.sh delete mode 100644 demos/ros2_examples/rviz/config_file.rviz delete mode 100644 demos/ros2_examples/scripts/fixtures/panda_arm.urdf delete mode 100644 demos/ros2_examples/scripts/task_space_control_loop.cpp delete mode 100644 demos/ros_examples/.dockerignore delete mode 100644 demos/ros_examples/.gitignore delete mode 100644 demos/ros_examples/CMakeLists.txt delete mode 100644 demos/ros_examples/Dockerfile delete mode 100644 demos/ros_examples/README.md delete mode 100644 demos/ros_examples/config/entrypoint.sh delete mode 100644 demos/ros_examples/config/update_bashrc delete mode 100644 demos/ros_examples/launch/joint_space_velocity_control.launch delete mode 100644 demos/ros_examples/launch/task_space_control_loop.launch delete mode 100644 demos/ros_examples/package.xml delete mode 100755 demos/ros_examples/run-demo.sh delete mode 100644 demos/ros_examples/rviz/config_file.rviz delete mode 100644 demos/ros_examples/scripts/fixtures/panda_arm.urdf delete mode 100644 demos/ros_examples/scripts/joint_space_velocity_control.cpp delete mode 100644 demos/ros_examples/scripts/task_space_control_loop.cpp rename demos/{control_loop_examples => }/run-demo.sh (58%) diff --git a/CHANGELOG.md b/CHANGELOG.md index a32a823d2..3ccd2f9ee 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -22,6 +22,7 @@ Release Versions: - Throw exception upon parameter validation in controllers (#290) - Add CartesianState attribute setters from std vector and coefficients (#291) - Add CMake config for clproto (#292) +- Update and extend demos with Python examples and migrate ROS demos away (#293) ## 5.2.0 diff --git a/VERSION b/VERSION index 879fd9751..2228e0ce5 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -5.2.9 +5.2.10 diff --git a/demos/control_loop_examples/CMakeLists.txt b/demos/CMakeLists.txt similarity index 79% rename from demos/control_loop_examples/CMakeLists.txt rename to demos/CMakeLists.txt index e1110fd7c..19ee77b0d 100644 --- a/demos/control_loop_examples/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -26,13 +26,10 @@ set(DEMOS_SCRIPTS ) set(FIXTURE_INSTALL_PATH /etc/fixtures/) -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/scripts/fixtures/ DESTINATION ${FIXTURE_INSTALL_PATH}) +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/fixtures/ DESTINATION ${FIXTURE_INSTALL_PATH}) foreach(SCRIPT ${DEMOS_SCRIPTS}) - add_executable(${SCRIPT} scripts/${SCRIPT}.cpp) + add_executable(${SCRIPT} cpp_scripts/${SCRIPT}.cpp) target_link_libraries(${SCRIPT} ${control_libraries_LIBRARIES}) target_compile_definitions(${SCRIPT} PRIVATE SCRIPT_FIXTURES="${FIXTURE_INSTALL_PATH}") - install(TARGETS ${SCRIPT} - RUNTIME DESTINATION bin - ) endforeach() \ No newline at end of file diff --git a/demos/Dockerfile b/demos/Dockerfile new file mode 100644 index 000000000..079a92d04 --- /dev/null +++ b/demos/Dockerfile @@ -0,0 +1,16 @@ +FROM ghcr.io/epfl-lasa/control-libraries/development-dependencies + +WORKDIR /tmp +ARG CONTROL_LIBRARIES_BRANCH=develop +RUN git clone -b ${CONTROL_LIBRARIES_BRANCH} --depth 1 https://github.com/epfl-lasa/control-libraries.git +RUN cd control-libraries/source && ./install.sh --auto +RUN cd control-libraries/protocol && ./install.sh --auto +RUN pip3 install control-libraries/python + +RUN rm -rf /tmp/* + +USER developer +WORKDIR ${HOME}/control_loop_examples +COPY ./ ./ + +RUN mkdir build && cd build && cmake .. && sudo make -j all && sudo make install diff --git a/demos/README.md b/demos/README.md new file mode 100644 index 000000000..202667304 --- /dev/null +++ b/demos/README.md @@ -0,0 +1,94 @@ +# `control_loop_examples` demonstration scripts + +## Table of contents: +* [Running a demo script](#running-demonstration-scripts) +* [Task Space control loop example](#task_space_control_loop) +* [Robot kinematics control loop example](#robot_kinematics_control_loop) + +## Running demonstration scripts +This package contains a set of demonstration scripts in both C++ and Python that showcase the functionalities introduced +in the different libraries of `control-libraries`. +The easiest way to run them is to use the `run-demo.sh` file. +Without arguments, this script creates a container and opens in interactive mode, allowing you to browse the different +demo scripts and run the one of your choice: + +```console +./run-demo-script.sh +# Run a python script +developer@xxxxxxxxx:~/control_loop_examples$ python3 python_scripts/