MetaSkeletonStateSpace.hpp

#ifndef AIKIDO_STATESPACE_DART_METASKELETONSTATESPACE_HPP_
#define AIKIDO_STATESPACE_DART_METASKELETONSTATESPACE_HPP_

#include <unordered_map>

#include <dart/dynamics/dynamics.hpp>

#include "aikido/common/pair.hpp"
#include "aikido/common/pointers.hpp"
#include "aikido/statespace/CartesianProduct.hpp"
#include "aikido/statespace/dart/JointStateSpace.hpp"

namespace aikido {
namespace statespace {
namespace dart {

AIKIDO_DECLARE_POINTERS(MetaSkeletonStateSpace)

class MetaSkeletonStateSpace : public CartesianProduct
{
public:
  class Properties
  {
  public:
    explicit Properties(const ::dart::dynamics::MetaSkeleton* metaskeleton);

    const std::string& getName() const;

    std::size_t getNumJoints() const;

    std::size_t getNumDofs() const;

    const std::vector<std::string>& getDofNames() const;

    std::size_t getDofIndex(std::size_t ijoint, std::size_t ijointdof) const;

    std::size_t getDofIndex(const std::string& dofName) const;

    const Eigen::VectorXd& getPositionLowerLimits() const;

    const Eigen::VectorXd& getPositionUpperLimits() const;

    const Eigen::VectorXd& getVelocityLowerLimits() const;

    const Eigen::VectorXd& getVelocityUpperLimits() const;

    bool operator==(const Properties& otherProperties) const;

    bool operator!=(const Properties& otherProperties) const;

  protected:
    std::string mName;

    std::size_t mNumJoints;

    std::vector<std::string> mDofNames;

    std::unordered_map<
        std::pair<std::size_t, std::size_t>,
        std::size_t,
        aikido::common::PairHash>
        mIndexMap;

    Eigen::VectorXd mPositionLowerLimits;

    Eigen::VectorXd mPositionUpperLimits;

    Eigen::VectorXd mVelocityLowerLimits;

    Eigen::VectorXd mVelocityUpperLimits;
  };

  using CartesianProduct::ScopedState;
  using CartesianProduct::State;

  explicit MetaSkeletonStateSpace(
      const ::dart::dynamics::MetaSkeleton* metaskeleton);

  const Properties& getProperties() const;

  bool isCompatible(const ::dart::dynamics::MetaSkeleton* metaskeleton) const;

  void checkCompatibility(
      const ::dart::dynamics::MetaSkeleton* metaskeleton) const;

  void checkIfContained(const ::dart::dynamics::Skeleton* skeleton) const;

  template <class Space = JointStateSpace>
  std::shared_ptr<Space> getJointSpace(
      const ::dart::dynamics::MetaSkeleton* _metaskeleton,
      const ::dart::dynamics::Joint* _joint) const;

  template <class Space = JointStateSpace>
  std::shared_ptr<const Space> getJointSpace(std::size_t _index) const;

  void convertPositionsToState(
      const Eigen::VectorXd& _positions, State* _state) const;

  void convertStateToPositions(
      const State* _state, Eigen::VectorXd& _positions) const;

  void getState(
      const ::dart::dynamics::MetaSkeleton* _metaskeleton, State* _state) const;

  void setState(
      ::dart::dynamics::MetaSkeleton* _metaskeleton, const State* _state) const;

  ScopedState getScopedStateFromMetaSkeleton(
      const ::dart::dynamics::MetaSkeleton* _metaskeleton) const;

  ::dart::dynamics::MetaSkeletonPtr getControlledMetaSkeleton(
      const ::dart::dynamics::SkeletonPtr& _skeleton) const;

private:
  Properties mProperties;
};

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

#include "detail/MetaSkeletonStateSpace-impl.hpp"

#endif // ifndef AIKIDO_STATESPACE_DART_METASKELETONSTATESPACE_HPP_