Rn.hpp

#ifndef AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_
#define AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_
#include <Eigen/Core>

#include "aikido/statespace/ScopedState.hpp"
#include "aikido/statespace/StateSpace.hpp"

namespace aikido {
namespace statespace {

// Defined in detail/Rn-impl.hpp
template <class>
class RStateHandle;

template <int N>
class R : public virtual StateSpace
{
public:
  class State : public StateSpace::State
  {
  public:
    static constexpr int DimensionAtCompileTime = N;

  protected:
    State() = default;
    ~State() = default;

    friend class R<N>;
  };

  static constexpr int DimensionAtCompileTime = N;

  using VectorNd = Eigen::Matrix<double, N, 1>;

  using StateHandle = RStateHandle<State>;
  using StateHandleConst = RStateHandle<const State>;

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

  using StateSpace::compose;

  R();

  explicit R(int dimension);

  ScopedState createState() const;

  ScopedState cloneState(const StateSpace::State* stateIn) const;

  Eigen::Map<const VectorNd> getValue(const State* _state) const;

  void setValue(State* _state, const VectorNd& _value) const;

  // Documentation inherited.
  std::size_t getStateSizeInBytes() const override;

  // Documentation inherited.
  StateSpace::State* allocateStateInBuffer(void* _buffer) const override;

  // Documentation inherited.
  void freeStateInBuffer(StateSpace::State* _state) const override;

  // Documentation inherited.
  void compose(
      const StateSpace::State* _state1,
      const StateSpace::State* _state2,
      StateSpace::State* _out) const override;

  // Documentation inherited
  void getIdentity(StateSpace::State* _out) const override;

  // Documentation inherited
  void getInverse(
      const StateSpace::State* _in, StateSpace::State* _out) const override;

  // Documentation inherited
  std::size_t getDimension() const override;

  // Documentation inherited
  void copyState(
      const StateSpace::State* _source,
      StateSpace::State* _destination) const override;

  void expMap(
      const Eigen::VectorXd& _tangent, StateSpace::State* _out) const override;

  void logMap(
      const StateSpace::State* _in, Eigen::VectorXd& _tangent) const override;

  void print(const StateSpace::State* _state, std::ostream& _os) const override;

private:
  Eigen::Map<VectorNd> getMutableValue(State* _state) const;

  int mDimension;
};

using R0 = R<0>;
using R1 = R<1>;
using R2 = R<2>;
using R3 = R<3>;
using R6 = R<6>;
using Rn = R<Eigen::Dynamic>;

} // namespace statespace
} // namespace aikido

#include "detail/Rn-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_REALVECTORSTATESPACE_HPP_