planners/aca/PL_ACA_Connect.cpp

Go to the documentation of this file.

#include <assert.h>
#include "synchronization/semaphore.h"
#include "PL_ACA_CONNECT.h"

#include "jma_switch.h"
#include "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 NUMBER_OF_EMBRYOS MAX_EMBRYOS
#define BIG_NUMBER 1.0e+37


#define THICKNESS 3
#define WIN_WIDTH  1000
#define WIN_HEIGHT 500
#define EPSILON  1.0e-3
#define BIG_NUMBER 1.0e+37

PL_ACA_CONNECT::~PL_ACA_CONNECT()
{

}

bool PL_ACA_CONNECT::Plan ()
{
        PlannerBase::StartTimer() ;
        path.Clear() ;
        if( collisionDetector->IsInterfering( GetStartConfig() ) )
        {
                path.AppendPoint( GetGoalConfig() ) ;
                path.AppendPoint( GetGoalConfig() ) ;
                return false ;
        }
        if( collisionDetector->IsInterfering( GetGoalConfig() ) )
        {
                path.AppendPoint( GetStartConfig() ) ;
                path.AppendPoint( GetStartConfig() ) ;
                return false ;
        }

        Semaphore semaphore( guid );
        semaphore.Lock();
    SetStart_and_GoalConfig (GetStartConfig(), GetGoalConfig());

        bool path_found=false;
    pointer=1;
    JMA_Roadmap_Tree *temp_tree, *temp_tree1, *temp_tree2;

        temp_tree1 = &tree1;
        temp_tree2 = &tree2;


    for(int i=1; i<MAX_LANDMARKS;i++)
        {
                if (!Search(temp_tree1,temp_tree2))
                {
                        if( HasTimeLimitExpired() == true )
                        {
                                return false ;
                        }                       
                        //Advance from current_node toward nearest_node
                        advance(temp_tree1,temp_tree2);
                // swap the two trees

                        temp_tree=temp_tree1;
                        temp_tree1=temp_tree2;
                        temp_tree2=temp_tree;
                        pointer=pointer+1;

                        //=== place a new landmark ===
                        Explore(temp_tree1);
            semaphore.Unlock();
            semaphore.Lock();
                }
                else
                {
                        path_found=true;
                        break;
                }

        }

    semaphore.Unlock();

        if ((!(path_found))||( (tree1.n_nodes +tree2.n_nodes)> MAX_LANDMARKS ))
            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)
    {
                GetIdsPath(&tree1, &tree2);
//              optimization();
                for( int i = 0; i < mark; i++ )
                {
                        int pointIndex = ids_list[ i ] ;
                        Configuration point = tree1.node[ pointIndex ].landmark.dof ;
                        path.AppendPoint( point ) ;
                }
                for(i = mark; i <= end_of_array ; i++ )
                {
                        int pointIndex = ids_list[ 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
         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 ;
}

double PL_ACA_CONNECT::get_distance(const Configuration& conf1, const Configuration& conf2) const
{
  return collisionDetector->DistanceBetween( conf1, conf2 ) ;
}
void  PL_ACA_CONNECT::MyRandomPath (int id_landmark, const int id_embryo, const int n_dof, const JMA_Configuration init_conf, JMA_Configuration* embryo) const
{ 
        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)) || (collisionDetector->IsInterferingLinear(current, config)));
        (*embryo).dof = config ;
}

int PL_ACA_CONNECT::GetNAncestors (const JMA_Roadmap_Tree* tree, const int landmark_id)
{
  int counter;
  int id;

  id = landmark_id;
  counter = 0;
  
  while(id != 0)
    {
      ++counter;
      id = (*tree).node[id].father_id;
    }
  return ++counter;
}

// Search between the new node and the nearest node in another tree
bool PL_ACA_CONNECT::Search (JMA_Roadmap_Tree* temp_tree1, JMA_Roadmap_Tree* temp_tree2) 
{
        int numberOfNodes = (*temp_tree1).n_nodes;
        current_node = (*temp_tree1).node[ numberOfNodes - 1 ].landmark.dof;

    get_nearest_node(&current_node, temp_tree2, &nearest_node);
        if( collisionDetector->IsInterferingLinear(current_node, nearest_node) )
        {
                return false ;
        }
        else
        {
                (*temp_tree1).node[(*temp_tree1).n_nodes].landmark.dof=nearest_node;
                (*temp_tree1).node[(*temp_tree1).n_nodes].father_id=(*temp_tree1).n_nodes-1;
                ++(*temp_tree1).n_nodes;
                return true ;
        }
}

void PL_ACA_CONNECT::get_nearest_node(Configuration* the_node, JMA_Roadmap_Tree* temp_tree2, Configuration* the_nearest_node) 
{
        double min_distance=10000.0;
        double temp_distance;
        int aaa=(*temp_tree2).n_nodes;

        for (int i=0; i<aaa; i++)
        {
        temp_distance=get_distance((*the_node),(*temp_tree2).node[i].landmark.dof);
                if(min_distance>temp_distance)
                {
                        min_distance=temp_distance;
                        nearest_node_id=i;
                }
        }
        (*the_nearest_node)=(*temp_tree2).node[nearest_node_id].landmark.dof;
}

void PL_ACA_CONNECT::InitRoadmapTree (JMA_Roadmap_Tree* ptr_tree1, const int n_dof, const int n_embryos, const JMA_Configuration init_conf, JMA_Roadmap_Tree* ptr_tree2, const JMA_Configuration goal_conf) const
{// generate embryoes for tree1, not for tree2.
  int i;
  double dist;

  (*ptr_tree1).N_dof = n_dof;
  (*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 = n_embryos;
  (*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;

  dist =-1000.0;
  for(i = 0; i < (*ptr_tree1).node[0].N_embryos; i++)
    {

      GenerateNewEmbryo(ptr_tree1,0,i);

      if((*ptr_tree1).node[0].embryo[i].dist > dist)
        {
          (*ptr_tree1).best_embryo_id = i;
          dist = (*ptr_tree1).node[0].embryo[i].dist;
        }
    }
  (*ptr_tree1).max_dist = dist;

  (*ptr_tree2).N_dof = n_dof;
  (*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 = n_embryos;
  (*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 =-1000.0;
  for(i = 0; i < (*ptr_tree2).node[0].N_embryos; i++)
    {

      GenerateNewEmbryo(ptr_tree2,0,i);

      if((*ptr_tree2).node[0].embryo[i].dist > dist)
        {
          (*ptr_tree2).best_embryo_id = i;
          dist = (*ptr_tree2).node[0].embryo[i].dist;
        }
    }
  (*ptr_tree2).max_dist = dist;
}

void PL_ACA_CONNECT::Explore (JMA_Roadmap_Tree* tree) const
{
  /*=== place a new landmark ===*/
  (*tree).node[(*tree).n_nodes].landmark = (*tree).node[(*tree).best_father_id].embryo[(*tree).best_embryo_id];
  (*tree).node[(*tree).n_nodes].father_id = (*tree).best_father_id;
  (*tree).n_nodes=(*tree).n_nodes+1;
  /*=== create embryos for the new landmark ===*/
  (*tree).node[(*tree).n_nodes-1].N_embryos = NUMBER_OF_EMBRYOS;
  for(int i = 0; i <MAX_EMBRYOS ; i++)
  {
      GenerateNewEmbryo(tree,(*tree).n_nodes-1,i);

  }

  GenerateNewEmbryo(tree,(*tree).best_father_id,(*tree).best_embryo_id);

  UpdateRoadmapTree(tree);
}

void PL_ACA_CONNECT::GenerateNewEmbryo (JMA_Roadmap_Tree* tree, const int id_landmark, const int id_embryo) const
{
        //(*tree).node[id_landmark].landmark.dof //IMPROVE: this is not assigned anything yet
        //((*tree).node[id_landmark].embryo[id_embryo]).dof = startConfig ;     //IMPROVE: this makes the DOF equal - should go elsewhere - constructor?
        int N_Dof = (*tree).N_dof ;
        MyRandomPath( id_landmark, id_embryo, N_Dof, (*tree).node[id_landmark].landmark, &((*tree).node[id_landmark].embryo[id_embryo]) );
        
        ComputeEmbryoDistance(tree,&((*tree).node[id_landmark].embryo[id_embryo]));
}

void PL_ACA_CONNECT::GetIdsPath (JMA_Roadmap_Tree* ptr_tree1, JMA_Roadmap_Tree* ptr_tree2)
{
        int landmark_id, counter, id;
        if ((pointer % 2)==1)//current_tree is tree1
        {
        landmark_id=(*ptr_tree1).n_nodes-1;
                counter = GetNAncestors(ptr_tree1,landmark_id);
                mark=counter-1;
                end_of_array=counter;
                id = landmark_id;
                while(id != 0)
                   { 
                     ids_list[mark] = id;
                     id = (*ptr_tree1).node[id].father_id;
                     --mark;
                   }
                ids_list[mark] = id;
                mark=counter;
        counter = GetNAncestors(ptr_tree2,nearest_node_id);
                id =nearest_node_id;
                while(id != 0)
                        { 
                        ids_list[end_of_array] = id;
                        id = (*ptr_tree2).node[id].father_id;
                        ++end_of_array;
                        }
                ids_list[ end_of_array ] = id;
        }
        else
        {
                counter = GetNAncestors(ptr_tree1,nearest_node_id);
                mark=counter-1;
                end_of_array=counter;
                id = nearest_node_id;
                while(id != 0)
                   { 
                     ids_list[mark] = id;
                     id = (*ptr_tree1).node[id].father_id;
                     --mark;
                   }
                ids_list[mark] = id;
                mark=counter;
                landmark_id=(*ptr_tree2).n_nodes-1;
        counter = GetNAncestors(ptr_tree2,landmark_id);
                id =landmark_id;
                while(id != 0)
                        { 
                        ids_list[end_of_array] = id;
                        id = (*ptr_tree2).node[id].father_id;
                        ++end_of_array;
                        }
                ids_list[ end_of_array ] = id;
        }
}

void PL_ACA_CONNECT::UpdateRoadmapTree (JMA_Roadmap_Tree* tree) const
{
  //## begin PL_Juan::UpdateRoadmapTree%929987271.body preserve=yes
  int i, j;
  double distance;

  (*tree).last_max_dist = (*tree).max_dist;
  (*tree).max_dist = 0.0;
  
  for(i = 0 ; i < (*tree).n_nodes; i++)
  {
      for(j = 0; j < (*tree).node[i].N_embryos; j++)
      {

        /*==== update the distance of embryo(i,j) w.r.t. the new landmark ====*/

              distance =get_distance((*tree).node[i].embryo[j].dof,
              (*tree).node[(*tree).n_nodes-1].landmark.dof); 

              if (distance < (*tree).node[i].embryo[j].dist)  
                  (*tree).node[i].embryo[j].dist = distance;

        /*==== meanwhile we take advantage of this cycle and 
               find the embryo with the bigest distance !!  ====*/

              if ((*tree).max_dist < (*tree).node[i].embryo[j].dist)
                  {
                  (*tree).max_dist = (*tree).node[i].embryo[j].dist;
                  (*tree).best_father_id = i;
                  (*tree).best_embryo_id = j;
                  }
          }
  }
}

void PL_ACA_CONNECT::ComputeEmbryoDistance (JMA_Roadmap_Tree* tree, JMA_Configuration* embryo) const
{ //distance between this embryo and the existing set of landmarks
  int i;
  double distance;
   
  (*embryo).dist = BIG_NUMBER;

  for(i=0; i < (*tree).n_nodes; i++)
    {
      /*==== compute distance w.r.t. landmark i ====*/
      distance = get_distance((*embryo).dof,(*tree).node[i].landmark.dof); 

      if (distance < (*embryo).dist)
        (*embryo).dist = distance;
    }
}

void PL_ACA_CONNECT::advance(JMA_Roadmap_Tree* temp_tree1, JMA_Roadmap_Tree* temp_tree2)
{
        bool one_step=get_new_node(&current_node, &nearest_node, &new_node);
        while((!( collisionDetector->IsInterferingLinear(new_node, current_node))) && (one_step))
        {
        int current_node_id=(*temp_tree1).n_nodes-1;
                (*temp_tree1).node[(*temp_tree1).n_nodes].landmark.dof=new_node;
                (*temp_tree1).node[(*temp_tree1).n_nodes].father_id=current_node_id;
                current_node=new_node;
                ++(*temp_tree1).n_nodes;
        (*temp_tree1).node[(*temp_tree1).n_nodes-1].N_embryos = NUMBER_OF_EMBRYOS;
                for(int i=0; i< NUMBER_OF_EMBRYOS; i++)
            GenerateNewEmbryo(temp_tree1,(*temp_tree1).n_nodes-1,i);
                UpdateRoadmapTree(temp_tree1);
        one_step=get_new_node(&current_node, &nearest_node, &new_node);
        }
}

//bool PL_ACA_CONNECT::get_new_node(Configuration* the_current_node, Configuration* the_nearest_node, Configuration* the_new_node) const
//{
//      double Fixed_Distance=0.1;///////need to adjust
//      double pp=get_distance((*the_current_node), (*the_nearest_node));
//      (*the_new_node)=(*the_nearest_node)*(1-Fixed_Distance/pp)+(*the_current_node)*(Fixed_Distance/pp);
//      if( collisionDetector->IsInterfering(*the_new_node))
//              return false;
//      return true;
//}


bool PL_ACA_CONNECT::get_new_node(Configuration* the_current_node, Configuration* the_nearest_node, Configuration* the_new_node) const
{
        double Fixed_Distance=150;
        double pp=get_distance((*the_current_node), (*the_nearest_node));
        (*the_new_node)=(*the_current_node)*(1-Fixed_Distance/pp)+(*the_nearest_node)*(Fixed_Distance/pp);
        if( collisionDetector->IsInterfering(*the_new_node))
                return false;
        return true;
}


void PL_ACA_CONNECT::SetStart_and_GoalConfig(const Configuration& configuration1, const Configuration& configuration2)
{
        //Configuration oldStart = this->startConfig ;
        //PL_Boolean_Output::SetStartConfig( configuration1 ) ;
        //if( configuration1 != oldStart )
        //{
                init_conf.dof = configuration1 ;
                init_conf.dist = 0 ;
//      }
//      Configuration oldGoal = this->goalConfig ;
//      PL_Boolean_Output::SetGoalConfig( configuration2 ) ;
        //if( configuration2 != oldGoal )
        //{
                goal_conf.dof = configuration2 ;
                goal_conf.dist = 0 ;
//      }
        InitRoadmapTree( &tree1, GetStartConfig().DOF(), MAX_EMBRYOS, init_conf, &tree2, goal_conf );
}

void PL_ACA_CONNECT::optimization()
{
        int i=0;
        int m=0;
        int j=mark-1;
        new_ids_list[m]=ids_list[0];
        ++m;
        while (i<mark)
        {
                while (j>i)
                {
                        if (!collisionDetector->IsInterferingLinear(tree1.node[i].landmark.dof, tree1.node[j].landmark.dof))
                        {
                                new_ids_list[m]=ids_list[j];
                                ++m;
                                i=j;
                                j=mark-1;
                        }
                        else
                                --j;
                }
                ++i;
                new_ids_list[m]=ids_list[i];
                ++m;
                j=mark-2;
        }
    mark=m;
        j=end_of_array;
        while (j>i)
        {
                if (!collisionDetector->IsInterferingLinear(tree2.node[i].landmark.dof, tree2.node[j].landmark.dof))
                {
                        new_ids_list[m]=ids_list[j];
                        ++m;
                        i=j;
                        j=end_of_array;
                }
                else
                        --j;
        }
        end_of_array=m-1;
}

bool PL_ACA_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;
}

---