SE2.hpp

Go to the documentation of this file.

#ifndef AIKIDO_STATESPACE_SE2STATESPACE_HPP_
#define AIKIDO_STATESPACE_SE2STATESPACE_HPP_
#include <Eigen/Geometry>
 
#include "aikido/statespace/ScopedState.hpp"
#include "aikido/statespace/StateSpace.hpp"
 
namespace aikido {
namespace statespace {
 
template <class>
class SE2StateHandle;
 
class SE2 : public virtual StateSpace
{
public:
  class State : public StateSpace::State
  {
  public:
    using Isometry2d
        = Eigen::Transform<double, 2, Eigen::Isometry, Eigen::DontAlign>;
 
    State();
 
    ~State() = default;
 
    explicit State(const Isometry2d& _transform);
 
    void setIsometry(const Isometry2d& _transform);
 
    const Isometry2d& getIsometry() const;
 
  private:
    Isometry2d mTransform;
 
    friend class SE2;
  };
 
  using StateHandle = SE2StateHandle<State>;
  using StateHandleConst = SE2StateHandle<const State>;
 
  using ScopedState = statespace::ScopedState<StateHandle>;
  using ScopedStateConst = statespace::ScopedState<StateHandleConst>;
 
  using StateSpace::compose;
 
  using Isometry2d = State::Isometry2d;
 
  SE2() = default;
 
  ScopedState createState() const;
 
  ScopedState cloneState(const StateSpace::State* stateIn) const;
 
  const Isometry2d& getIsometry(const State* _state) const;
 
  void setIsometry(State* _state, const Isometry2d& _transform) 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* _state, Eigen::VectorXd& _tangent)
      const override;
 
  void print(const StateSpace::State* _state, std::ostream& _os) const override;
};
 
} // namespace statespace
} // namespace aikido
 
#include "detail/SE2-impl.hpp"
 
#endif // ifndef AIKIDO_STATESPACE_SE2STATESPACE_HPP_