libmove3d-planners
 All Classes Namespaces Files Functions Variables Enumerations Enumerator Friends Macros Groups Pages
Public Member Functions | Public Attributes | List of all members
GridGraph Class Reference
Inheritance diagram for GridGraph:
Graph

Public Member Functions

 GridGraph (double length, std::vector< double > orig, std::vector< double > end, std::vector< uint > degrees, p3d_graph *g, Robot *r, LocalpathFactory *pathFactory, SamplingAPI *sampler, double maxDist=P3D_HUGE)
 
 GridGraph (const char *filename, p3d_graph *g, Robot *r, LocalpathFactory *pathFactory, SamplingAPI *sampler, double maxDist=P3D_HUGE)
 
void initSampler ()
 
void writeGrid (const char *filename)
 
bool withinBounds (std::vector< double > point)
 
int coords_to_n (Configuration &q)
 
std::vector< double > n_to_coords (int n)
 
void sampleGrid (int tries)
 
int markCubeFree (Configuration &q)
 
void sampleUnderThreshold (std::vector< Node * > &newNodes, double max_neighbours, int max_tries, uint max_nodes, int tries)
 
void sampleIso (std::vector< Node * > &newNodes, uint n_per_cube, int tries)
 
bool terminationCube (Cube &c, double max_neighbours, int max_tries, uint max_nodes)
 
bool terminationCondition (double max_neighbours, int max_tries, uint max_nodes)
 
NodeinsertNode (shared_ptr< Configuration > q)
 
- Public Member Functions inherited from Graph
 Graph (Robot *R, graph *ptGraph)
 
 Graph (Robot *R)
 
 Graph (graph *G)
 
 Graph (const Graph &G)
 Graph copy constructor : be sure that the connected componants are updated. More...
 
bool isGraphChanged ()
 return the value of the Change flag More...
 
graph * getGraphStruct () const
 Creates a p3d_graph from Boost Graph. More...
 
void setGraph (graph *G)
 stocke la structure p3d_graph More...
 
RobotgetRobot () const
 obtient le Robot pour lequel le Graph est créé More...
 
void setRobot (Robot *R)
 sets the Robot of the Graph
 
std::map< node *, Node * > getNodesTable ()
 obtient la table des noeuds du Graph More...
 
unsigned int getNumberOfNodes ()
 obtient le nombre de Node dans le Graph More...
 
std::vector< Node * > getNodes () const
 obtient le vecteur des Node du Graph More...
 
std::vector< Node * > getSortedNodes () const
 
std::vector< Node * > getNotTrajNodes () const
 
NodegetNode (unsigned int i) const
 Get Node ith node in Graph.
 
unsigned int getNumberOfEdges () const
 obtient le nombre de Node dans le Graph More...
 
std::vector< Edge * > getEdges () const
 obtient le vecteur des Edge du Graph More...
 
std::vector< Edge * > getEdges (std::tr1::shared_ptr< Configuration > q, bool from=false)
 
std::vector< Edge * > getEdges (Node *nq, bool from=false)
 
EdgegetEdge (unsigned int i) const
 Returns the ith edge.
 
unsigned int getNumberOfCompco () const
 
ConnectedComponentgetConnectedComponent (int ith) const
 Get Compco. More...
 
std::vector< ConnectedComponent * > getConnectedComponents () const
 Get Compcos.
 
NodegetNode (node *N)
 obtient le Node correspondant au p3d_node More...
 
NodegetLastNode ()
 obtient le dernier Node ajouté au Graph More...
 
std::string getName ()
 obtient le nom du Graph More...
 
void setName ()
 modifie le nom du Graph; un nom standard est alors mis
 
void setName (std::string Name)
 modifie le nom du Graph More...
 
bool equal (Graph *G)
 teste si deux Graph sont égaux en comparant leur listNode More...
 
bool equalName (Graph *G)
 teste si deux Graph ont le même nom More...
 
bool isInGraph (Node *N)
 vérifie si un node est dans le Graph More...
 
EdgeisEdgeInGraph (Node *N1, Node *N2)
 Looks if two Node make an edge in the Graph. More...
 
NodesearchConf (Configuration &q)
 Gets the node that corresponds to a configuration. More...
 
void createCompco (Node *node)
 New Compco. More...
 
NodeinsertNode (Node *node)
 insert un Node dans le Graph More...
 
NodeinsertExtremalNode (Node *node)
 insert un Node de type ISOLATE More...
 
void addNode (Node *node)
 Add the given node to the storing structure of the graph (without creating any link).
 
NodeinsertNode (Node *expansionNode, LocalPath &path)
 Link a Configuration to a Node for the RRT Planner. More...
 
NodeinsertConfigurationAsNode (std::tr1::shared_ptr< Configuration > q, Node *from, double step)
 Link a Configuration to a Node for the RRT Planner. More...
 
void sortEdges ()
 trie les Edges en fonction de leur longueur More...
 
void updateSortedNodes (Node *node)
 trie les Nodes en fonction de leur distance à la position du robot via la trajectoire courante
 
NodegetRandomNodeNearTraj ()
 
void updateSepareNodes (Node *n)
 
void separateNodes ()
 
std::map< double, Node * > getNodesOnTraj (Node *near, bool neighbour=false)
 
std::vector< Node * > getKNodesOnTraj (Node *near, uint k)
 
void sortNodes (double init=-1)
 
EdgeaddEdge (Node *source, Node *target, bool compute_length=true, double length=0., bool compute_cost=true, double cost=0.)
 ajoute un Edge au Graph More...
 
EdgeaddEdge (Node *source, Node *target, localPathPtr_t path)
 Add an edge to the graph, going from the given source to the given target. More...
 
void addEdges (Node *N1, Node *N2, bool computeLength=true, double length=0., bool computeCost=true, double cost=0.)
 ajoute deux Edge au Graph More...
 
void addEdges (Node *N1, Node *N2, localPathPtr_t path)
 Add two opposite edges between the nodes N1 and N2. More...
 
void addOrientedEdge (Node *N1, Node *N2, localPathPtr_t path)
 Add the oriented edge N1->N2. More...
 
void removeEdge (Edge *E)
 Remove edge. More...
 
void removeEdges (Node *N1, Node *N2)
 Remove the edges between the nodes N1 and N2. More...
 
void sortNodesByDist (Node *N)
 Sort nodes against one given node. More...
 
void sortNodesByDist (confPtr_t q)
 Sort nodes against one given configuration. More...
 
void addNode (Node *N, double maxDist)
 ajoute un Node s'il est à moins d'une distance max du Graph More...
 
void addNodes (std::vector< Node * > N, double maxDist)
 ajoute des Node s'ils sont à moins d'une distance max du Graph More...
 
void removeNode (Node *node)
 Remove the given node from the graph.
 
bool linkNode (Node *N)
 lie un Node au Graph More...
 
bool linkNodeCompMultisol (Node *N, ConnectedComponent *compPt)
 Links a node to a compco with multisol. More...
 
bool linkNodeWithoutDist (Node *N)
 Lie un node au Graph sans prendre en compte la distance max. More...
 
bool linkNodeAtDist (Node *N)
 Lie un Node au Graph en prenant en compte la distance max. More...
 
bool linkToAllNodes (Node *N)
 lie un Node à tous les autres Nodes visibles More...
 
bool areNodesLinked (Node *node1, Node *node2, double &dist)
 Function that computes if a node is linked to another node. More...
 
bool linkNodeAndMerge (Node *node1, Node *node2)
 Link 2 nodes and merge them. More...
 
void createRandConfs (int NMAX)
 crée des Node à des Configurations aléatoires More...
 
NoderandomNodeFromComp (Node *comp)
 Gets Random node in the connected component. More...
 
bool connectNodeToCompco (Node *node, Node *compco)
 Function that trys to connect a node to a given connected component. More...
 
std::vector< Node * > KNearestWeightNeighbour (confPtr_t q, int K, double radius, bool weighted, int distConfigChoice)
 K Nearest Weighted Neighbors in graph. More...
 
std::vector< Node * > kNearestNeighbours (unsigned k, confPtr_t conf)
 
std::vector< Node * > neighborsInBall (confPtr_t conf, double radius)
 Return the nodes whose configurations are within a ball (in the C-space) having the given radius and centered on the given configuration.
 
NodenearestWeightNeighbour (Node *compco, confPtr_t C, bool weighted, int distConfigChoice)
 obtient le plus proche voisin d'une composante connexe More...
 
NodegetCompco (unsigned int ith)
 Get Ith Compco. More...
 
std::vector< Node * > getNodesInTheCompCo (Node *node)
 Vector of nodes in the same compco. More...
 
void addCycles (Node *node, double step)
 ajoute des Edges formant des cycles dans le Graph More...
 
void mergeComponents (Node *N1, Node *N2)
 Merge the connected components to which N1 and N2 belong.
 
int mergeComp (Node *CompCo1, Node *CompCo2, double DistNodes, bool compute_edge_cost)
 merge deux composantes connexes More...
 
int rebuildCompcoFromBoostGraph ()
 Rebuild the connected component using the strongly connected component algo from the BGL.
 
void mergeCheck ()
 teste si des composantes connexes doivent être merger et le fait le cas échéant More...
 
void removeCompco (ConnectedComponent *CompCo)
 Delete the compco. More...
 
bool checkConnectedComponents ()
 Check C and C++ Connected Components.
 
void recomputeCost ()
 Recompute the Graph cost (Edges and Nodes) More...
 
bool checkAllEdgesValid ()
 Recompute All Edges Valid. More...
 
API::TrajectoryextractBestTrajSoFar (confPtr_t qi, confPtr_t qf)
 Extract best traj from qi that is closest to q_f.
 
std::pair< bool, std::vector
< Node * > > 
extractBestNodePathSoFar (confPtr_t qi, confPtr_t qf)
 Extract best vector of nodes from qi that is closest to q_f. More...
 
API::TrajectoryextractTreeShortestPath (Node *initN, Node *endN)
 Return a trajectory (i.e. the shortest path) from initN to endN, using parenthood in a tree. More...
 
std::vector< double > computeDijkstraShortestPaths (Node *n)
 Compute the lengths of all the shortest paths having for source the node n, using Dijkstra.
 
std::vector< double > computeDijkstraShortestPaths (Node *n, std::map< double, Node * > to)
 
std::vector< Node * > extractAStarShortestNodePaths (Node *initN, Node *endN)
 Return the shortest path (as a list of nodes) from initN to endN, using A*.
 
std::vector< Node * > extractAStarShortestNodePaths (std::vector< Node * > passingBy)
 
std::vector< Node * > extractAStarShortestNodePaths (confPtr_t initC, confPtr_t endC)
 Return the shortest path (as a list of nodes) from initC to endC, using A*.
 
API::TrajectoryextractAStarShortestPath (Node *initN, Node *endN)
 Return a trajectory (i.e. the shortest path) from initN to endN, using A*. More...
 
API::TrajectoryextractAStarShortestPath (confPtr_t initC, confPtr_t endC)
 Return a trajectory (i.e. the shortest path) from initC to endC, using A*. More...
 
double getAStarShortestPathLength (confPtr_t initC, confPtr_t endC)
 Return the lenght of the shortest path (using A*) in the graph between initC and endC Return P3D_HUDGE if no such path exists.
 
API::TrajectoryextractBestTraj (confPtr_t qi, confPtr_t qf)
 Extract best traj.
 
void initMotionPlanning (node *start, node *goal)
 Init Motion planning problem. More...
 
BGL_Vertex findVertexDescriptor (Node *N)
 
BGL_Edge findEdgeDescriptor (Edge *E)
 
void saveBGLGraphToDotFile (const std::string &filename)
 
BGL_Graph & get_BGL_Graph ()
 
void draw ()
 
void resetConnections ()
 
void unflagAll ()
 
unsigned int getNbScc ()
 
std::vector< std::vector< Node * > > getStronglyConnectedComponents ()
 
std::vector< double > sortSCC (unsigned int id, Node *N1, bool forward)
 
void addScc (Node *node)
 Adds the singleton {node} to the list of the strongly connected components. More...
 
bool areSccConnected (unsigned int i, unsigned int j)
 Returns true if in the transitive closure of the graph of the strongly connected components, the strongly connected component (scc) of index i is connected to the scc j.
 
void connectScc (unsigned int i, unsigned int j)
 Adds an edge in the transitive closure of the graph of the strongly connected components from strongly connected component (scc) of index i to connected to the scc j. More...
 
bool mergeScc (unsigned int i, unsigned int j)
 Merges strongly connected component (scc) of index i with scc of idex j. More...
 

Public Attributes

double mLength
 
std::vector< double > mOrig
 
std::vector< double > mEnd
 
std::vector< uint > mDegrees
 
std::vector< uint > mFactor
 
std::vector< uint > mCumulFactor
 
uint mDim
 
std::vector< Cube * > mCubes
 
std::vector< bool > mFreeSpace
 
uint mFreeSpaceN
 
uint mCubesN
 
double mMaxDist
 

Additional Inherited Members

- Static Public Member Functions inherited from Graph
static void sortNodesByDist (std::vector< Node * > &nodes, confPtr_t config)
 Compute the K nearest nodes. More...
 

The documentation for this class was generated from the following file: