planners/atace/PL_PRM_Constrained.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;

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

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

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


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

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

        bool GetClosedConfiguration(Configuration &conf);
        void SetUseJacobian(BOOL use) {useJacobian = (use == TRUE);};
        void SetUsePlanarConstraint(BOOL use) {usePlanarConstraint = (use == TRUE);};
        void SetUseGoalPose(BOOL use) {useGoalPose = (use == TRUE);}
        void SetUseOrientConstraint(BOOL use) {useOrientConstraint = (use == TRUE);};

        BOOL GetUseJacobian( ) {return (useJacobian)?TRUE:FALSE;};
        BOOL GetUsePlanarConstraint( ) {return (usePlanarConstraint)?TRUE:FALSE;};
        BOOL GetUseGoalPose( ) {return (useGoalPose)?TRUE:FALSE;};
        BOOL GetUseOrientConstraint( ){return (useOrientConstraint)?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;

        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) const;
        virtual Configuration GenerateRandomConfig () const;
        virtual Configuration GenerateRandomConfig ( const Configuration& seed, const double& std_dev ) const;
        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 ) const;
        virtual bool IsInterfering ( const Configuration& c1 ) const;
        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;
        bool useJacobian;
        bool useGoalPose;
        bool usePlanarConstraint;
        bool useOrientConstraint;
        Frame planeDesired; //Assume the plane parallel to x-z plane.
        Frame orientDesired;
        bool MakeItClosed(Configuration &conf) const;
        //bool ConnectNodes(Configuration &q1, Configuration &q2, Fragment *frag);
        bool IsClosed(const Configuration &conf) const;
        void UpdatePlanarConstraint();
        void UpdateOrientConstraint();

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


//typedef PL_PRM_Constraint PL_PRM_ClosedChain;
//typedef PL_RRT_Constraint PL_PRM_ClosedChain;

#endif

---