StateSpace.hpp

#ifndef AIKIDO_STATESPACE_STATESPACE_HPP_
#define AIKIDO_STATESPACE_STATESPACE_HPP_

#include <memory>

#include <Eigen/Dense>

#include "aikido/common/RNG.hpp"
#include "aikido/common/pointers.hpp"
#include "aikido/statespace/ScopedState.hpp"

namespace aikido {
namespace statespace {

AIKIDO_DECLARE_POINTERS(StateSpace)

class StateSpace
{
public:
  class State;

  using StateHandle = statespace::StateHandle<StateSpace, State>;
  using StateHandleConst = statespace::StateHandle<StateSpace, const State>;

  using ScopedState = statespace::ScopedState<StateHandle>;
  using ScopedStateConst = statespace::ScopedState<StateHandleConst>;

  virtual ~StateSpace() = default;

  ScopedState createState() const;

  ScopedState cloneState(const State* stateIn) const;

  virtual State* allocateState() const;

  virtual void freeState(State* _state) const;

  virtual std::size_t getStateSizeInBytes() const = 0;

  virtual State* allocateStateInBuffer(void* _buffer) const = 0;

  virtual void freeStateInBuffer(State* _state) const = 0;

  virtual void compose(
      const State* _state1, const State* _state2, State* _out) const = 0;

  virtual void compose(State* _state1, const State* _state2) const;

  virtual void getIdentity(State* _out) const = 0;

  virtual void getInverse(const State* _state, State* _out) const = 0;

  virtual void getInverse(State* _state) const;

  virtual std::size_t getDimension() const = 0;

  virtual void copyState(
      const StateSpace::State* _source,
      StateSpace::State* _destination) const = 0;

  virtual void expMap(const Eigen::VectorXd& _tangent, State* _out) const = 0;

  virtual void logMap(const State* _in, Eigen::VectorXd& _tangent) const = 0;

  virtual void print(const State* _state, std::ostream& _os) const = 0;
};

class StateSpace::State
{
protected:
  State() = default;

  ~State() = default;
};

} // namespace statespace
} // namespace aikido

#endif // ifndef AIKIDO_STATESPACE_STATESPACE_HPP_