planners/atace/PL_ATACE.h

Go to the documentation of this file.

//
//Contents: Class PL_ATACE, (used to be call PL_JPRM and PL_APACE)
// 

#ifndef _PL_ATACE_HEADER_
#define _PL_ATACE_HEADER_

//Return value for different result
#define ERR_FAIL            0xff
#define ERR_SUCCESS         0x00
#define ERR_TIMEOUT         0x10
#define ERR_ILLPARAMETER    0x20

//Flag for node in the random tree
#define FLAG_NEW            0x01 //Normal node
#define FLAG_OBSOLETE       0x02 //Node which has been deleted

//Flags for task defintion
#define TYPE_START_CONF     0x01
#define TYPE_START_POSE     0x02
#define TYPE_GOAL_CONF      0x04
#define TYPE_GOAL_POSE      0x08

//Metrics for random tree extension
#define FLAG_C_METRICS      0x10 //C-space metrics
#define FLAG_P_METRICS      0x01 //Phy-space metrics
#define FLAG_B_METRICS      (FLAG_C_METRICS|FLAG_P_METRICS) //Combined

//Different Local planners
#define LOCAL_JACOBIAN          0  //Deterministic Jacobian-based local planner
#define LOCAL_RRT                       1  //Probabilistic local planner

#pragma warning( disable : 4250 )

#include "kinematics/Configuration.h"
#include "geometry/Frame.h"
#include "Planners/PL_Boolean_Output.h"
#include "Planners/PL_OpenGL.h"
//#include "CollisionDetectors\CD_BasicStyle.h"
#include "planners/inversekin/IK_InvKinBase.h"
#include "robots/R_OpenChain.h"
#include <LEDA/dynamic_trees.h>
//#include "LocalPlanner.h"
#include "kinematics/Pose.h"

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

class TrajPlanner;

typedef PA_Points Path;
typedef vertex Vertex;
typedef std::vector<Frame> EEPath;
//typedef Frame Pose;

//Data structure for a landmark before it is put into the tree
typedef struct
{
        //int flag;
        Pose pose;
        Configuration conf;
}
Node;

//Data structure for the landmark that is in the tree
typedef struct
{
        int flag;
        bool collisionChecked;
        Pose pose;
        Configuration conf;
        Pose dirPose;
        Configuration dirConf;
        Path edge;
        EEPath eeEdge;
}
VertexInfo;

//All callback functions for constraints
typedef bool (*ProcIsStaified)(Frame &pose);
typedef void (*ProcAdjustToSatisfy)(Frame &pose);
typedef void (*ProcGetVelocity)(Frame &pose, VectorN &vel);
typedef void (*ProcGetDirectedVelocity)(Frame &pose, Frame &dir, VectorN &vel, Frame &projection, bool greedy, double timeInterval);

class PL_ATACE: public IK_InvKinBase //public PL_Boolean_Output, public PL_OpenGL
{
public:
        PL_ATACE();
        virtual ~PL_ATACE();

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

        //Member functions for task-definition
        virtual void SetStartConfig( const Configuration& config );
        virtual void SetGoalConfig ( const Configuration& config );
        void SetStartPose(const Pose &start);
        void SetGoalPose(const Pose &goal);
        void SetProblemType(unsigned int type);
        bool IsReady();       //To check whether a task is properly set.

        //Parameter functions
        //Sampling time interval when explore for end-effector path in the task space
        void SetTimeInterval(double interval){m_timeInterval = interval;}
        double GetTimeInterval(){return m_timeInterval;}
        
        //Set step size, the distance between two landmarks, in the task space.
        void SetStepSize(double step){m_stepSize = step;}
        double GetStepSize(){return m_stepSize;}
        
        //Set the metrics for random tree extension, possible choice listed in macro definition.
        void SetMetrics(int metrics){m_metrics = metrics;}
        int GetMetrics(){return m_metrics;}
        
        //Set what projection of the random tree to be renderred. C-space or phy-space
        void SetDrawSpace(bool drawCSpace){m_drawCSpace = drawCSpace;}
        bool GetDrawSpace(){return m_drawCSpace;}
        
        //Set orientation option.
        void SetOrientation(bool considered, bool always){m_useOrientation = considered; m_useOrientationAlways = always;}
        bool GetOrientation(){return m_useOrientation;}

        //Set Lazy option. Whether or not use Lazy strategy.
        void SetLazy(bool use){m_useLazy = use;}
        bool GetLazy(){return m_useLazy;}

        //Set which local planner is used
        void SetLocalPlanner(int localPlanner);
        int GetLocalPlanner(){return m_nLocalPlanner;}

protected:
        R_OpenChain *m_robot;        //Access to the planner in CollisionDetector.
        int  m_toolFrame;            //Frame ID of the toolframe in the given robot
        TrajPlanner *m_localPlanner; //Pointer to local planner
        int m_nLocalPlanner;         //Which local planner to use, (*m_localPlanner)
        int m_metrics;               //Which metric to use
        bool m_useLazy;              //Whether use Lazy strategy
        bool m_drawCSpace;           //Which projection of random tree to draw
        bool m_useOrientation;       //Whether consider orientation at goal
        bool m_useOrientationAlways; //Whether consider orienation all the time
        double m_stepSize;           //Step size between landmarks
        double m_timeInterval;       //Sampling time along the end-effector path

        Pose m_startPose;            //Start End-effector pose
        Pose m_goalPose;             //Goal End-effector Pose
        bool m_useGoalPose;          //Whether task
        bool m_useGoalConf;          
        bool m_useStartPose;
        bool m_useStartConf;
        bool m_isGoalPoseSet;
        bool m_isGoalConfSet;
        bool m_isStartPoseSet;
        bool m_isStartConfSet;

protected:
        Vertex m_goalVert;
        int ExtendWithConstraint(Node &fromNode, Pose &dirPose, Node &toNode, Path &path, EEPath &endeffpath, bool greedy);
        int ExtendToGoal(Vertex &fromVert, Pose &dirPose, Vertex &endVert);
        int LazyTrackEEPath();
        void RetrievePath(Vertex &goalVert);
        void GenerateRandomNode(Node &randomNode);
        Configuration GenerateRandomConfig();

protected:  //********* About collision detector
        Universe *m_trajUniverse;
        CD_BasicStyle *m_trajCollisionDetector;
        void CreateTrajectoryCD(CD_BasicStyle* collisionDetector);
        void DeleteTrajectoryCD();
        bool IsInterfering (const Pose& pose1, const Pose& pose2);

private:  //********* All about metrics
        VectorN distWeight;
        double Distance(const Pose &pos1, const Pose &pos2);
        double Distance(const Configuration &conf1, const Configuration &conf2);

private:  //********* Tree related members
        Vertex m_rootVert;              //Root of the tree
        dynamic_trees m_trajTree;       //Data structure for the edges
        std::vector<vertex> m_vertices; //Data structure for the nodes

        bool CreateTree(Node &rootNode);   //Create the random tree
        void ClearTree();                  //Destroy the random tree
        void TrimTreeFrom(Vertex failVert);//Delete part of a tree
        //Add a new node into the rand tree
        Vertex AddNodeInTree(Vertex parentVertex, Node &childNode, Pose &dirPose, Path &localPath, EEPath &endeffPath);
        //The following functions get the cloest neighbor, w.r.t different space
        Vertex FindClosestInTree(Pose &pose); 
        Vertex FindClosestInTree(Configuration &conf); 
        Vertex FindClosestInTree(Node node); 
        //The following functions get corresponding information of a node
        Configuration& GetConfigurationFromTree(Vertex vert);
        Pose& GetPoseFromTree(Vertex vert);
        Path& GetPathFromTree(Vertex vert);

public:  //********* About Constraints
        //The following functions set call-back functions which will be set by user.
        void SetPoseChecker(ProcIsStaified proc); 
        void SetPoseAdjuster(ProcAdjustToSatisfy proc);
        void SetVelocityChecker(ProcGetVelocity proc);
        void SetDirectedVelocityChecker(ProcGetDirectedVelocity proc);
private:
        //Function pointers to store the call-back functions
        ProcIsStaified            IsPoseSatified;
        ProcAdjustToSatisfy       AdjustPoseToSatisfy;
        ProcGetVelocity           GetVelocity;
        ProcGetDirectedVelocity   GetDirectedVelocity;
    //Compute next feasible pose, with the given call back function.
        void GetNextPose(Pose &prePose, Pose &dirPose, Pose &postPose, bool greedy);

private:  //********* About path manipulation
        inline void CopyPath(Path &target, Path &source);
        inline void CopyEEPath(EEPath &target, EEPath &source);
        void AppendPath(Path &collect, Path &local);
        void AppendPath(Path &collect, PA_Points *local);
        void InsertPath(Path &target, Path &source);
        void InterpolatePath(Path &target, Configuration &start, Configuration &end);
};

#endif

---