JointStateSpace.hpp

Go to the documentation of this file.

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