Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added continuous class #49

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Added continuous class #49

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1a621a1
Added continuous class
Mar 19, 2020
52ad3ec
Removed get_distance method call
Mar 27, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
84 changes: 58 additions & 26 deletions include/statespace/SE2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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<aikido::statespace::SE2>()),
m_distance_metric(aikido::distance::SE2(m_statespace)) {}
m_num_theta_vals(num_theta_vals) {}

~SE2();

Expand All @@ -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]
Expand All @@ -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
Expand All @@ -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,
Expand All @@ -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]
Expand Down Expand Up @@ -205,9 +240,6 @@ class SE2 : public virtual StateSpace {

/// Resolution of environment (mm)
const double m_resolution;

std::shared_ptr<aikido::statespace::SE2> m_statespace;
aikido::distance::SE2 m_distance_metric;
};

} // namespace statespace
Expand Down
52 changes: 31 additions & 21 deletions include/statespace/StateSpace.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
///
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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

Expand Down
3 changes: 2 additions & 1 deletion src/distance/SE2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
85 changes: 56 additions & 29 deletions src/statespace/SE2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<const ContinuousState&>(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]);
Expand All @@ -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);
Expand All @@ -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<const State&>(_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<ContinuousState*>(_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<const ContinuousState&>(_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<State*>(_discrete_state);
*discrete_state = State(position.x(), position.y(), theta);
Expand Down Expand Up @@ -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<const State&>(_source);
Expand Down