ClearanceUtilities.h

Go to the documentation of this file.

#ifndef PMPL_CLEARANCE_UTILITY_H_
#define PMPL_CLEARANCE_UTILITY_H_
 
#include "MPLibrary/MPBaseObject.h"
#include "Geometry/Bodies/Body.h"
#include "Geometry/Bodies/MultiBody.h"
//#include "MPLibrary/LocalPlanners/StraightLine.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/CDInfo.h"
#include "Utilities/MetricUtils.h"
#include "Utilities/MPUtils.h"
 
 
class Robot;
 
 
struct ClearanceStats {
  double m_avg{0};
  double m_min{0};
  double m_max{0};
  double m_var{0};
  std::vector<double> m_clearanceAlongPath;
  double m_pathLength{0};
};
 
 
template<class CfgType>
struct Ray {
  CfgType m_incr;
  CfgType m_tick;
 
  Ray(const CfgType& _i, const CfgType& _t) : m_incr(_i), m_tick(_t) {}
};
 
 
class ClearanceUtility : virtual public MPBaseObject {
 
  public:
 
 
    typedef typename MPBaseObject::WeightType     WeightType;
    typedef typename MPBaseObject::RoadmapType    RoadmapType;
    typedef typename RoadmapType::VID         VID;
 
 
    ClearanceUtility(
        string _vcLabel = "", string _dmLabel = "",
        bool _exactClearance = false, bool _exactPenetration = false,
        size_t _clearanceRays = 10, size_t _penetrationRays = 10,
        double _approxStepSize = MAX_DBL, double _approxResolution = MAX_DBL,
        bool _useBBX = true, bool _positionalDofsOnly = true, bool _debug = false);
 
    ClearanceUtility(XMLNode& _node);
 
    virtual ~ClearanceUtility() = default;
 
 
    virtual void Print(ostream& _os) const override;
 
    virtual void Initialize() override;
 
 
    const string& GetDistanceMetricLabel() const {return m_dmLabel;}
 
    const string& GetValidityCheckerLabel() const {return m_vcLabel;}
 
    void SetValidityCheckerLabel(const string& _s) {m_vcLabel = _s;}
 
    bool GetExactClearance() const {return m_exactClearance;}
 
    double GetPositionResolution() const {return m_rayTickResolution;}
 
    double GetOrientationResolution() const {return m_orientationResolution;}
 
    bool IsInitialized() const {return m_initialized;}
 
 
    bool CollisionInfo(Cfg& _cfg, Cfg& _clrCfg,
        const Boundary* const _b, CDInfo& _cdInfo,
        const bool& _useOppValidityWitness = true);
 
    bool ExactCollisionInfo(Cfg& _cfg, Cfg& _clrCfg,
        const Boundary* const _b, CDInfo& _cdInfo,
        const bool _useOppValidityWitness = true);
 
    bool ApproxCollisionInfo(Cfg& _cfg, Cfg& _clrCfg,
        const Boundary* const _b, CDInfo& _cdInfo,
        const bool& _useOppValidityWitness = true);
 
    bool FindApproximateWitness(const std::size_t& _numRays,
        std::vector<Ray<Cfg> >& _rays, const Cfg& _sampledCfg,
        const bool& _initValidity, const Boundary* const _b,
        std::pair<size_t, Cfg>& _candidate,
        const bool& _useOppValidityWitness = true);
 
    void MakeRays(const Cfg& _sampledCfg, const std::size_t& _numRays,
        std::vector<Ray<Cfg> >& _rays);
 
    bool ValidateCandidate(const std::pair<size_t, Cfg>& _cand,
        const std::vector<Ray<Cfg> >& _rays,
        const Boundary* const _b, const bool& _useOppValidityWitness = true);
 
    ClearanceStats RoadmapClearance();
 
    ClearanceStats PathClearance(vector<VID>& _path);
 
    ClearanceStats PathClearance(vector<Cfg>& _path);
 
    vector<double> EdgeClearance(const Cfg& _c1, const Cfg& _c2,
        const WeightType& _weight);
 
    const bool GetNearestVertexWitness(Cfg& _cfg, CDInfo& _cdInfo,
                                       const Boundary* const _b);
 
    void SetRobot(Robot* _robot);
 
 
  protected:
 
 
    string m_vcLabel{"cd4"};        
    string m_dmLabel{"euclidean"};  
 
    bool m_useBBX{true};            
 
    bool m_positionalDofsOnly{true};        
 
    bool m_exactClearance{false};   
    bool m_exactPenetration{false}; 
 
    size_t m_clearanceRays{10};  
    size_t m_penetrationRays{10};
 
    // These replaced two similar members from the ClearanceUtility
    // class (m_approxStepSize and m_approxResolution):
    double m_rayTickResolution;//no default values as these are required in node
    double m_orientationResolution;
 
    //These two are linearly related based on the resolution for ray ticking.
    // The max ray magnitude is just how far out a ray will be attempted.
    double m_maxRayMagnitude;
    size_t m_maxRayIterations;
 
    //These are just for getting rayTickResolution and orientationResolution.
    // They get multiplied into their respective values from the environment,
    // and should only really be used there. See Initialize() for more info.
    double m_orientationResFactor{1.}; 
    double m_positionalResFactor{1.}; 
    double m_maSearchResolutionFactor{1}; 
 
    bool m_initialized{false};
 
    Robot* m_robot;
 
 
    bool AdjustWitnessToEnsureValidity(
                                const Cfg& _cfg, const bool _initValidity,
                                Cfg& _witnessCfg, const Boundary* const _b,
                                const bool _useOppValidityWitness = true);
 
    int abcd = 0;
};
 
#endif