WrenchAccessibilityTool.h

Go to the documentation of this file.

#ifndef PMPL_WRENCH_ACCESSIBILITY_TOOL_H_
#define PMPL_WRENCH_ACCESSIBILITY_TOOL_H_
 
#include "MPLibrary/MPBaseObject.h"
#include "MPLibrary/ValidityCheckers/CollisionDetectionValidityMethod.h"
#include "Utilities/MPUtils.h"
#include <iostream>
 
class WrenchAccessibilityTool : public MPBaseObject {
 
  public:
 
 
      typedef typename MPBaseObject::RoadmapType        RoadmapType;
      typedef typename RoadmapType::VID                 VID;
 
 
 
 
 
    WrenchAccessibilityTool();
 
    WrenchAccessibilityTool(XMLNode& _node);
 
    virtual ~WrenchAccessibilityTool() = default;
 
 
    void Initialize();
 
    void ComputeWrenchAccessibility();
 
 
 
 
 
  private:
 
    void ReadDisks(const string& _filename);
 
    Vector3d SampleInDisk(double _r);
 
 
    static double Angle(Vector3d& _v);
 
 
    static bool AngleCompare(Vector3d& _v1, Vector3d& _v2);
 
    double FindLargestFreeAngle(vector<Vector3d>& _v, size_t _idx);
    
 
 
    vector<Transformation>* m_disks;                  //< The set of disks
    vector<vector<size_t>*>* m_obstIdxs;              //< The subsets of obstacles to check for collisions
 
    double m_radius{1};                               //< The radius of the wrench for which
                                                      //< Accessibility is computed
 
    size_t m_numSamples{30};                          //< The number of samples for each disk
 
    CollisionDetectionValidityMethod* m_cd; //< Collision detection
 
    string m_inputFile;                                //< Path to the file containing the disks
    string m_outputFile{"Disks.txt"};                               //< Path to the output file (will be created if doesnt exist)
 
    ofstream m_outputStream;                    //< Output stream
 
 
};
 
#endif