MetaSkeletonStateSpace.hpp

Go to the documentation of this file.

#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_