DisplacementPlanner.h

#ifndef DISPLACEMENT_PLANNER_H
#define DISPLACEMENT_PLANNER_H

#include <KrisLibrary/Logger.h>
#include "MotionPlanner.h"
#include <KrisLibrary/utils/Subset.h>
#include <KrisLibrary/structs/IndexedPriorityQueue.h>

class ObstacleDisplacementCSpace : public CSpace 
{
public:
  ObstacleDisplacementCSpace();
  virtual ~ObstacleDisplacementCSpace() {}

  //these should be implemented by the subclass
  virtual std::shared_ptr<CSpace> DisplacementSpace(int obstacle) const { return NULL; }
  virtual bool IsFeasible(const Config& q,int obstacle,const Vector& d)=0;
  virtual bool IsFeasibleAll(const Config& q,int obstacle) { return false; }
  virtual bool IsVisibleAll(const Config& a,const Config& b,int obstacle) { return false; }

  virtual void SetDisplacement(int obstacle,const Vector& d);
  virtual void GetDisplacement(int obstacle,Vector& d) const;
  virtual void SetDisplacementRef(int obstacle,const Vector& d);
  virtual void GetDisplacementRef(int obstacle,Vector& d) const;
  bool IsDisplaceable(int obstacle) const;

  //overriding ExplicitCSpace's method
  virtual bool IsFeasible(const Config& q,int obstacle);

  //re-initializes all displacements to 0, gets the displacementSpaeces
  void InitZeroDisplacements();

  std::vector<Vector> obstacleDisplacements;
  std::vector<std::shared_ptr<CSpace> > displacementSpaces;
};

class DisplacementPlanner
{
 public:
  struct TestResult
  {
    int allFeasible;  //0 = false, 1 = true, -1 = unknown
    Subset feasible;
    Subset infeasible;
  };
  typedef std::vector<TestResult> TestResults;

  struct Milestone {
    Config q;
    TestResults tests;
  };
  struct Edge {
    EdgePlannerPtr e;
    TestResults tests;
  };
  typedef Graph::UndirectedGraph<Milestone,Edge> Roadmap;

  struct PathSearchNode
  {
    int vertex;
    std::vector<int> assignment;
    double pathLength,totalCost;
    PathSearchNode* parent;
  };

  DisplacementPlanner(ObstacleDisplacementCSpace* space);
  void Init(const Config& start,const Config& goal);
  bool Plan(int numIters,int numExpandsPerDisp,int numLocalOptimize,Real expandLimitStep,std::vector<int>& bestPath,std::vector<int>& bestDisplacements);
  void Expand(Real maxTotalCost,std::vector<int>& newNodes);
  int AddNewDisplacement(Real maxTotalCost = Inf);
  virtual std::pair<int,Real> PickObstacleToSample(Real maxTotalCost);
  virtual std::pair<int,Real> PickExploreObstacle(Real maxTotalCost);
  virtual std::pair<int,Real> PickGoalExploreObstacle(Real maxTotalCost);
  virtual std::pair<int,Real> PickRefineObstacle(Real maxTotalCost);
  virtual std::pair<int,Real> PickGoalRefineObstacle(Real maxTotalCost);
  bool IsCandidateForExploration(int i) const;
  void BuildRoadmap(Real maxExplanationCost,RoadmapPlanner& prm);

  //helpers
  double Cost(const std::vector<int>& assignment) const;

  int AddNode(const Config& q,int parent=-1);
  void AddEdge(int i,int j);
  void AddEdge(int i,int j,const TestResults& tests);
  int CheckImmovableAndAddNode(const Config& q,int parent=-1);
  int ExtendEdge(int i,const Config& q);  //returns index of q
  void KNN(const Config& q,int k,std::vector<int>& neighbors,std::vector<Real>& distances);
  void KNN(const Config& q,Real maxTotalCost,int k,std::vector<int>& neighbors,std::vector<Real>& distances);
  bool UpdateCoversIn(int nstart,Real maxTotalCost);
  void UpdateCoversOut(int nstart,Real maxTotalCost);
  std::shared_ptr<PathSearchNode> UpdateEdge(int i,int j,PathSearchNode* ni,Real maxTotalCost);
  bool CheckUpstreamConstraints(PathSearchNode* n,int constraint,int sample);
  void PropagateDownstream(PathSearchNode* n,std::vector<std::shared_ptr<PathSearchNode> >& nodes,Real maxTotalCost);
  bool FindMinimumAssignment(PathSearchNode* n,Real maxTotalCost);
  bool CheckNodeConstraint(Milestone& v,int constraint,int sample);
  bool CheckEdgeConstraint(Edge& e,int constraint,int sample);
  Real OptimalCost(int i) const;
  PathSearchNode* OptimalPathTo(int i);
  void PruneSearchNode(PathSearchNode* n,IndexedPriorityQueue<PathSearchNode*,Real>* q=NULL);
  bool Revisited(PathSearchNode* n);
  bool Dominates(PathSearchNode* a,PathSearchNode* b);
  //returns true if all of the displacements of setting b are at least as
  //costly as the corresponding displacements of assignment a
  bool ParetoDominates(const std::vector<int>& a,const std::vector<int>& b) const;

  void LocalImprovementCandidates(int i,std::vector<int>& obstacles);
  bool GenerateDisplacementSample(int obstacle, Real maxDispCost, Real maxTotalCost, int numTries);
  bool AddDisplacementSample(int obstacle,const Vector& disp);
  void AddDisplacementSampleRaw(int obstacle,const Vector& disp);
  bool RefineGoalDisplacements(int numIters,Real perturbRadiusFrac=1.0);
  bool RefineGoalPathAndDisplacements(int numIters,Real perturbRadiusFrac=1.0,Real lipschitzDisp=1.0);
  bool ShortcutGoalPath(int skip,int numIters);

  void GetPath(PathSearchNode* n,std::vector<int>& path) const;
  void GetMilestonePath(const std::vector<int>& path,MilestonePath& mpath) const;
  void GetMilestonePath(PathSearchNode* n,MilestonePath& mpath) const;

  bool SanityCheck(bool checkOneCoverGreedy=false);
 
  Config start,goal;
  ObstacleDisplacementCSpace* space;

  //minimize pathCostWeight*length(path) + sum of displacement costs
  //if initialDisplacementCosts is nonempty and displacement i is nonzero,
  //it incurs an extra cost of initialDisplacementCosts[i].
  std::vector<Real> initialDisplacementCosts;
  Real pathCostWeight; 

  //settings for RRT* like expansion
  bool dynamicDomainExpansion;
  int numConnections;
  Real connectThreshold,expandDistance,goalConnectThreshold;
  Real goalBiasProbability;  //probability of expanding toward the goal
  bool bidirectional;        //not functional yet
  
  //settings for path update
  bool updatePathsComplete;
  int updatePathsMax;

  //settings for obstacle sampling
  int obstacleSampleCount,obstacleDescendIters;

  Roadmap roadmap;
  std::vector<std::vector<Vector> > displacementSamples;
  std::vector<std::vector<Real> > displacementSampleCosts;
  std::vector<std::vector<int> > displacementSampleOrders;

  struct DynamicShortestPathNode
  {
    //store either a single cover (in case of greedy search)
    //or multiple pareto-optimal covers (in case of optimal search)
    std::vector<std::shared_ptr<PathSearchNode> > covers;
  };
  std::vector<DynamicShortestPathNode> pathCovers;

  //planning statistics
  int numExpands,numRefinementAttempts,numRefinementSuccesses,numExplorationAttempts,
    numEdgeChecks,numConfigChecks,
    numUpdateCovers,numUpdateCoversIterations;
  double timeNearestNeighbors,timeRefine,timeExplore,timeUpdateCoversIn,timeUpdateCoversOut,timeOverhead;
};

#endif