From 1a621a1b43413080f616b8adb93ee3b2e047c6a0 Mon Sep 17 00:00:00 2001 From: Andressa Date: Wed, 18 Mar 2020 22:28:51 -0700 Subject: [PATCH 1/2] Added continuous class --- include/statespace/SE2.hpp | 84 ++++++++++++++++++++---------- include/statespace/StateSpace.hpp | 52 +++++++++++-------- src/statespace/SE2.cpp | 85 ++++++++++++++++++++----------- 3 files changed, 145 insertions(+), 76 deletions(-) diff --git a/include/statespace/SE2.hpp b/include/statespace/SE2.hpp index 20afc25d..10d6e7fb 100644 --- a/include/statespace/SE2.hpp +++ b/include/statespace/SE2.hpp @@ -86,6 +86,54 @@ class SE2 : public virtual StateSpace { friend class SE2; }; + class ContinuousState : public StateSpace::ContinuousState { + public: + /// Constructs identity state + ContinuousState() : x_mm(0), y_mm(0), theta_rad(0) {} + + ~ContinuousState() = default; + + /// Constructs state with given parameters + explicit ContinuousState( + const double& x, + const double& y, + const double& theta); + + /// Documentation Inherited + /// Vector in format [x, y, theta] + void from_vector(const Eigen::VectorXd& state); + + /// Documentation Inherited + /// void to_vector(const StateSpace::ContinuousState& state); + + /// Documentation Inherited + bool operator== ( + const StateSpace::ContinuousState& state) const override; + + /// Custom state hash + friend std::size_t hash_value(const ContinuousState& state) { + std::size_t seed = 0; + boost::hash_combine(seed, boost::hash_value(state.x_mm)); + boost::hash_combine(seed, boost::hash_value(state.y_mm)); + boost::hash_combine(seed, boost::hash_value(state.theta_rad)); + return seed; + } + + double X_mm() const; + double Y_mm() const; + double Theta_rad() const; + + /// Documentation Inherited + Eigen::VectorXd vector() const override; + + private: + double x_mm; + double y_mm; + double theta_rad; + + friend class SE2; + }; + /// Constructs a discretized SE2 state space /// /// \param resolution_m Resolution of the environment (mm) @@ -95,9 +143,7 @@ class SE2 : public virtual StateSpace { const double& resolution_m, const int& num_theta_vals) : \ m_resolution(resolution_m), - m_num_theta_vals(num_theta_vals), - m_statespace(std::make_shared()), - m_distance_metric(aikido::distance::SE2(m_statespace)) {} + m_num_theta_vals(num_theta_vals) {} ~SE2(); @@ -106,7 +152,7 @@ class SE2 : public virtual StateSpace { /// Documentation inherited int get_or_create_state( - const aikido::statespace::StateSpace::State& _state) override; + const StateSpace::ContinuousState& _state) override; /// Documentation inherited /// Input vector in format [x, y, theta] @@ -115,12 +161,11 @@ class SE2 : public virtual StateSpace { /// Documentation inherited void discrete_state_to_continuous( const StateSpace::State& _state, - aikido::statespace::StateSpace::State* - _continuous_state) const override; + StateSpace::ContinuousState* _continuous_state) const override; /// Documentation inherited void continuous_state_to_discrete( - const aikido::statespace::StateSpace::State& _state, + const StateSpace::ContinuousState& _state, StateSpace::State* _discrete_state) const override; /// Documentation inherited @@ -137,16 +182,6 @@ class SE2 : public virtual StateSpace { /// Documentation inherited int size() const override; - /// Documentation inherited - double get_distance( - const StateSpace::State& _state_1, - const StateSpace::State& _state_2) const override; - - /// Documentation inherited - double get_distance( - const aikido::statespace::StateSpace::State& _state_1, - const aikido::statespace::StateSpace::State& _state_2) const override; - /// Documentation inherited void copy_state( const StateSpace::State& _source, @@ -155,18 +190,18 @@ class SE2 : public virtual StateSpace { /// Documentation inherited double get_resolution() const override; - private: - /// Creates a new state and adds it to the statespace - /// - /// \return pointer to the state - StateSpace::State* create_state() override; - /// Gets normalized angle (radians) in [0, 2pi] /// /// \param theta_rad Angle (radians) /// \return normalized angle double normalize_angle_rad(const double& theta_rad) const; + private: + /// Creates a new state and adds it to the statespace + /// + /// \return pointer to the state + StateSpace::State* create_state() override; + /// Converts discrete angle to continuous (radians) /// /// \param theta Discrete angle in [0, num_theta_vals] @@ -205,9 +240,6 @@ class SE2 : public virtual StateSpace { /// Resolution of environment (mm) const double m_resolution; - - std::shared_ptr m_statespace; - aikido::distance::SE2 m_distance_metric; }; } // namespace statespace diff --git a/include/statespace/StateSpace.hpp b/include/statespace/StateSpace.hpp index b965fa16..91237ff4 100644 --- a/include/statespace/StateSpace.hpp +++ b/include/statespace/StateSpace.hpp @@ -40,6 +40,9 @@ class StateSpace { /// Base class for all discrete states class State; + /// Base class for all continuous states + class ContinuousState; + /// Checks if given (discrete) state exists in the statespace; if not, /// creates and adds the state to the statespace /// @@ -53,7 +56,7 @@ class StateSpace { /// \param _state Input state /// \return State ID virtual int get_or_create_state( - const aikido::statespace::StateSpace::State& _state) = 0; + const ContinuousState& _state) = 0; /// Checks if given state via vector representation exists in the /// statespace; if not, creates and adds the state to the statespace @@ -71,14 +74,14 @@ class StateSpace { /// \param[out] _continuous_state Output continuous state virtual void discrete_state_to_continuous( const State& _state, - aikido::statespace::StateSpace::State* _continuous_state) const = 0; + ContinuousState* _continuous_state) const = 0; /// Converts the given continuous state into a discrete state /// /// \param _state Input discrete state /// \param[out] _discrete_state Output continuous state virtual void continuous_state_to_discrete( - const aikido::statespace::StateSpace::State& _state, + const ContinuousState& _state, State* _discrete_state) const = 0; /// Gets the state ID for the given state if the state exists in the @@ -107,24 +110,6 @@ class StateSpace { /// \return Number of states virtual int size() const = 0; - /// Gets the distance between two SE2 states - /// - /// \param _state_1, _state_2 The discrete states to calculate the distance - /// between; distance metric varies based on states (assumption: states are - /// valid) - /// \return Distance between the states - virtual double get_distance( - const State& _state_1, const State& _state_2) const = 0; - - /// Gets the distance between two SE2 states - /// - /// \param _state_1, _state_2 The continuous states to calculate the - /// distance between; distance metric varies based on states - /// \return Distance between the states - virtual double get_distance( - const aikido::statespace::StateSpace::State& _state_1, - const aikido::statespace::StateSpace::State& _state_2) const = 0; - /// Copies a discrete state /// /// \param _source Input state (state to copy) @@ -188,6 +173,31 @@ class StateSpace::State { ~State() = default; }; +class StateSpace::ContinuousState { + /// Convert state to its vector representation + /// + /// \return Output state vector + virtual Eigen::VectorXd vector() const = 0; + + /// Converts state value given vector representation + /// Throws an exception if the state vector size is incorrect; allowed + /// vector size depends on derived class + /// + /// \param state State vector + virtual void from_vector(const Eigen::VectorXd& state) = 0; + + /// Converts vector representation to its continuous state value + /// virtual void to_vector(const ContinuousState& state) = 0; + + /// Equality operator + virtual bool operator== (const ContinuousState& state) const = 0; + protected: + // This is a base class that should only only be used in derived classes. + ContinuousState() = default; + + ~ContinuousState() = default; +}; + } // namespace statespace } // namespace libcozmo diff --git a/src/statespace/SE2.cpp b/src/statespace/SE2.cpp index a58836eb..f5996ffd 100644 --- a/src/statespace/SE2.cpp +++ b/src/statespace/SE2.cpp @@ -73,6 +73,49 @@ int SE2::State::Theta() const { return theta; } +SE2::ContinuousState::ContinuousState( + const double& x, const double& y, const double& theta) : \ + x_mm(x), y_mm(y), theta_rad(theta) {} + +bool SE2::ContinuousState::operator== ( + const StateSpace::ContinuousState& state) const { + auto state_ = static_cast(state); + return x_mm == state_.x_mm && + y_mm == state_.y_mm && + theta_rad == state_.theta_rad; +} + +Eigen::VectorXd SE2::ContinuousState::vector() const { + Eigen::VectorXd state_vector(3); + state_vector << x_mm, y_mm, theta_rad; + return state_vector; +} + +void SE2::ContinuousState::from_vector(const Eigen::VectorXd& state) { + if (state.size() != 3) { + std::stringstream msg; + msg << "state has incorrect size: expected 3" + << ", got " << state.size() << ".\n"; + throw std::runtime_error(msg.str()); + } + + x_mm = state[0]; + y_mm = state[1]; + theta_rad = state[2]; +} + +double SE2::ContinuousState::X_mm() const { + return x_mm; +} + +double SE2::ContinuousState::Y_mm() const { + return y_mm; +} + +double SE2::ContinuousState::Theta_rad() const { + return theta_rad; +} + SE2::~SE2() { for (int i = 0; i < m_state_map.size(); ++i) { delete(m_state_map[i]); @@ -94,7 +137,7 @@ int SE2::get_or_create_state(const StateSpace::State& _state) { } int SE2::get_or_create_state( - const aikido::statespace::StateSpace::State& _state) { + const StateSpace::ContinuousState& _state) { State discrete_state; continuous_state_to_discrete(_state, &discrete_state); return get_or_create_state(discrete_state); @@ -114,26 +157,26 @@ int SE2::get_or_create_state( void SE2::discrete_state_to_continuous( const StateSpace::State& _state, - aikido::statespace::StateSpace::State* _continuous_state) const { + StateSpace::ContinuousState* _continuous_state) const { const State state = static_cast(_state); - Eigen::VectorXd state_log(3); - state_log.head<2>() = + const Eigen::Vector2d position = discrete_position_to_continuous(Eigen::Vector2i(state.x, state.y)); - state_log[2] = - discrete_angle_to_continuous(state.theta); - m_statespace->expMap(state_log, _continuous_state); + const double theta = discrete_angle_to_continuous(state.theta); + + ContinuousState* continuous_state = + static_cast(_continuous_state); + *continuous_state = ContinuousState(position.x(), position.y(), theta); } void SE2::continuous_state_to_discrete( - const aikido::statespace::StateSpace::State& _state, + const StateSpace::ContinuousState& _state, StateSpace::State* _discrete_state) const { - Eigen::VectorXd log_state; - m_statespace->logMap(&_state, log_state); - + const ContinuousState state = static_cast(_state); const Eigen::Vector2i position = - continuous_position_to_discrete(log_state.head<2>()); - const int theta = continuous_angle_to_discrete(log_state[2]); + continuous_position_to_discrete( + Eigen::Vector2d(state.x_mm, state.y_mm)); + const int theta = continuous_angle_to_discrete(state.theta_rad); State* discrete_state = static_cast(_discrete_state); *discrete_state = State(position.x(), position.y(), theta); @@ -168,22 +211,6 @@ int SE2::size() const { return m_state_map.size(); } -double SE2::get_distance( - const StateSpace::State& _state_1, - const StateSpace::State& _state_2) const { - aikido::statespace::SE2::State continuous_state_1; - discrete_state_to_continuous(_state_1, &continuous_state_1); - aikido::statespace::SE2::State continuous_state_2; - discrete_state_to_continuous(_state_2, &continuous_state_2); - return m_distance_metric.distance(&continuous_state_1, &continuous_state_2); -} - -double SE2::get_distance( - const aikido::statespace::StateSpace::State& _state_1, - const aikido::statespace::StateSpace::State& _state_2) const { - return m_distance_metric.distance(&_state_1, &_state_2); -} - void SE2::copy_state( const StateSpace::State& _source, StateSpace::State* _destination) const { const State& source = static_cast(_source); From 52ad3ec74124d731904dcda920e3a641d55693ba Mon Sep 17 00:00:00 2001 From: Andressa Date: Thu, 26 Mar 2020 22:01:32 -0700 Subject: [PATCH 2/2] Removed get_distance method call --- src/distance/SE2.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/distance/SE2.cpp b/src/distance/SE2.cpp index e6c60e1b..2aa9d6ca 100644 --- a/src/distance/SE2.cpp +++ b/src/distance/SE2.cpp @@ -42,7 +42,8 @@ namespace distance { double SE2::get_distance( const statespace::StateSpace::State& _state_1, const statespace::StateSpace::State& _state_2) const { - return m_statespace->get_distance(_state_1, _state_2); + // return m_statespace->get_distance(_state_1, _state_2); + return 0.0; } } // namespace distance