CollisionFree.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONSTRAINT_DART_COLLISIONFREE_HPP_
#define AIKIDO_CONSTRAINT_DART_COLLISIONFREE_HPP_
 
#include <memory>
#include <tuple>
#include <vector>
 
#include <dart/collision/CollisionDetector.hpp>
#include <dart/collision/CollisionFilter.hpp>
#include <dart/collision/CollisionGroup.hpp>
#include <dart/collision/CollisionOption.hpp>
 
#include "aikido/common/pointers.hpp"
#include "aikido/constraint/Testable.hpp"
#include "aikido/constraint/dart/CollisionFreeOutcome.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
namespace aikido {
namespace constraint {
namespace dart {
 
AIKIDO_DECLARE_POINTERS(CollisionFree)
 
class CollisionFree : public Testable
{
public:
  CollisionFree(
      statespace::dart::ConstMetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
      ::dart::dynamics::MetaSkeletonPtr _metaskeleton,
      std::shared_ptr<::dart::collision::CollisionDetector> _collisionDetector,
      ::dart::collision::CollisionOption _collisionOptions
      = ::dart::collision::CollisionOption(
          false,
          1,
          std::make_shared<::dart::collision::BodyNodeCollisionFilter>()));
 
  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;
 
  bool isSatisfied(
      const aikido::statespace::StateSpace::State* _state,
      TestableOutcome* outcome = nullptr) const override;
 
  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
  void addPairwiseCheck(
      std::shared_ptr<::dart::collision::CollisionGroup> _group1,
      std::shared_ptr<::dart::collision::CollisionGroup> _group2);
 
  void removePairwiseCheck(
      std::shared_ptr<::dart::collision::CollisionGroup> _group1,
      std::shared_ptr<::dart::collision::CollisionGroup> _group2);
 
  void addSelfCheck(std::shared_ptr<::dart::collision::CollisionGroup> _group);
 
  void removeSelfCheck(
      std::shared_ptr<::dart::collision::CollisionGroup> _group);
 
private:
  using CollisionGroup = ::dart::collision::CollisionGroup;
 
  aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
      mMetaSkeletonStateSpace;
  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
  std::shared_ptr<::dart::collision::CollisionDetector> mCollisionDetector;
  ::dart::collision::CollisionOption mCollisionOptions;
  std::vector<std::pair<
      std::shared_ptr<CollisionGroup>,
      std::shared_ptr<CollisionGroup>>>
      mGroupsToPairwiseCheck;
  std::vector<std::shared_ptr<CollisionGroup>> mGroupsToSelfCheck;
};
 
} // namespace dart
} // namespace constraint
} // namespace aikido
 
#endif // AIKIDO_CONSTRAINT_DART_COLLISIONFREE_HPP_