planners/sequential/PL_Sequential.h

Go to the documentation of this file.

//## begin module%3752D78501D0.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%3752D78501D0.cm

//## begin module%3752D78501D0.cp preserve=no
//## end module%3752D78501D0.cp

//## Module: PL_Sequential%3752D78501D0; Pseudo Package specification
//## Source file: E:\mpk\code\Planners\PL_Sequential.h

#ifndef PL_Sequential_h
#define PL_Sequential_h 1

//## begin module%3752D78501D0.additionalIncludes preserve=no
//## end module%3752D78501D0.additionalIncludes

#pragma warning( push )
#pragma warning( disable : 4250 )

//## begin module%3752D78501D0.includes preserve=yes
//#include <stdafx.h>   //IMPROVE: this makes MFC happy, and now the order of includes is unimportant
#include <stdio.h>
#include <math.h>
#include "Planners\sequential\sp3dstct.h"
#include "Planners\sequential\spfiles.h"

#ifdef PC_PROTO
#include <planners\output\o_bitmap.h>
#endif
//## end module%3752D78501D0.includes

// PL_Boolean_Output
#include "Planners\PL_Boolean_Output.h"
#include "Planners\PL_OpenGL.h"
//## begin module%3752D78501D0.additionalDeclarations preserve=yes
//## end module%3752D78501D0.additionalDeclarations


//## begin PL_Sequential%3752D78501D0.preface preserve=yes
//## end PL_Sequential%3752D78501D0.preface

//## Class: PL_Sequential%3752D78501D0
//      improve: both this and PL_Linear should derive from an
//      intermediate class
//## Category: Planners%36FB140B003C
//## Persistence: Transient
//## Cardinality/Multiplicity: n

class PL_Sequential 
: 
        public PL_Boolean_Output,  //## Inherits: <unnamed>%376A932700BA
        public PL_OpenGL
{
  //## begin PL_Sequential%3752D78501D0.initialDeclarations preserve=yes
  //## end PL_Sequential%3752D78501D0.initialDeclarations

  public:
    //## Destructor (generated)
      virtual ~PL_Sequential();

          //draws the planner to the output window
          virtual bool DrawExplicit () const;

      //## Operation: Plan%928173143
      virtual bool Plan ();

      //## Operation: initialize_parameters%930592279
      void initialize_parameters ();

      //## Operation: SetCollisionDetector%933360621
      virtual void SetCollisionDetector (CD_BasicStyle* collisionDetector);

    // Additional Public Declarations
      //## begin PL_Sequential%3752D78501D0.public preserve=yes
      //## end PL_Sequential%3752D78501D0.public
  protected:
    // Additional Protected Declarations
      //## begin PL_Sequential%3752D78501D0.protected preserve=yes
      //## end PL_Sequential%3752D78501D0.protected

  private:
    // Additional Private Declarations
      //## begin PL_Sequential%3752D78501D0.private preserve=yes
          //FUNCTIONS
        void convert_angle_to_index() ;
        int rad_to_index(double angle) ;
        double index_to_rad( int index ) ;
        void allocate_space_for_link_cs(int L) ;
        void allocate_space_for_working_tq_path() ;
        void allocate_space_for_Link_tq_path(int link1, int length);
        void free_space_for_link_cs(int L);
        int plan_path_for_first_link() ;
        int plan_path_for_nth_link(     int lnkno, 
                                TQLinkCSpace *ptqlink, 
                                TQConfig *pblock,
                                TQConfig *pblock_last, 
                                int *bl, 
                                int *BT_Flag, 
                                int *block_flag,
                                int *narrow_flag);
        char **malloc2D ( int dy , int dz , int oc );
        void retrieve_global_path(double **globalpath, int pathlen,
                           double *startdof, double *goaldof);
        void write_map(char *flnm, FileType filetype,
                char **map, int dimx, int dimy, int bitsize);
        void write_global_path(char *flnm, int length, int degree, double **gpath);
        int smoothpath(int nb_degree, int intervals, double **dofpath, int *pathlen);
        int compute_a_column_for_link_cspace(TQLinkCSpace       *tqlcs,
                                                                                 int                    lnkno, 
                                                                                 int                    col, 
                                                                                 TQConfig               *para);
        void write_linkpath(char *flnm, FileType filetype,
                    char *map, int dim, int bitsize);
        void write_linkpathtxt(char *flnm,
                    TQConfig *map, int dim);
        void write_tqif(char *flnm, TQConfig tqi, TQConfig tqf);
        int compute_cspace_obstacle_map(int lnkno);
        void modify_cspace_obstacle_map(TQLinkCSpace    *ptqlink, 
                                TQConfig        *tqi,
                                TQConfig        *tqf, 
                                TQConfig        *pblock,
                int             len, 
                                int             L1, 
                                int             *BT_FLAG, 
                                int             *narrow_flag);
        int find_main_path(     
                        int                             lnkno, 
                                                IntPoint                *tqinitial, 
                                                IntPoint                *tqfinal, 
                                                TQLinkCSpace    *ptqlink, 
                                                LinkPath                *result, 
                                                int                             *narrow_flag);
        void find_forward_extension(    int             lnkno, 
                                                                IntPoint        *tqinitial, 
                                                                IntPoint        *tqfinal, 
                                                                TQLinkCSpace    *ptqlink, 
                                                                LinkPath        *result);
        void find_backward_extension(   int                     lnkno, 
                                                                IntPoint        *tqinitial, 
                                                                IntPoint        *tqfinal, 
                                                                TQLinkCSpace    *ptqlink, 
                                                                LinkPath        *result);
        void combine_three_paths(       LinkPath                *combpath, 
                                                        LinkPath                *mainpath, 
                                                        LinkPath                *frwdpath, 
                                                        LinkPath                *bkwdpath,
                                                        int                             *iposition, 
                                                        int                             *fposition);
        void reparameterize_path(       int             bound,
                                                                        LinkPath        *whole_path, 
                                                                        LinkPath        *result, 
                                                                        int             *ipos, 
                                                                        int             *fpos, 
                                                                        int             L1);
        int line_free(double **dofpath, int intervals, int start, int end,
                double  **tmppath, int *path_len);
        int     shift_dofpath(double            **dofpath, 
                                  int                   pathlen, 
                                  double                **gpath, 
                                  int                   npathlen, 
                                  int                   pt1, 
                                  int                   pt2);
        void free2D(void **pp);
        void distance2d( unsigned char **in, int dimx, int dimy, short int **V, unsigned char **voro);
        void voro_heuristic2d(unsigned char             **in, 
                                          int                           dimx, 
                                          int                           dimy, 
                                          IntPoint                      goal, 
                                          short int                     **V, 
                                          unsigned char         **voro, 
                                          short int                     **heurpot, 
                                          int                           *flag);
        void find_path_by_heuristic(    LinkPath                *result, 
                                                                short int       **heurpot, 
                                                                int                     dimx, 
                                                                int                     dimy, 
                                                                IntPoint                *start, 
                                                                IntPoint                *goal);
        double qdist(double     *q1, double     *q2);
        int insert (int xx, int yy, IntPoint **FRONT, IntPoint *PFRONT, int nbfront, short int **V);
        int deletemin (int xx, int yy, IntPoint **FRONT, int nbfront, short int **V);

        //DATA VARIABLES
        int     debug,                  // debug level  
        arm_degree,                             // Arm real degree              
        //dimx, dimy, dimz,             // Workspace working dimension  
        dimt[MAX_DEGREE],
        dimq[MAX_DEGREE];               // TxQ space working dimension  

        unsigned char   *wall;

        //double   COEFX, COEFY, COEFZ; // Coefficients converting real space to working space  
        double   COEFD_Q, COEFQ_D;              // Coefficients between angle to look-up table index    

        //double   *TCOS, *TSIN; (?)

        //Arm original_arm,     // First readed in arm          
        //      converted_arm,  /* arm converted into working space     */
        //      working_arm;    /* converted for working                */

        double     *orig_initial_config, *orig_final_config;  // Initial and final configurations       
        IntConfig       initial_config, final_config;  // Converted configuration in look-up space
        JointLimit      original_joint_limit[MAX_DEGREE];
        IntJointLimit   joint_limit[MAX_DEGREE];   // Joint limits              

        //unsigned char   **ws_map;     /* bitmap for obstacles         */
        //short int       **ws_dist;      /* distance map for the work space  */ 
        //unsigned char   **ws_voro;      /* voronoi diagram for the work space*/ 
        //short int     **link_map;
                        /* bitmap for links to check selfcollision */
        //short int     linkfill[MAX_DEGREE],   /* Fill selfcollision data */
        //      linkmask[MAX_DEGREE];   /* Selfcollision mask           */

        //FloatPoint    base;           /* converted base in working space */
        
        TQCSpace        tqcs;           // The Whole TxQ Cspace struct  

        LinkPath        *lnkcps, bkwdpath, frwdpath, mainpath, combpath; // Link Pathes in TxQ                  

        // Record t, q parameters in linkpath corresponding to
        // initial and final positions                          
        TQConfig        initial_tq[MAX_DEGREE], final_tq[MAX_DEGREE];

        double          **globalpath;
        int     globaldim;

        char            datadir[40];

        int             max_level, max_bt_no;  // Planner control parameters    

        int     sp_debug_level;  // Debug Control 
        double distance_detect_range;
        //double link_radius[MAX_DEGREE+1];

        int BT_level, BT_level_test, BT_number[MAX_BACKTRACKING_LEVEL+2]; //in plannr3d.cpp

      //## end PL_Sequential%3752D78501D0.private
  private: //## implementation
    // Additional Implementation Declarations
      //## begin PL_Sequential%3752D78501D0.implementation preserve=yes
      //## end PL_Sequential%3752D78501D0.implementation

};

//## begin PL_Sequential%3752D78501D0.postscript preserve=yes
//## end PL_Sequential%3752D78501D0.postscript

// Class PL_Sequential 

//## begin module%3752D78501D0.epilog preserve=yes
//## end module%3752D78501D0.epilog

#pragma warning( pop )
#endif

---