Motion Planning Kernel: planners/atace/PL_RRT

00001 #ifndef PL_RRT_CLOSEDCHAIN
00002 #define PL_RRT_CLOSEDCHAIN 1
00003 
00004 #include "PL_PRM_ClosedChain.h"
00005 
00006 
00007 class PL_RRT_ClosedChain: public PL_PRM_ClosedChain
00008 {
00009 public:
00010         PL_RRT_ClosedChain();
00011         ~PL_RRT_ClosedChain();
00012 
00013         void SetCollisionDetector(CD_BasicStyle* collisionDetector);
00014         virtual bool DrawExplicit () const;
00015         virtual bool Plan ();
00016 
00017 protected:
00018         vertex m_goalVert;
00019         vertex m_rootVert;
00020 
00021         /*Use Jacobian configuration generation.*/
00022 public:
00023         bool            m_bUseJacobian;
00024 private:
00025         R_OpenChain *   m_pRobot;             // to get DH parameters
00026         int             m_nDof;
00027         int             m_nToolFrame;
00028         Frame           m_frEndEffector;
00029         CJacobian       *jacobian;
00030 
00031         //********* Tree related members
00032 private:
00033         dynamic_trees m_trajTree;
00034         std::vector<vertex> m_vertices;
00035 
00036         bool CreateTree(Configuration &conf);
00037         void ClearTree();
00038         vertex AddNodeInTree(vertex parentVertex, Configuration &childConf, PA_Points &localPath);
00039         vertex FindClosestInTree(Configuration &conf);
00040         Configuration& GetConfigurationFromTree(vertex vert);
00041         PA_Points& GetPathFromTree(vertex vert);
00042         double Distance(const Configuration &conf1, const Configuration &conf2);
00043         int Extend(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &edgePath);
00044         int ExtendJacobian(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &edgePath);
00045         bool AdjustConfiguration(Configuration &config);
00046         int ConnectToGoal(vertex fromVert);
00047         int ConnectToGoalJacobian(vertex fromVert);
00048         int ConnectToGoal();
00049         int ConnectToGoal2();
00050         void CompareJacobianAndRGD();
00051         bool TestRGDConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath);
00052         bool TestJacobianConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath);
00053 
00054 private:
00055         void RetrievePath(vertex &goalVert);
00056         inline void CopyPath(PA_Points &target, PA_Points &source){target = source;};
00057         void AppendPath(PA_Points &collect, PA_Points &local);
00058         void AppendPath(PA_Points &collect, PA_Points *local);
00059         void InsertPath(PA_Points &target, PA_Points &source);
00060 };
00061 #endif
planners/atace/PL_RRT_Constrained.h
