DynamicRegionRRT.h

Go to the documentation of this file.

#ifndef PPL_DYNAMIC_REGION_RRT_H_
#define PPL_DYNAMIC_REGION_RRT_H_
 
#include "BasicRRTStrategy.h"
 
#include "Geometry/Boundaries/CSpaceBoundingSphere.h"
#include "Utilities/XMLNode.h"
#include "Workspace/WorkspaceSkeleton.h"
 
class DynamicRegionRRT : virtual public BasicRRTStrategy {
  public:
 
    typedef typename MPBaseObject::WeightType   WeightType;
    typedef typename MPBaseObject::RoadmapType  RoadmapType;
    typedef typename RoadmapType::VID           VID;
 
 
    using SkeletonEdgeDescriptor   = WorkspaceSkeleton::ED;
    using SkeletonEdgeIterator     = WorkspaceSkeleton::adj_edge_iterator;
    using SkeletonVertexDescriptor = WorkspaceSkeleton::vertex_descriptor;
    using SkeletonVertexIterator   = WorkspaceSkeleton::vertex_iterator;
 
 
    struct SamplingRegion {
 
 
      SkeletonEdgeIterator edgeIterator; 
      size_t edgeIndex{0};   
      double attempts{1};    
      double successes{1};   
 
 
      SamplingRegion(const SkeletonEdgeIterator& _eit)
          : edgeIterator(_eit) {}
 
      void TrackSuccess(const size_t _success, const size_t _attempts) {
        successes *= .9;
        attempts  *= .9;
        successes += _success;
        attempts  += _attempts;
      }
 
      double GetWeight() const noexcept {
        return successes / attempts;
      }
 
      const Vector3d& GetCenter() const noexcept {
        return (*edgeIterator).property()[edgeIndex];
      }
 
      bool LastPoint() const noexcept {
        return edgeIndex == (*edgeIterator).property().size() - 1;
      }
 
      void Advance() noexcept {
        ++edgeIndex;
      }
 
      SamplingRegion& operator=(const SamplingRegion& _region) {
        if(this != &_region) {
          edgeIterator = _region.edgeIterator;
          edgeIndex = _region.edgeIndex;
          attempts = _region.attempts;
          successes = _region.successes;
        }
        return *this;
      }
 
      bool operator==(const SamplingRegion& _region) const {
        bool eit = edgeIterator == _region.edgeIterator;
        bool idx = edgeIndex == _region.edgeIndex;
        bool att = attempts == _region.attempts;
        bool succ = successes == _region.successes;
        return eit and idx and att and succ;
      }
 
    };
 
 
    DynamicRegionRRT();
 
    DynamicRegionRRT(XMLNode& _node);
 
    virtual ~DynamicRegionRRT() = default;
 
 
    virtual void Print(std::ostream& _os) const override;
 
 
  protected:
 
    virtual void Initialize() override;
 
 
    virtual Cfg SelectTarget() override;
 
    virtual std::pair<VID, bool> AddNode(const Cfg& _newCfg) override;
 
 
    Cfg Sample(SamplingRegion* _region);
 
    Cfg Sample(const Boundary* const _boundary, const std::string* _samplerLabel);
 
    const Vector3d GetVelocityBias(SamplingRegion* _region);
 
    bool IsTouching(const Cfg& _cfg, SamplingRegion& _region);
 
    CSpaceBoundingSphere MakeBoundary(const Vector3d& _v);
    
 
    void BuildSkeleton();
 
    void DirectSkeleton();
 
    const size_t SelectSamplingRegion();
 
    std::vector<double> ComputeProbabilities();
 
    void BiasVelocity(Cfg& _cfg, SamplingRegion* _region);
 
    void CheckRegionProximity(const Point3d& _p);
 
    std::vector<SamplingRegion*> 
    CreateRegions(const WorkspaceSkeleton::vertex_iterator _iter);
 
    void AdvanceRegions(const Cfg& _cfg);
 
    bool AdvanceRegionToCompletion(const Cfg& _cfg, SamplingRegion& _region);
 
 
    WorkspaceSkeleton m_originalSkeleton; 
    WorkspaceSkeleton m_skeleton;         
 
    std::string m_skeletonType{"reeb"}; 
    std::string m_decompositionLabel; 
    std::string m_scuLabel;           
 
    bool m_velocityBiasing{false};    
    double m_velocityAlignment{.1};   
 
    bool m_initialized{false};    
 
    std::pair<Point3d, Point3d> m_queryPair;
 
    std::vector<SamplingRegion> m_regions;
 
    std::unordered_map<WorkspaceSkeleton::VD, bool> m_visited;
 
    double m_regionFactor{2};
 
    double m_regionRadius{0}; 
 
    double m_explore{.5};
 
    double m_penetrationFactor{1};
 
};
 
#endif