JointStateSpace.hpp

#ifndef AIKIDO_STATESPACE_DART_JOINTSTATESPACE_HPP_
#define AIKIDO_STATESPACE_DART_JOINTSTATESPACE_HPP_

#include <dart/dynamics/dynamics.hpp>

#include "aikido/statespace/StateSpace.hpp"

namespace aikido {
namespace statespace {
namespace dart {

class JointStateSpace : public virtual StateSpace
{
public:
  class Properties
  {
  public:
    explicit Properties(const ::dart::dynamics::Joint* joint);

    const std::string& getName() const;

    const std::string& getType() const;

    std::size_t getNumDofs() const;

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

    bool hasPositionLimit(std::size_t index) const;

    bool isPositionLimited() 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::string mType;

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

    Eigen::VectorXd mPositionLowerLimits;

    Eigen::VectorXd mPositionUpperLimits;

    Eigen::Matrix<bool, Eigen::Dynamic, 1> mPositionHasLimits;

    Eigen::VectorXd mVelocityLowerLimits;

    Eigen::VectorXd mVelocityUpperLimits;

    // TODO: why doesn't this exist in DART??
    // Eigen::Matrix<bool, Eigen::Dynamic, 1> mVelocityHasLimits;
  };

  explicit JointStateSpace(const ::dart::dynamics::Joint* joint);

  virtual ~JointStateSpace() = default;

  const Properties& getProperties() const;

  bool isCompatible(const ::dart::dynamics::Joint* joint) const;

  void checkCompatibility(const ::dart::dynamics::Joint* joint) const;

  virtual void convertPositionsToState(
      const Eigen::VectorXd& positions, StateSpace::State* state) const = 0;

  virtual void convertStateToPositions(
      const StateSpace::State* state, Eigen::VectorXd& positions) const = 0;

  virtual void getState(
      const ::dart::dynamics::Joint* joint, StateSpace::State* state) const;

  virtual void setState(
      ::dart::dynamics::Joint* joint, const StateSpace::State* state) const;

protected:
  Properties mProperties;
};

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

#endif // ifndef AIKIDO_STATESPACE_DART_JOINTSTATESPACE_HPP_