planners/obsolete/PL_IanRandom.cpp

Go to the documentation of this file.

//## begin module%3775A6140215.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%3775A6140215.cm

//## begin module%3775A6140215.cp preserve=no
//## end module%3775A6140215.cp

//## Module: PL_IanRandom%3775A6140215; Pseudo Package body
//## Source file: E:\mpk\code\Planners\PL_IanRandom.cpp

//## begin module%3775A6140215.additionalIncludes preserve=no
//## end module%3775A6140215.additionalIncludes

//## begin module%3775A6140215.includes preserve=yes
//## end module%3775A6140215.includes

// PL_IanRandom
#include "PL_IanRandom.h"
//## begin module%3775A6140215.additionalDeclarations preserve=yes
//## end module%3775A6140215.additionalDeclarations


// Class PL_IanRandom 

PL_IanRandom::~PL_IanRandom()
{
  //## begin PL_IanRandom::~PL_IanRandom%.body preserve=yes
  //## end PL_IanRandom::~PL_IanRandom%.body
}



//## Other Operations (implementation)
bool PL_IanRandom::Plan ()
{
  //## begin PL_IanRandom::Plan%930457209.body preserve=yes
        //always start at the start
        path.AppendPoint( GetStartConfig() ) ;

        //check the initial and the goal point in a direct line
        if( !collisionDetector->IsInterferingLinear( GetStartConfig(), GetGoalConfig() ) )
        {
                path.AppendPoint( GetGoalConfig() ) ;
                return true ;
        }

        //start trying some random configurations
        for( int i = 0; i < 100000; i++ )       //IMPROVE: don't hardcode the 100
        {
                Configuration intermediate = GenerateRandomConfig() ;
                if( !collisionDetector->IsInterferingLinear( GetStartConfig(), intermediate ) &&
                        !collisionDetector->IsInterferingLinear( intermediate, GetGoalConfig() ) )
                {
                        path.AppendPoint( intermediate ) ;
                        path.AppendPoint( GetGoalConfig() ) ;
                        return true ;
                }

                //check the timer 
                if( HasTimeLimitExpired() == true )
                {
                        return false ;
                }
        }
        return false ;
  //## end PL_IanRandom::Plan%930457209.body
}

Configuration PL_IanRandom::GenerateRandomConfig () const
{
  //## begin PL_IanRandom::GenerateRandomConfig%930611904.body preserve=yes
        int dof = GetStartConfig().DOF() ;
        Configuration returnme ;
        returnme.SetLength( dof ) ;

        for( int i = 0; i < dof; i++ )
        {
                double max = collisionDetector->JointMax( i ) ;
                double min = collisionDetector->JointMin( i ) ;
                double random = double( rand() ) / RAND_MAX ; //this is a random number between [0..1]
                returnme[ i ] = min + ( max - min ) * random ;
        }

        return returnme ;
  //## end PL_IanRandom::GenerateRandomConfig%930611904.body
}

Configuration PL_IanRandom::GenerateValidRandomConfig () const
{
  //## begin PL_IanRandom::GenerateValidRandomConfig%930611905.body preserve=yes
        Configuration returnMe = GenerateRandomConfig() ;
        while( collisionDetector->IsInterfering( returnMe ) )
        {       
                //IMPROVE: this is potentially an infinite loop if the cspace is all blocked up
                returnMe = GenerateRandomConfig() ;
        }
        return returnMe ;
  //## end PL_IanRandom::GenerateValidRandomConfig%930611905.body
}

// Additional Declarations
  //## begin PL_IanRandom%3775A6140215.declarations preserve=yes
  //## end PL_IanRandom%3775A6140215.declarations

//## begin module%3775A6140215.epilog preserve=yes
//## end module%3775A6140215.epilog

---