planners/atace/PL_RGD_Constraint.h

Go to the documentation of this file.

#ifndef _HEADER_PL_RGD_CONSTRAINED_
#define _HEADER_PL_RGD_CONSTRAINED_

#include "planners/prm/PL_PRM.h"
#include <LEDA\dynamic_trees.h>

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

#define STRATEGY_RRT  0
#define STRATEGY_PRM  1

#define CONSTRAINT_COLSURE      0
#define CONSTRAINT_PLANAR       1
#define CONSTRAINT_ORIENTATION  2
#define CONSTRAINT_SPHERE       3
#define CONSTRAINT_OTHER        4

class IK_Jacobian;
class LocalPlannerClosed;

typedef list<Configuration> Fragment;
//typedef double (*CostFunction)(const Configuration &conf, const Frame &pose);

class PL_RGD_PRM: public PL_PRM
{
public:
        PL_RGD_PRM();
        ~PL_RGD_PRM();

        virtual bool Plan ();
        virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);

        bool GetSatisfiedConfiguration(Configuration &conf);
    void SetConstraintType(int type) {typeOfConstraint = type;};
        void SetUseJacobian(BOOL use) {useJacobian = (use == TRUE);};
        void SetUseGoalPose(BOOL use) {useGoalPose = (use == TRUE);}

    int GetConstraintType() {return typeOfConstraint;};
        BOOL GetUseJacobian( ) {return (useJacobian)?TRUE:FALSE;};
        BOOL GetUseGoalPose( ) {return (useGoalPose)?TRUE:FALSE;};

        //double Dist(const Configuration &p1, const Configuration &p2) const;
        double Error(const Configuration &p1) const;
        double Error1(const Configuration &p1, const Frame &pose) const;
        double Error2(const Configuration &p1, const Frame &pose) const;
        double Error3(const Configuration &p1, const Frame &pose) const;
        double Error4(const Configuration &p1, const Frame &pose) const;
        double Error5(const Configuration &p1, const Frame &pose) const;
        double Error6(const Configuration &p1, const Frame &pose) const;

        Frame GetToolFrame( const Configuration& config ) const;
        virtual bool GenerateRandomConfigForPose (Configuration& conf, Frame &endEffPose);

protected:
        // Create the Random Configurations of the C-space.
        bool GetSatisfactoryConfiguration(Configuration &conf, Frame &pose);
        virtual Configuration GenerateRandomConfig ();
        virtual Configuration GenerateRandomConfig ( const Configuration& seed, const double& std_dev );
        virtual void ConnectEdgesFull( const node& n1, const double& radius_squared );  // Connects the given node with edges, Full PRM style.
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );
        virtual bool IsInterfering ( const Configuration& c1 );
        virtual SuccessResultType TranslatePath();
        bool Plan_As_Usual ();
        void GetRotAngles( const Matrix4x4& frame, double& roll, double& pitch, double& yaw ) const;

protected:
        Fragment *edgeFrag;
        CD_BasicStyle *pCollisionDetector;
        int typeOfConstraint;
        bool useJacobian;
        bool useGoalPose;
        Frame planeDesired; //Assume the plane parallel to x-z plane.
        Frame orientDesired;
        bool MakeItSatisfied(Configuration &conf);
        //bool ConnectNodes(Configuration &q1, Configuration &q2, Fragment *frag);
        bool IsSatisfied(const Configuration &conf) const;
        void UpdatePlanarConstraint();
        void UpdateOrientConstraint();
        void UpdateShpereConstraint();

private:
        edge_map<Fragment> edgePath;
        LocalPlannerClosed* planner;
                                //Record the position of plane, y-coordinate value;
};


class PL_RGD_RRT: public PL_RGD_PRM
{
public:
        PL_RGD_RRT();
        ~PL_RGD_RRT();

        virtual bool DrawExplicit () const;
        virtual bool Plan ();

protected:
        vertex m_goalVert;
        vertex m_rootVert;

        //********* Tree related members
protected:
        dynamic_trees m_trajTree;
        std::vector<vertex> m_vertices;

        bool CreateTree(Configuration &conf);
        void ClearTree();
        vertex AddNodeInTree(vertex parentVertex, Configuration &childConf, PA_Points &localPath);
        vertex FindClosestInTree(Configuration &conf);
        Configuration& GetConfigurationFromTree(vertex vert);
        PA_Points& GetPathFromTree(vertex vert);
        double Distance(const Configuration &conf1, const Configuration &conf2);
        int Extend(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &edgePath);
        //int ConnectToGoal(vertex fromVert);
        int ConnectToGoal();

private:
        void RetrievePath(vertex &goalVert);
        inline void CopyPath(PA_Points &target, PA_Points &source){target = source;};
        void AppendPath(PA_Points &collect, PA_Points &local);
        void AppendPath(PA_Points &collect, PA_Points *local);
        void InsertPath(PA_Points &target, PA_Points &source);
};

class PL_RGD_Constraint: public PL_RGD_RRT
{
public:
        PL_RGD_Constraint();
        //~PL_RGD_Constraint();

        void SetStrategy(int strategy);
        int GetStrategy();
        virtual bool DrawExplicit () const;
        virtual bool Plan ();
private:
        int m_strategy;
};


//typedef PL_RGD_PRM PL_PRM_ClosedChain;
//typedef PL_RGD_RRT PL_PRM_ClosedChain;

#endif

---