planners/closedchain/PL_PRM_ClosedChain.h

Go to the documentation of this file.

#ifndef _HEADER_PL_PRM_CLOSED_
#define _HEADER_PL_PRM_CLOSED_

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

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

class IK_Jacobian;
class CRedundant;

typedef enum{
        ALG_CLOSEDCHAIN_RGD = 0,     //Randomized Gradient Descent
        ALG_CLOSEDCHAIN_APD,         //Active Passive Link Decomposition
        ALG_CLOSEDCHAIN_JACOBIAN,    //Jacobian
        ALG_CLOSEDCHAIN_LOCAL        //Local Jacobian-based
} ClosedChainAlgorithm;

typedef list<Configuration> Fragment;

class LocalPlannerClosed : public IK_Jacobian
{
public:
        LocalPlannerClosed(){};
        ~LocalPlannerClosed(){};
    virtual bool Plan ();

protected:
        double Distance(Configuration &frame1, Configuration &frame2);
};

class PL_PRM_ClosedBase: virtual public PL_PRM
{
public:
        PL_PRM_ClosedBase();
        ~PL_PRM_ClosedBase();

        virtual bool Plan ();
        virtual bool MakeItClosed(Configuration &conf)=0;
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 ) = 0;

        double Error(const Configuration &p1) const;
        Frame GetToolFrame( const Configuration& config ) const;

protected:
        // Create the Random Configurations of the C-space.
        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 );
        virtual SuccessResultType TranslatePath();
        void GetRotAngles( const Matrix4x4& frame, double& roll, double& pitch, double& yaw ) const;

protected:
        Fragment *edgeFrag;
        //bool ConnectNodes(Configuration &q1, Configuration &q2, Fragment *frag);
        bool IsClosed(const Configuration &conf) const;

protected:
        edge_map<Fragment> edgePath;
};

class PL_PRM_ClosedRGD: virtual public PL_PRM_ClosedBase
{
public:
        PL_PRM_ClosedRGD();
        ~PL_PRM_ClosedRGD();
        virtual bool MakeItClosed(Configuration &conf);
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );

};

class PL_PRM_ClosedLocalJacobian: virtual public PL_PRM_ClosedBase
{
public:
        PL_PRM_ClosedLocalJacobian();
        ~PL_PRM_ClosedLocalJacobian();

        virtual bool Plan ();
        virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);
        virtual bool MakeItClosed(Configuration &conf);
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );

protected:
        LocalPlannerClosed* planner;
        CD_BasicStyle *pCollisionDetector;
};

class PL_PRM_ClosedJacobian: virtual public PL_PRM_ClosedBase
{
public:
        PL_PRM_ClosedJacobian();
        ~PL_PRM_ClosedJacobian();

        virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);
        virtual bool MakeItClosed(Configuration &conf);
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );

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

};

class PL_PRM_ClosedAPDecomp: virtual public PL_PRM_ClosedBase
{
public:
        PL_PRM_ClosedAPDecomp();
        ~PL_PRM_ClosedAPDecomp();

protected:
        virtual bool MakeItClosed(Configuration &conf);
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );
        virtual void SetCollisionDetector(CD_BasicStyle* collisionDetector);

protected:
        CRedundant redundant;
};

class PL_PRM_ClosedChain: 
        virtual public PL_PRM_ClosedRGD, 
        virtual public PL_PRM_ClosedAPDecomp,
        virtual public PL_PRM_ClosedJacobian,
        virtual public PL_PRM_ClosedLocalJacobian
{
public:
        PL_PRM_ClosedChain();
        ~PL_PRM_ClosedChain();

        virtual bool Plan ();
        virtual bool MakeItClosed(Configuration &conf);
        virtual bool IsInterfering ( const Configuration& c1, const Configuration& c2 );
        virtual void SetCollisionDetector(CD_BasicStyle* collisionDetector);

        void SetAlgorithm(ClosedChainAlgorithm alg){m_nAlgorithm=alg;};
        ClosedChainAlgorithm GetAlgorithm(){return m_nAlgorithm;};
protected:
        ClosedChainAlgorithm m_nAlgorithm;
};

#endif

---