planners/rrt/PL_RRT.cpp

Go to the documentation of this file.

#include <assert.h>
#include "synchronization/semaphore.h"
#include "PL_RRT.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


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

PL_RRT::~PL_RRT()
{

}

bool PL_RRT::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_landmarks;



        bool path_found=false;

    JMA_Roadmap_Tree *temp_tree;

        temp_tree = &tree;
        Semaphore semaphore( guid );
        semaphore.Lock();

        //int _MAX_LANDMARKS = 3.5*MAX_LANDMARKS;
    if(!( collisionDetector->IsInterferingLinear(GetStartConfig(),GetGoalConfig())))
                path_found=true;
        else
        {
            //for(i=1; i<_MAX_LANDMARKS;i++)
                do
                {
                        Get_random_config(&random_config);
                        if ((Extend_trapped(temp_tree, &(temp_tree->n_nodes), &random_config))!=3)
                        {                                       
                        if(!( collisionDetector->IsInterferingLinear(new_config,GetGoalConfig())))
                                {
                                        last_config=new_config;
                                        path_found=true;
                    (tree).node[tree.n_nodes].landmark.dof=GetGoalConfig();     
                                        (tree).node[tree.n_nodes].father_id=tree.n_nodes-1;
                    ++(tree.n_nodes);
                                        break;
                                }
                        }
                        semaphore.Unlock();
                        semaphore.Lock();
                } while ((!path_found) && (HasTimeLimitExpired() == false) );
        }
        semaphore.Unlock();
        //if ((!(path_found))||( HasTimeLimitExpired() == true )||( tree.n_nodes > _MAX_LANDMARKS/2 ))
        //    return false;

        //double elapsedTime = timer.ElapsedTime();
        //int mmmmm=tree.n_nodes;
        //semaphore.Unlock();


        if (path_found)
    {
// get ids of landmarks of the  paths
                GetIdsPath(&tree,tree.n_nodes-1,ids_list,&n_landmarks);

                for( i = n_landmarks-1; i >=0; i--)
                {
                        int pointIndex = ids_list[ i ] ;
                        Configuration point = tree.node[ pointIndex ].landmark.dof ;
                        path.AppendPoint( point ) ;
                }
        path.AppendPoint( GetGoalConfig() ) ;           
        }
        else
    {
                if( path.Size() == 0 )
                {
                        path.AppendPoint( GetStartConfig() ) ;
                }
                return false ;
        }

//optimize the found path
         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 ] ;
                assert( !collisionDetector->IsInterferingLinear( p1, p2 ) ) ;
                if( collisionDetector->IsInterferingLinear( p1, p2 ) )
                {
                        assert( false ) ;
                }
        }

        return true ;
}

int PL_RRT::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::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, &near_config);

        Get_new_config(&random_config, &near_config, &new_config);
        if(!( collisionDetector->IsInterferingLinear(near_config,new_config)))
        {
            (*Ptr_tree).node[*the_n_nodes].landmark.dof=new_config;
                        new_config=new_config;
                        (*Ptr_tree).node[*the_n_nodes].father_id=near_neighbour_id;
            ++(*the_n_nodes);
                        return 1;  //reach 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::Get_nearest_neighbour(JMA_Roadmap_Tree* Ptr_tree1, Configuration* the_random_config, Configuration* the_near_config)
{
        double min_distance=10000.0;
        double temp_distance;
        int indicator;
        for (int i=0; i<(Ptr_tree1->n_nodes); i++)
        {
        temp_distance=MyDistanceFunc((*Ptr_tree1).node[i].landmark.dof, *the_random_config);
                if(min_distance>temp_distance)
                {
                        min_distance=temp_distance;
                        indicator=i;
                }
        }
        *the_near_config=(*Ptr_tree1).node[indicator].landmark.dof;
        return indicator;
}


int PL_RRT::Get_new_config(Configuration* the_random_config, Configuration* the_near_config, Configuration* the_new_config)
{
        double Fixed_Distance=0.5;
        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::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;
//}

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

  return collisionDetector->DistanceBetween( conf1, conf2 ) ;

}


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


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

void PL_RRT::GetIdsPath (JMA_Roadmap_Tree* Ptr_tree, 
                                                                 const int landmark_id, int ids_list[], 
                                                                 int* n_landmarks)
{
  //landmark_id is the id of the last landkmark of the tree(last_config).
  //and the tree is connected to goalConfig at last_config)

    int counter=1,id;

    id = landmark_id;
    ids_list[counter-1]=id; 
        while (id!=0)
        {
            id = (*Ptr_tree).node[id].father_id;          
                ids_list[counter]=id; 
                ++counter;
        }
    (*n_landmarks) = counter;
}


void PL_RRT::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( &tree, GetStartConfig().DOF(), init_conf);
}


bool PL_RRT::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 );
        glPointSize(5.0f);      // Set size of points associated with the nodes.

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

        double g0, g1, g2;

        double v0, v1, p0, p1;
        double v2 = 0;
        double p2 = 0;
        for( i = 0; i < tree.n_nodes; i++ )
        {
                JMA_Node node = tree.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 );
                        glColor3f(1.0f,1.0f,0.0f);
                        glVertex3f( v0, v1, v2 );
                glEnd();

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


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

                        //glColor3f(1.0f,0.0f,0.0f);
                        glBegin( GL_LINES );
                                glColor3f(1.0f,0.5f,0.0f);      // set colour to orange
                                glVertex3f( v0, v1, v2 );
                                glColor3f(1.0f,0.0f,0.0f);      // set colour to red
                                glVertex3f( p0, p1, p2 );
                        glEnd();
                }


        }
        

        glColor3f( 1.0f, 1.0f, 0.0f );
        glBegin( GL_POINTS );
                g0 = g1 = g2 = 0;
                if( dof > 0 )
                {
                        g0 = this->GetGoalConfig()[ 0 ];
                }
                if( dof > 1 )
                {
                        g1 = this->GetGoalConfig()[ 1 ];
                }
                if( dof > 2 )
                {
                        g2 = this->GetGoalConfig()[ 2 ];
                }

                glColor3f(0.0f,0.0f,1.0f);    //Set the start and end to be blue

                glVertex3f( g0, g1, g2 );
        glEnd();

        glColor3f( 1.0f, 1.0f, 0.0f );
        glBegin( GL_POINTS );
                g0 = g1 = g2 = 0;
                if( dof > 0 )
                {
                        g0 = this->GetStartConfig()[ 0 ];
                }
                if( dof > 1 )
                {
                        g1 = this->GetStartConfig()[ 1 ];
                }
                if( dof > 2 )
                {
                        g2 = this->GetStartConfig()[ 2 ];
                }

                glColor3f(0.0f,0.0f,1.0f);    //Set the start and end to be blue

                glVertex3f( g0, g1, g2 );
        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;
}

---