planners/ik_mpep/PL_MPEP.h

Go to the documentation of this file.

//
//Contents: Class PL_MPEP
// 
//PL_MPEP: Planner for Motion Planning along given End-effector Path.
//

#ifndef _PL_MPEP_HEADER_
#define _PL_MPEP_HEADER_

#define DEFAULT_SAMPLE_RATE       10
#define DEFAULT_TOLERANCE         0.1
#define DEFAULT_RANDOM_RETRY      150
#define DEFAULT_EXTEND_RETRY      50  //times the number of sample
#define DEFAULT_EXPLORE_RETRY     20
#define DEFAULT_ITERATION         10
#define DEFAULT_SMGNODE_NUM       10

//#define MAX_GREEDY_STEP_RETRY     50
//#define MAX_RRT_RETRY             10

#define ALG_GREEDY_PLANNER        0x00
#define ALG_RRT_LIKE              0x01
#define ALG_RRT_CONNECT_LIKE      0x02
#define ALG_RRT_GREEDY_LIKE       0x03
#define ALG_RRT_GREED_CONNECT     0x04
#define ALG_IMP_GREEDY_PLANNER    0x05
#define ALG_SMG_GREEDY_PLANNER    0x06
#define ALG_SMG_RRT_PLANNER       0x07
#define ALG_SMG_CONNECT_PLANNER   0x08

#define ERROR_OK                          0x00
#define ERROR_GET_RANDOM_CONF_FAILURE     0x01
#define ERROR_GET_COLLISION_FREE_FAILURE  0x02
#define ERROR_RRT_EXTEND_FAILURE          0x03
#define ERROR_START_CONFIG_INVALID        0x04
#define ERROR_GOAL_CONFIG_INVALID         0x05
#define ERROR_SMGRAPH_FULL                0x06
#define ERROR_PLANNING_TIMEOUT            0x0FE

#define ERROR_UNKNOW_PLANNER              0x0FF

#define TRAJ_LINE       0x00
#define TRAJ_SEMICIRCLE 0x01
#define TRAJ_CIRCLE     0x02

#pragma warning( disable : 4250 )

#include "Kinematics/Jacobian.h"
#include "Robots/R_OpenChain.h"
#include "geometry/Frame.h"
#include "kinematics/Configuration.h"
#include "CollisionDetectors/CD_BasicStyle.h"
#include "kinematics/FrameManager.h"
#include "Robots\RobotBase.h"
#include "../inversekin/Redundant.h"
//#include "SMGraph.h"
#include "Planners/PL_Boolean_Output.h"
#include "Planners/PL_OpenGL.h"

#include "Planners/PL_GraphBase.h"

#include <LEDA/dynamic_trees.h>
//#include <LEDA/edge_map.h>
//#include <LEDA/node_list.h>
//#include <LEDA/list.h>

#undef array
#undef vector
#undef map
#undef string
#undef set

//using namespace std;

class SMGraph;
class CRedundant;

typedef struct
{
        int poses;
        SMGraph *graph;
        Configuration conf;
}
Pose_Node;

class PL_MPEP: public PL_Boolean_Output, public PL_OpenGL
{
public:
        PL_MPEP();
        virtual ~PL_MPEP();

        //Inherited form ancestor
        virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);
        virtual bool DrawExplicit () const;
        virtual bool Plan ();
        
        Frame GetToolFrame( const Configuration& config ) const;

        //-----------The following function set the parameter from upper layer----------
        void SetTrajectory(std::vector<Frame> &traj);
        void SetPassiveNum(unsigned int num);         //The number of passive joints
        void SetAlgorithm(unsigned int alg);          //Which algorithm to use
        void SetTolerance(double dist);               //What is the tolerant diviation
        void SetUsingStartConfig(bool use);           //Whether use the specified start conf.
        void SetUsingGoalConfig(bool use);            //Whether use the specified goal configuration, when grow up goal tree
        void SetRandomRetry(unsigned int retry);      //The number of retry to get a random configuration
        void SetExtendNum(unsigned int num);          //In RRT-series planner, it's the number of extended lankmark
        void SetExploreNum(unsigned int num);         //In Greedy-series planner, it's the number of retry in Greedy planner
        void SetIterationNum(unsigned int num);       //The number of main iteration
        //How many time, they re-do from the very beginning
        void SetSMGNodeNum(unsigned int num);         //Set the node limitation in the self-motion graph
        void SetUsingJacobianExt(bool use);

protected:
        //---------The following functions just try to get configuration with certain requirement-----
        //Get a configuration in a given (random) direction
        bool GetConfigurationByDirection(Configuration &conf, Configuration &base, Configuration &direct);
        //Get a configuration purely randomly
        bool GetRandomConfiguration(Configuration &conf);
        //Get a CLOSED configuration, which satisfies the pose of end-effector specified in frame.
        bool GetRandomConfiguration(Configuration &conf, Frame &frame);
        //Get a CLOSED configuration in a certain distance with bias.
        bool GetRandomConfiguration(Configuration &conf, Frame &frame, Configuration &bias);

        //----------The subroutines for planner------
        //Used in Greedy-series
        int Greedy_Step(int &begin, int end, vertex &base);
        //Used in RRT-series
        int RRT_Extend_Like(dynamic_trees &tr, std::vector<vertex> &vert, int &extPose, vertex &extended, int dir=1);
        int Step(dynamic_trees &tr, std::vector<vertex> &vert, int begin, int end, vertex &base, int dir=1);

        bool IsJointWithinLimits(Configuration& config);
        bool AdjustConfiguration(Configuration &config, int nPose);
        int Extend_Node_Self_Motion_Graph_Jacobian(vertex g_vertex, Configuration **nextConf);
        //The following functions are used in Planners enabling self-motion graph
        //Create and explore Self-motion Graph for a certain node in Random tree
        int Establish_Self_Motion_Graph(dynamic_trees &tr, vertex extended);
        //Get valid configuration for Self_Motion Graph.
        int Extend_Node_Self_Motion_Graph(vertex g_vertex, Configuration **);
        //Check the connectivity from a node to a the random tree growing from the other direction.
        int CheckConnectivity(dynamic_trees &tr, std::vector<vertex> &vert, vertex extendedVert, vertex &connectedVert, int dir);

        //----------Algorithms for planning-----------
        int Planner_Greedy();
        int Planner_RRT_Like();
        int Planner_RRT_Connect_Like();
        int Planner_RRT_Greedy_Like();
        int Planner_RRT_Greedy_Connect();

        //Improved Greedy Planner, with backtracking
        int Planner_IMP_Greedy();
        //Improved Greedy Planner, with Self-Motion-Graph
        int Planner_SMG_Greedy();
        //Improve RRT-Connect-Like Planner, with Self-Motion-Graph
        int Planner_SMG_RRT_Connect_Like();
        //Improve RRT-Greed-Like Planner, with Self-Motion-Graph;
        int Planner_SMG_RRT_Greedy_Like();

protected:
        //------------- The following functions operate the Random tree created in RRT-series planner ---------
        void ClearTree();
        int InitializeTree();
        void RetrievePathFromTree();
        vertex ConnectNode(dynamic_trees &tr, std::vector<vertex> &vert, vertex &parent, Pose_Node &child);
        //Get the node in the tree which is closest to given configuration.
        vertex GetNearestNodeInTree(dynamic_trees &tr, std::vector<vertex> &vert, Configuration &conf);
        //Get furthest pose currently explored
        vertex GetClosestPoseVert(dynamic_trees &tr, std::vector<vertex> &vert);

        //------------- The following functions operate the Self-Motion Graph ------------
        void *GetNearestNodeInGraph(SMGraph *graph, Configuration *conf);

        //The following function computing the distance between two configurations
        double Distance(Configuration &conf1, Configuration &conf2);
        double DistanceInActiveJoints(Configuration &conf1, Configuration &conf2);

        //Just use for debug purpose when debugging the programe
        void Display_Error_Message(int result);

protected:
        R_OpenChain *   m_pRobot;             // to get DH parameters
        int             m_nDof;
        int             m_nToolFrame;
        Frame           m_frEndEffector;
        CJacobian      *m_pJacobian;

protected:
        //A class implementing closed-form inverse kinematics.
        CRedundant redundant;

        //Used when retrieving the path; Set after planning
        vertex goalPoseVertex;
        vertex goalPoseVertex2;

        //Used when retrieving the path; Set after planning
        //  The difference to the previous one is:
        //    this one is for the Random Tree growing up from GOAL;
        vertex startPoseVertex;

        //Random-tree data structure
        //  the vertex in the tree actually stored independent to the tree
        //  in LEDA, dynamic_tree just stores the relationship among the vertices.
        dynamic_trees tree;
        std::vector<vertex> vertices;

        //Random-tree growing from goal, using in RRT-CONNECT like
        //  when enabling S-M-G.
        dynamic_trees goal_tree;
        std::vector<vertex> goal_vertices;

        //Store the path, when we finish planning
        std::vector<vertex> itinerary;

        //The end effect poses
        //All sampling trajectory points are stored here.
        std::vector<Frame> poses;

        //private member corresponding to the upper layer parameter setting.
        unsigned int algorithm;
        unsigned int passiveNum;
        unsigned int randomRetry;
        unsigned int extendNum;
        unsigned int exploreNum;
        unsigned int iterationNum;
        unsigned int smgNodeNum;
        double distTolerance;
        bool usingStartConfig;
        bool usingGoalConfig;
        bool usingGoalTree;
        bool usingJacobianExt;

private:
};

#endif

---