planners/rrt/PL_RRT_Connect.cpp

Go to the documentation of this file.

#include <assert.h>
#include "synchronization/semaphore.h"
#include "PL_RRT_CONNECT.h"
#include "planners/aca/jma_switch.h"
#include "planners/aca/jma_aca_macros.h" //IMPROVE: these should be c++ function calls
#include <iosfwd>
#include <iostream>
#include "kinematics/configuration.h"
#include "opengl/glos.h"
#include <gl/gl.h>

#define THICKNESS 3
#define WIN_WIDTH  1000
#define WIN_HEIGHT 500
#define EPSILON  1.0e-3
#define BIG_NUMBER 1.0e+37
#define STEP_SIZE       (5*collisionDetector->DOF())


//IMPROVE: these definitions should be constants, or removed
//## end module%376A87F4037E.additionalDeclarations

PL_RRT_CONNECT::~PL_RRT_CONNECT()
{

}

bool PL_RRT_CONNECT::Plan()
{
        //MessageBox(NULL, "This is Rex's RRT_CONNECT planner", "Hello", MB_OK);
        //return false;

        PlannerBase::StartTimer() ;
        path.Clear() ;
//      StartOutput() ;         //IMPROVE: this is for the output window - shouldnt be here
 
        if( collisionDetector->IsInterfering( GetStartConfig() ) )
        {
                path.AppendPoint( GetGoalConfig() ) ;
                path.AppendPoint( GetGoalConfig() ) ;
                return false ;
        }
        if( collisionDetector->IsInterfering( GetGoalConfig() ) )
        {
                path.AppendPoint( GetStartConfig() ) ;
                path.AppendPoint( GetStartConfig() ) ;
                return false ;
        }

//set start and goal configuration and initialize two trees.

    SetStart_and_GoalConfig (GetStartConfig(), GetGoalConfig());

        int i;
        int n_landmarks1, n_landmarks2;
        bool path_found=false;
    JMA_Roadmap_Tree *temp_tree, *temp_tree1, *temp_tree2;

        temp_tree1 = &tree1;
        temp_tree2 = &tree2;
        Semaphore semaphore( guid );
        semaphore.Lock();
        //int _MAX_LANDMARKS = 3.5*MAX_LANDMARKS;
    //for(i=1; i<_MAX_LANDMARKS;i++)
        do
        {
                Get_random_config(&random_config);
                if ((Extend_trapped(temp_tree1, &(temp_tree1->n_nodes), &random_config))!=3)
                {
                        old_new_config=new_config;
                        if (connect(temp_tree2, old_new_config))
                        {
                                path_found=true;
                                break;
                        }
                }
                // swap the two trees
                
                temp_tree=temp_tree1;
                temp_tree1=temp_tree2;
                temp_tree2=temp_tree;

                semaphore.Unlock();
                semaphore.Lock();
        } while ((!path_found) && (HasTimeLimitExpired() == false) );

        semaphore.Unlock();

        //if ((!(path_found))||( HasTimeLimitExpired() == true )||( (tree1.n_nodes +tree2.n_nodes)> _MAX_LANDMARKS/2 ))
        //    return false ;

        //double elapsedTime = timer.ElapsedTime();
        //int mmmmm=tree1.n_nodes +tree2.n_nodes;
        //int m1=tree1.n_nodes;
        //int m2=tree2.n_nodes;

        if (path_found)
    {
// get ids of landmarks of the two paths of the two trees
                GetIdsPath(&tree1,&tree2, tree1.n_nodes-1,tree2.n_nodes-1, ids_list1,&n_landmarks1, ids_list2,&n_landmarks2);

                for( i = n_landmarks1-1; i >=0; i--)
                {
                        int pointIndex = ids_list1[ i ] ;
                        Configuration point = tree1.node[ pointIndex ].landmark.dof ;
                        path.AppendPoint( point ) ;
                }
                for( i =1; i <n_landmarks2; i++)
                {
                        int pointIndex = ids_list2[ i ] ;
                        Configuration point = tree2.node[ pointIndex ].landmark.dof ;
                        path.AppendPoint( point ) ;
                }
        }
        else
    {
                if( path.Size() == 0 )
                {
                        path.AppendPoint( GetStartConfig() ) ;
                }
                return false ;
        }

//optimize the found path
#ifdef _PATH_OPTIMIZATION_
         PA_Points the_path;
         int size=path.Size();
         the_path.AppendPoint(path[0]);
         Configuration p1 = path[ 0 ] ;
         int n=size-1;
         int indicater=0;
     while (indicater!=size-1)
         {
            Configuration p2 = path[n];
                while (collisionDetector->IsInterferingLinear( p1, p2 ))
                {
                        n=n-1;
                        if (n==indicater)
                                assert(false);
                        p2=path[n];
                }
                the_path.AppendPoint (p2);
                p1=p2;
                indicater=n;
                n=size-1;
         }
         path.Clear();
         for (n=0;n<the_path.Size();n++)
                 path.AppendPoint(the_path[n]);
         the_path.Clear();

        //verify the outputs
        for( i = 0; i < path.Size() - 1 ; i++ )
        {
                Configuration p1 = path[ i ] ;
                Configuration p2 = path[ i + 1 ] ;
                int e=i;

                assert( !collisionDetector->IsInterferingLinear( p1, p2 ) ) ;
                if( collisionDetector->IsInterferingLinear( p1, p2 ) )
                {
                        assert( false ) ;
                }
        }
#endif

        return true ;
}

int PL_RRT_CONNECT::Get_random_config(Configuration* the_random_config)
{
        Configuration config= GetStartConfig() ;        //this just initializes the config
        //bool isConfigInCollision = collisionDetector->IsInterfering( config ) ;
        //assert( isConfigInCollision == false ) ;
        //Configuration current = init_conf.dof ;
        do
        {
                for( int j = 0; j < config.DOF(); j++ )
                {
                        double jointMax = collisionDetector->JointMax( j ) ;
                        double jointMin = collisionDetector->JointMin( j ) ;
                        double random01 = (double) switch_random()/BIGEST_RND_NUMBER ;
                        double jointVal = jointMin + random01 * ( jointMax - jointMin );
                        config[ j ] = jointVal ;
                }
        } while( collisionDetector->IsInterfering(config ) );
        (*the_random_config) = config ;
        return 0;
}

int PL_RRT_CONNECT::Extend_trapped(JMA_Roadmap_Tree* Ptr_tree, int* the_n_nodes, Configuration* the_random_config)
{
        //get nearest neighbour of random_config
        int near_neighbour_id=Get_nearest_neighbour(Ptr_tree, the_random_config);

//      Get_new_config(&random_config, &near_config, &new_config);
        if(!( collisionDetector->IsInterferingLinear(near_config,random_config)))
        {
            (*Ptr_tree).node[*the_n_nodes].landmark.dof=random_config;
                        new_config=random_config;
                        (*Ptr_tree).node[*the_n_nodes].father_id=near_neighbour_id;
            ++(*the_n_nodes);
                        return 1;  //reach the random_config
        }
        return 3; // trapped
}

int PL_RRT_CONNECT::Simple_Extend_trapped(JMA_Roadmap_Tree* Ptr_tree, 
                                                                                  int* the_n_nodes, Configuration old_new_config)
{
        Get_new_config(&old_new_config, &near_config, &new_config);
        if((!(collisionDetector->IsInterferingLinear(near_config,new_config))) && ((!(collisionDetector->IsInterfering(new_config)))))
        {
                if (MyDistanceFunc(near_config, new_config)>=MyDistanceFunc(near_config, old_new_config))
                {
            (*Ptr_tree).node[*the_n_nodes].landmark.dof=old_new_config;//double use of this Configuration
                        //new_config=random_config;
                        (*Ptr_tree).node[*the_n_nodes].father_id=near_neighbour_id;
            (*the_n_nodes)=(*the_n_nodes)+1;
                        return 1;  //two trees get connected
                }
                else
                {

                    (*Ptr_tree).node[*the_n_nodes].landmark.dof=new_config;     
                        (*Ptr_tree).node[*the_n_nodes].father_id=near_neighbour_id;
            (*the_n_nodes)=(*the_n_nodes)+1;
                    near_config=new_config;
                        near_neighbour_id=(*the_n_nodes)-1;
                        return 2;//advance
                }
        }
        return 3;

}



int PL_RRT_CONNECT::Get_nearest_neighbour(JMA_Roadmap_Tree* Ptr_tree, Configuration* the_random_config)
{
        double min_distance=100000.0;
        double temp_distance;
        int indicator;
        for (int i=0; i<(*Ptr_tree).n_nodes; i++)
        {
        temp_distance=MyDistanceFunc((*Ptr_tree).node[i].landmark.dof, *the_random_config);
                if(min_distance>temp_distance)
                {
                        min_distance=temp_distance;
                        indicator=i;
                }
        }
        near_config=(*Ptr_tree).node[indicator].landmark.dof;
        return indicator;
}


int PL_RRT_CONNECT::Get_new_config(Configuration* the_random_config, Configuration* the_near_config, Configuration* the_new_config)
{
        double Fixed_Distance=STEP_SIZE;
        double pp=MyDistanceFunc((*the_random_config),(*the_near_config));
        (*the_new_config)=(*the_near_config)*(1-Fixed_Distance/pp)+(*the_random_config)*(Fixed_Distance/pp);
        return 0;
}


//int PL_RRT_CONNECT::Get_new_config(Configuration* the_random_config, Configuration* the_near_config, Configuration* the_new_config)
//{
//      double Fixed_Distance=0.1;///////need to adjust
//      double pp=MyDistanceFunc((*the_random_config),(*the_near_config));
//      (*the_new_config)=(*the_random_config)*(1-Fixed_Distance/pp)+(*the_near_config)*(Fixed_Distance/pp);
//      return 0;
//}



bool PL_RRT_CONNECT::connect(JMA_Roadmap_Tree* My_tree, Configuration old_new_config)
{
    near_neighbour_id=Get_nearest_neighbour(My_tree, &old_new_config);
        int i=2;
    while (i==2)
        {

            i=Simple_Extend_trapped(My_tree, &((*My_tree).n_nodes), old_new_config);
        }
        if(i==1)
                return true;
        else 
                return false;
}

double PL_RRT_CONNECT::MyDistanceFunc (const Configuration& conf1, const Configuration& conf2) const
{ 

  return collisionDetector->DistanceBetween( conf1, conf2 ) ;

}


void PL_RRT_CONNECT::InitRoadmapTree( JMA_Roadmap_Tree* Ptr_tree1, 
                                                                         JMA_Roadmap_Tree* Ptr_tree2, 
                                                                         const int n_dof1, 
                                                                         const int n_dof2, 
                                                                         const JMA_Configuration init_conf, 
                                                                         const JMA_Configuration goal_conf) const
{
  double dist;  
  (*Ptr_tree1).N_dof = n_dof1;
  (*Ptr_tree1).n_nodes = 1;
  (*Ptr_tree1).max_dist = 0.0;
  (*Ptr_tree1).last_max_dist = -1.0;
  (*Ptr_tree1).best_father_id = 0;
  (*Ptr_tree1).node[0].N_embryos = 0;
  (*Ptr_tree1).node[0].father_id = NIL;


  (*Ptr_tree1).node[0].landmark.dof = init_conf.dof;            //can't just assign JMA_Configurations
  (*Ptr_tree1).node[0].landmark.dist = init_conf.dist;

  (*Ptr_tree2).N_dof = n_dof2;
  (*Ptr_tree2).n_nodes = 1;
  (*Ptr_tree2).max_dist = 0.0;
  (*Ptr_tree2).last_max_dist = -1.0;
  (*Ptr_tree2).best_father_id = 0;
  (*Ptr_tree2).node[0].N_embryos = 0;
  (*Ptr_tree2).node[0].father_id = NIL;


  (*Ptr_tree2).node[0].landmark.dof = goal_conf.dof;            //can't just assign JMA_Configurations
  (*Ptr_tree2).node[0].landmark.dist = goal_conf.dist;


  dist = 0.0;
}
                                                                                     

void PL_RRT_CONNECT::GetIdsPath (JMA_Roadmap_Tree* Ptr_tree1,JMA_Roadmap_Tree* Ptr_tree2, 
                                                                 const int landmark_id1, const int landmark_id2, int ids_list1[], 
                                                                 int* n_landmarks1, int ids_list2[], int* n_landmarks2)
{
  //landmark_id1 and landmark_id2 are the ids of the last landkmark of the two trees.
  //and the two trees are connected by the two landmarks


    int counter=1;
        int id;

    id = landmark_id1;
    ids_list1[counter-1]=id; 
        while (id!=0)
        {
            id = (*Ptr_tree1).node[id].father_id;         
                ids_list1[counter]=id; 
                ++counter;
        }
    (*n_landmarks1) = counter;

        counter=1;
    id = landmark_id2;
    ids_list2[counter-1]=id; 
        while (id!=0)
        {
            id = (*Ptr_tree2).node[id].father_id;         
                ids_list2[counter]=id; 
                ++counter;
        }
    (*n_landmarks2) = counter;



//...............................................

// // This function also performs path smothing
//    int counter=1,id1,id2,marker;
//    Configuration ppp1,ppp2;
//    id1 = landmark_id1;
 //   ids_list1[counter-1]=id1; 
//      while (id1!=0)
//      {
  //      ppp1=(*Ptr_tree1).node[id1].landmark.dof;
        //    id1 = (*Ptr_tree1).node[id1].father_id;     
        //      marker=1;
        //      while (id1 != 0 && marker==1)
        //      {
        //        id2 = (*Ptr_tree1).node[id1].father_id;
        //        ppp2=(*Ptr_tree1).node[id2].landmark.dof;
        //              if (!( collisionDetector->IsInterferingLinear(ppp1, ppp2)))
        //              {
        //                      id1=id2;
        //                      marker=1;
        //              }
        //              else
        //                      marker=0;
        //      }
        //      ids_list1[counter]=id1; 
        //      ++counter;
//      }
  //  (*n_landmarks1) = counter;


 //   id1 = landmark_id2;
//      counter=1;
  //  ids_list2[counter-1]=id1; 
        //while (id1!=0)
//      {
  //      ppp1=(*Ptr_tree2).node[id1].landmark.dof;
        //    id1 = (*Ptr_tree2).node[id1].father_id;     
        //      marker=1;
        //      while (id1 != 0 && marker==1)
        //      {
        //        id2 = (*Ptr_tree2).node[id1].father_id;
        //        ppp2=(*Ptr_tree2).node[id2].landmark.dof;
        //              if (!( collisionDetector->IsInterferingLinear(ppp1, ppp2)))
        //              {
        //                      id1=id2;
        //                      marker=1;
        //              }
        //              else
        //                      marker=0;
        //      }
        //      ids_list2[counter]=id1; 
        //      ++counter;
//      }
  //  (*n_landmarks2) = counter;
//...................................................
}


void PL_RRT_CONNECT::SetStart_and_GoalConfig (const Configuration& configuration1,const Configuration& configuration2)
{
        init_conf.dof = configuration1 ;
        init_conf.dist = 0 ;
        goal_conf.dof = configuration2 ;
        goal_conf.dist = 0 ;
        InitRoadmapTree( &tree1, &tree2, GetStartConfig().DOF(), GetGoalConfig().DOF(), init_conf, goal_conf );
}


bool PL_RRT_CONNECT::DrawExplicit () const
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();

        glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );

        //disable lighting
        BOOL lightingState = glIsEnabled( GL_LIGHTING );
        glDisable( GL_LIGHTING );

        //disable Z buffering too
        BOOL zbufferState = glIsEnabled( GL_DEPTH_TEST );
        glDisable( GL_DEPTH_TEST );

        //enable smooth shading too
        glShadeModel( GL_SMOOTH );
        

        int i = 0;
        glColor4d( 1.0, 1.0, 1.0, 1.0 );
        glPointSize( 3.0 );

        int dof = this->GetGoalConfig().DOF();

        glColor3f( 1.0f, 1.0f, 0.0f );
        glBegin( GL_POINTS );
                double g0 = 0;
                double g1 = 0;
                double g2 = 0;
                double v0 = 0; 
                double v1 = 0; 
                double v2 = 0; 
                double p0 = 0;
                double p1 = 0;
                double p2 = 0;
                if( dof > 0 )
                {
                        g0 = this->GetGoalConfig()[ 0 ];
                }
                if( dof > 1 )
                {
                        g1 = this->GetGoalConfig()[ 1 ];
                }
                if( dof > 2 )
                {
                        g2 = this->GetGoalConfig()[ 2 ];
                }
                glVertex3f( g0, g1, g2 );
        glEnd();

        for( i = 0; i < tree1.n_nodes; i++ )
        {
                JMA_Node node = tree1.node[ i ];
                JMA_Configuration current = node.landmark;


                v0 = current.dof[ 0 ];
                v1 = current.dof[ 1 ];
                if( dof > 2 )
                {
                        v2 = current.dof[ 2 ];
                }

                glBegin( GL_POINTS );
                        glVertex3f( v0, v1, 0 );
                glEnd();

                int parent = node.father_id;
                if( parent == -1 )
                {
                }
                else
                {
                        JMA_Node parentNode = tree1.node[ parent ];
                        JMA_Configuration parentConfig = parentNode.landmark;


                        p0 = parentConfig.dof[ 0 ];
                        p1 = parentConfig.dof[ 1 ];
                        if( dof > 2 )
                        {
                                p2 = parentConfig.dof[ 2 ];
                        }

                        glColor4d( 1.0, 1.0, 1.0, 1.0 );
                        glBegin( GL_LINES );
                                glVertex3f( v0, v1, v2 );
                                glVertex3f( p0, p1, p2 );
                        glEnd();
                }


        }
        for( i = 0; i < tree2.n_nodes; i++ )
        {
                JMA_Node node = tree2.node[ i ];
                JMA_Configuration current = node.landmark;


                v0 = current.dof[ 0 ];
                v1 = current.dof[ 1 ];
                if( dof > 2 )
                {
                        v2 = current.dof[ 2 ];
                }

                glBegin( GL_POINTS );
                        glVertex3f( v0, v1, 0 );
                glEnd();

                int parent = node.father_id;
                if( parent == -1 )
                {
                }
                else
                {
                        JMA_Node parentNode = tree2.node[ parent ];
                        JMA_Configuration parentConfig = parentNode.landmark;

                        p0 = parentConfig.dof[ 0 ];
                        p1 = parentConfig.dof[ 1 ];
                        if( dof > 2 )
                        {
                                p2 = parentConfig.dof[ 2 ];
                        }

                        glColor4d( 1.0, 1.0, 1.0, 1.0 );
                        glBegin( GL_LINES );
                                glVertex3f( v0, v1, v2 );
                                glVertex3f( p0, p1, p2 );
                        glEnd();
                }


        }

        
        if( lightingState != 0x00 )
        {
                glEnable( GL_LIGHTING );
        }

        //disable Z buffering too
        if( zbufferState )
        {
                glEnable( GL_DEPTH_TEST );
        }
        else
        {
                glDisable( GL_DEPTH_TEST );
        }

        semaphore.Unlock();
        return true;
}

---