FrameTestable.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONSTRAINT_DART_FRAMETESTABLE_HPP_
#define AIKIDO_CONSTRAINT_DART_FRAMETESTABLE_HPP_
 
#include <dart/dynamics/dynamics.hpp>
 
#include "aikido/constraint/Testable.hpp"
#include "aikido/statespace/SE3.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
namespace aikido {
namespace constraint {
namespace dart {
 
class FrameTestable : public Testable
{
public:
  FrameTestable(
      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
      ::dart::dynamics::MetaSkeletonPtr _metaskeleton,
      ::dart::dynamics::ConstJacobianNodePtr _frame,
      TestablePtr _poseConstraint);
 
  bool isSatisfied(
      const statespace::StateSpace::State* _state,
      TestableOutcome* outcome = nullptr) const override;
 
  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
  // Documentation inherited
  statespace::ConstStateSpacePtr getStateSpace() const override;
 
private:
  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
  ::dart::dynamics::ConstJacobianNodePtr mFrame;
  TestablePtr mPoseConstraint;
  std::shared_ptr<const statespace::SE3> mPoseStateSpace;
};
 
} // namespace dart
} // namespace constraint
} // namespace aikido
 
#endif // AIKIDO_CONSTRAINT_DART_FRAMETESTABLE_HPP_