SE3Joint.hpp

#ifndef AIKIDO_STATESPACE_DART_SE3JOINTSTATESPACE_HPP_
#define AIKIDO_STATESPACE_DART_SE3JOINTSTATESPACE_HPP_

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

namespace aikido {
namespace statespace {
namespace dart {

class SE3Joint
  : public SE3
  , public JointStateSpace
  , public std::enable_shared_from_this<SE3Joint>
{
public:
  using SE3::State;

  explicit SE3Joint(const ::dart::dynamics::FreeJoint* 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_SE3JOINTSTATESPACE_HPP_