SE2Joint.hpp

#ifndef AIKIDO_STATESPACE_DART_SE2JOINTSTATESPACE_HPP_
#define AIKIDO_STATESPACE_DART_SE2JOINTSTATESPACE_HPP_

#include "aikido/statespace/SE2.hpp"
#include "aikido/statespace/dart/JointStateSpace.hpp"

namespace aikido {
namespace statespace {
namespace dart {

class SE2Joint
  : public SE2
  , public JointStateSpace
  , public std::enable_shared_from_this<SE2Joint>
{
public:
  using SE2::Isometry2d;
  using SE2::State;

  explicit SE2Joint(const ::dart::dynamics::PlanarJoint* _joint);

  // Documentation inherited.
  void convertPositionsToState(
      const Eigen::VectorXd& _positions,
      StateSpace::State* _state) const override;

  // Documentation inherited.
  void convertStateToPositions(
      const StateSpace::State* _state,
      Eigen::VectorXd& _positions) const override;
};

} // namespace dart
} // namespace statespace
} // namespace aikido

#endif // ifndef AIKIDO_STATESPACE_SE2JOINTSTATESPACE_HPP_