planners/fortest/PL_ForTest.h

Go to the documentation of this file.

#ifndef PL_FORTEST_h
#define PL_FORTEST_h 1

#pragma warning( disable : 4250 )

#include "Kinematics/Jacobian.h"
#include "Robots/R_OpenChain.h"
#include "geometry/Frame.h"
#include "planners/ik_mpep/PL_MPEP.h"
#include "planners/atace/LocalPlanner.h"

class PL_ForTest: public PL_MPEP
{
public:
        PL_ForTest();
        virtual ~PL_ForTest();

        //does all the planning
        virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);
        virtual bool PL_ForTest::DrawExplicit () const;
        virtual bool Plan ();

        void SetStepAhead(int nAhead){m_nStepAhead = nAhead;};
        void SetMethodUsed(int nMethod){m_bUseJacobianSMGExploration=(nMethod==0);};
        void SetSMGStepSize(int dStep){m_nSMGStepSize = dStep;};
        void SetUseBacktracking(bool bUse){m_bUseBacktracking = bUse;};
        void SetObstacleAvoidLP(bool bUse){m_bUseObstacleAvoidLP = bUse;};

protected:
        VectorN ComputeDeltaEndEff(Frame &fromPose, Frame &toPose);
        bool FindLocalPath(Configuration &start, int nStart, int nEnd, PA_Points &localPath);
        bool FindLocalPath2(Configuration &start, int nStart, int nEnd, PA_Points &localPath);
        int Establish_Self_Motion_Graph(dynamic_trees &tr, vertex extended);
        int Extend_Node_Self_Motion_Graph(vertex g_vertex, Configuration **nextConf);
        int Extend_Node_Self_Motion_Graph_Jacobian(vertex g_vertex, Configuration **nextConf);
        bool AdjustConfiguration(Configuration &config, int nPose);
        bool IsJointWithinLimits(Configuration& config);
        void RemoveNode(vertex failVert);

protected:
        void ResetStatistics();
        void LogStatistics();

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

protected:
        int             m_nStepAhead;
        bool            m_bUseJacobianSMGExploration;
        bool            m_bUseBacktracking;
        bool            m_bUseObstacleAvoidLP;
        double          m_nSMGStepSize;

protected:
        int             m_nSMGNodes;
        int             m_nSMGraphs;
};
#endif

---