RnJoint.hpp

#ifndef AIKIDO_STATESPACE_DART_REALVECTORJOINTSTATESPACE_HPP_
#define AIKIDO_STATESPACE_DART_REALVECTORJOINTSTATESPACE_HPP_

#include "aikido/statespace/Rn.hpp"
#include "aikido/statespace/dart/JointStateSpace.hpp"
#include "aikido/statespace/dart/detail/RnJointTraits.hpp"

namespace aikido {
namespace statespace {
namespace dart {

template <int N>
class RJoint
  : public R<N>
  , public JointStateSpace
  , public std::enable_shared_from_this<RJoint<N>>
{
public:
  static constexpr int DimensionAtCompileTime = N;

  using typename R<DimensionAtCompileTime>::State;

  using VectorNd = typename R<DimensionAtCompileTime>::VectorNd;

  using DartJoint = typename detail::RJointTraits<N>::DartJoint;

  explicit RJoint(const DartJoint* 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;
};

using R0Joint = RJoint<0>;
using R1Joint = RJoint<1>;
using R2Joint = RJoint<2>;
using R3Joint = RJoint<3>;
using R6Joint = RJoint<6>;

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

#include "aikido/statespace/dart/detail/RnJoint-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_REALVECTORJOINTSTATESPACE_HPP_