planners/astar/PL_Astar.cpp

Go to the documentation of this file.

//=========================================================================
//
//  File:  PL_Astar.h
//
//  Created by Shane Schneider
//
//              Base A* Planner. 
//

// PL_Astar
#include "synchronization/semaphore.h"
#include "PL_Astar.h"

// LEDA Include Files
#include <LEDA/node_map.h> 
#include <LEDA/node_pq.h>

#include "math/math2.h"
#include <iosfwd>
#include <iostream>

using std::endl;


//--------------------- Constants ---------------
const int DEFAULTNUMSTEPS = 40;
const double DEFAULTWEIGHT = 0.9;
const double COMPTOL = 1e-8;
// File Constants
static const char FILEEXT[] = ".a8";            // File extension for a base graph planner
static const char FILEHEADER[] = "PL_ASTAR";


// Class PL_Astar 

PL_Astar::PL_Astar()
{
        // Assign the file header and extension for this type
        strcpy( fileext, FILEEXT );
        strcpy( fileheader, FILEHEADER );

        // assign the open list pointer
        openp = NULL;
        
        // Clear the graph and initialize the node maps and open list
        ClearGraph();
                
        // Default Parameter Settings
        weight = DEFAULTWEIGHT;

        // Initialize the Step Settings
        SetDefaultStepSize();

        TRACE("PL_Astar Default Constructor completed.\n");

}


PL_Astar::~PL_Astar()
{
        // Delete the open list pointer
        if ( openp != NULL )
        {
                delete openp;
        }

}

//=============================================================================
// Clearing Functions
//
// Purpose: Clears the graph and start and goal nodes, initializes the node maps
//                  and initializes the open node list.
//                  
//=============================================================================
void PL_Astar::ClearGraph()
{
        // Call Parent's ClearGraph
        PL_GraphBase::ClearGraph();

        // Initialize the node maps to the correct graph.
        nodeExpanded.init(G,FALSE);
        
        InitNewSearch();        
}

//=============================================================================
// InitNewSearch 
//
// Purpose: Initializes the graph parameters for a new search
//                  
//=============================================================================
void PL_Astar::InitNewSearch()
{
        // Clear the existing path
        ClearGraphPath();

        // Initialize node maps for searching the graph
        pred.init(G,nil);
        dist.init(G,0);

        // Initilize a new open list
        if ( openp == NULL )
        {
                openp = new node_pq<double>(G); 
        }
        openp->clear();
        
        // Start the search from the current start node (if it exists)
        if ( startNode != nil )
        {
                openp->insert( startNode, 0 );  // Use 0 Priority to ensure it is the first node selected.
                dist[startNode] = 0;
                pred[startNode] = nil;
        }

        // Trick the planner into starting a new search.
        plan_success = TIMER_EXPIRED;
}


//=============================================================================
// LoadContents
//
// Purpose: Loads the graph and other related base planner parameters to the the
//                  given ifstream.
//      
//                      Returns success of load.
//=============================================================================
bool PL_Astar::LoadContents( std::ifstream& infile )
{
        // Load the Graph and other base parameters
        bool success = PL_GraphBase::LoadContents( infile );
        if ( !success )
        {
                return false;
        }
        
        // Load the A* Parameters
        infile >> stepsize;
        infile >> goalOnGrid >> weight;

        InvertStepSize();

        // Load the Node Information
        node n0;
        forall_nodes( n0, G )
        {
                int nodeID;
                infile >> nodeID;
                infile >> nodeExpanded[n0];
        }

        // Get Planner to use new graph
        InitNewSearch();

        return true;
        
}

//=============================================================================
// SaveContents
//
// Purpose: Saves the graph and other related base planner parameters to the the
//                  given ofstream.
//      
//                      Returns success of save.
//=============================================================================
bool PL_Astar::SaveContents ( std::ofstream& outfile ) const
{
        // Save the Graph and parent parameters
        bool success = PL_GraphBase::SaveContents( outfile );
        if ( !success )
        {
                return false;
        }

        // Save the A* parameters
        outfile << stepsize << endl;
        outfile << goalOnGrid << " " << weight << endl;

        // Save Node Information
        node n1;
        forall_nodes( n1, G )
        {
                outfile << n1->id() << " " << nodeExpanded[n1] << endl;
        }
        
        // Planning success will be refreshed in the load.
        
        return true;
}

//=============================================================================
// Does Planning
//
// Purpose: performs the planning task
//
//=============================================================================
bool PL_Astar::Plan ()
{
        // If both start and goal are the same configuration
        if ( startNode == goalNode )
        {
                graphPath.clear();
                TranslatePath();
                plan_success = PASS;
        }

        // Perform early checks
        if ( plan_success == PASS )
        {
                return true;
        }
        else if ( plan_success == FAIL )
        {
                return false;
        }


        // Start the timer
        StartTimer();

        // Establish the semaphore
        Semaphore semaphore( guid );
        semaphore.Lock();

        // Assume path will not be found.
        plan_success = FAIL;

        // Perform A* Search by continuing searching graph until the list of open nodes is exhausting, timer has expired, 
        // or solution has been found.

        // While OPEN is not empty
        while ( !openp->empty() )
        {
                // Check to make sure timer has not expired.
                if ( HasTimeLimitExpired() )
                {
                        // Timer has expired.

                        // current state variables are saved, include the open priority queue which is not deleted.

                        // Report the timer has expired.
                        plan_success = TIMER_EXPIRED;
                        break;
                }

                //Remove the lowest cost node from open queue
                node n1 = openp->del_min();

                // check if goal has been found
                if ( n1 == goalNode )
                // found goal.
                {
                        // Store the current path.
                        graphPath.clear();
                        graphPath.push( n1 );

                        // Starting with the goal, go up through path, adding each of the prev nodes
                        // to the path as they are found.
                        while ( n1 != startNode )
                        {
                                node n0 = pred[n1];             // find the prev node in the path.
                                
                                // Add prev node to front of the path.
                                graphPath.push( n0 );

                                // Prepare for next iteration: the prev node will be the current node.
                                n1 = n0;
                        }

                        // No longer need the open priority queue list.  Delete it.
                        delete openp;
                        openp = NULL;

                        // A path has been successfully found.
                        plan_success = PASS;
                        break;
                                        
                }
                else
                {
                        // Expand all successor nodes
                        // NOTE:  TO find the successor nodes, we search all nodes connected by edges (bi-diretional) to
                        //                the current node.  Eventually the prev node in the path will be selected, however, it will
                        //                never be updated as one of the potential successor nodes since the current cost of getting
                        //                to the current node, includes going through the previous node.  This means a newcost of the
                        //                actual cost to the current node plus the cost of travelling back from the current node
                        //                to the prev node will be higher than the cost of getting to the prev node by not passing through
                        //                the current node.  Cost:  cost[A] < cost[A] + cost(A to B) + cost(B back to A)!

                        // Add all nodes within the stepsize of the current node to the graph.
                        ExpandNode( n1 );       

                        node n2;        // successor node.  n1 = curr node.
                        edge e1;        // scanning edge.

                        // Search all the successor nodes.              
                        forall_inout_edges(e1, n1)
                        {
                                // find the successor node.   (since edges are in both directions, will have to examine
                                // both the target and source node of the edge.  If either is the current node, then the
                                // opposite node is the correct successor node. 

                                n2 = G.source(e1);
                                if ( n2 == n1)
                                {
                                        // edge was pointing from current node, so select the target node as the successor
                                        n2 = G.target(e1);
                                }

                                // check if new node is uninitialized
                                if ( ( pred[n2] == nil ) && ( n2 != startNode ) )
                                {
                                        // record the prev node
                                        pred[n2] = n1;
                                        
                                        // Assign a cost (dist) to successor node
                                        dist[n2] = dist[n1] + G.inf(e1);  // length of path upto current node point plus cost of using edge to this node
                                                
                                        //Add this node to open priority queue based on its estimated cost for the entire path
                                        openp->insert( n2, Astar_f(n2, dist[n2]) );
                                }
                                else
                                {
                                        // check if path to this node is cheaper than the existing path
                                        double currcost = dist[n2];
                                        double newcost  = dist[n1] + G.inf(e1);

                                        // if new path is cheaper, update the parameters
                                        if ( newcost < currcost )
                                        {
                                                // record the prev node.
                                                pred[n2] = n1;

                                                // Assign new cost to this node
                                                dist[n2] = newcost;

                                                // Find priority for new node:  cost upto this node + estimated distance to goal. 
                                                double priority = Astar_f(n2, dist[n2]);

                                                // check if this node is in the priority queue
                                                if ( openp->member(n2) )
                                                {
                                                        // already in queue, decrease its priority
                                                        openp->decrease_p( n2, priority );
                                                }
                                                else
                                                {
                                                        // not in queue, add it
                                                        openp->insert( n2, priority );
                                                }
                                        } // end if newcost < currcost
                                } // end if uninitialized
                        } // end forall_inout_edges

                } // end if not goal

                // Allow for Multithreaded Draws, by releasing the semaphore tempoary
                semaphore.Unlock();
                semaphore.Lock();
                                
        }  // end while open list is not empty.



        
        // Get the Path
        SuccessResultType success = TranslatePath();

        // Release semaphore permanetly.
        semaphore.Unlock();

        // Report the success of the planner.
        if ( success == PASS )
        {
                // planner has completed and a path has been found and saved.
                return true;
        }
        else
        {
                // planner has not completed and has terminated early or no valid path was found.
                return false;
        }


}  // END Plan

//=============================================================================
// SetStartConfig
//
// Purpose: Prepares for a new A* search by clearing the current graph and associated
//                  parameters, assigning the start configuration and it as a node into a 
//                      new graph.
//=============================================================================
void PL_Astar::SetStartConfig( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( startNode != nil ) && ( GetStartConfig() == config ))
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // A new configuration has been requested.  Need to clear the current graph and search parameters
        ClearGraph();

        // Set the startConfig, add it to the graph, and assign the startNode to it by
        // calling the parent's SetStartConfig.
        PL_GraphBase::SetStartConfig( config );

        // Verify the startNode was initiated.
        ASSERT( startNode != nil );

        // Make sure the start node is valid
        if ( IsInterfering( GetStartConfig() ) )
        {
                // Remove invalid start from graph.
                G.del_node( startNode );
                startNode = nil;

                // Start Interfers, there will be no valid path possible.
                plan_success = FAIL;
                return;
        }

        // Assume the current goal config is correct, but still need to add it to new graph 
        // and assign the goal node.
        PL_GraphBase::SetGoalConfig( GetGoalConfig() );

        // Verify the goalNode was initiated.
        ASSERT( goalNode != nil );

    int gSize = GetGoalConfig().Size();
    int cSize = config.Size();
        if( ( gSize != cSize ) || ( IsInterfering( GetGoalConfig() ) ) )
        {
                // Remove the invalid goal from graph
                G.del_node( goalNode );
                goalNode = nil;

                // Goal Interfers, there will be no valid path possible.
                plan_success = FAIL;
                return;
        }

        // Only allow the goal node to be expanded if it is on the grid.
        if ( GetGoalConfig().Compare( AlignConfig( GetGoalConfig() ) , COMPTOL ) )
        {
                goalOnGrid = TRUE;
                nodeExpanded[ goalNode ] = FALSE;       // making this switch false, this node will be allowed to be expanded later.
        }
        else
        {
                goalOnGrid = FALSE;
                nodeExpanded[ goalNode ] = TRUE;        // Do not allow node to be expanded, by forcing it to report expanded.
        }

        // Initialize for a new search
        InitNewSearch();

        // This is a fresh graph, so need to ensure the start node can be expanded
        nodeExpanded[ startNode ] = FALSE;
        
}

//=============================================================================
// SetGoalConfig
//
// Purpose: Prepares for a new A* search by moving the goal node and performing
//                      updates in graph parameters to request a new search.
//=============================================================================
void PL_Astar::SetGoalConfig( const Configuration& config )
{
        // Check if the configuration has changed.
        if (( goalNode != nil ) && ( GetGoalConfig() == config ))
        {
                // no change in configuration.  Can exit early.
                return;
        }

        // Remove old goal from graph if it was not on the grid
        if ( !goalOnGrid )
        {
                G.del_node( goalNode );
                goalNode = nil;
        }

        // Set the goalConfig, add it to the graph, and assign the goalNode to it by
        // calling the parent's SetGoalConfig.
        int oldNumNodes = NodeCount();
        PL_GraphBase::SetGoalConfig( config );


        // Verify the goalNode was initiated.
        ASSERT( goalNode != nil );

        // Ensure the new goal is valid
        if ( IsInterfering( GetGoalConfig() ) )
        {
                G.del_node( goalNode );
                goalNode = nil;

                // Goal interferes.  There is no point in performing a search.  Exit early
                plan_success = FAIL;
                return;
        }

        // Determine if new goal is on grid and is allowed to be expanded.
        if ( GetGoalConfig().Compare( AlignConfig( GetGoalConfig() ) , COMPTOL ) )
        {
                goalOnGrid = TRUE;
                
                if ( oldNumNodes == NodeCount() )
                {
                        // Do not change status of node being expanded, since goal is one of the existing nodes in graph
                }
                else
                {
                        nodeExpanded[ goalNode ] = FALSE;       // This is a new node.  It has not been expanded.
                }
        }
        else
        {
                goalOnGrid = FALSE;
                nodeExpanded[ goalNode ] = TRUE;        // Do not allow node to be expanded, by forcing it to report expanded.

                // Since goal is not allowed to be expanded, have to check if it is already among expanded nodes and
                // connect goal to neighbours
                node n;
                forall_nodes( n, G )
                {
                        if ( n != goalNode )
                        {
                                // Connect edge if node is within 1 grid box of goal and does not cause a collision
                                Configuration testConfig = G.inf(n);
                                if ( (Distance( testConfig, GetGoalConfig(), invStepSize ) < 1) && (!IsInterfering( testConfig, GetGoalConfig() )) )
                                {
                                        double dist = sqrt( Distance( testConfig, GetGoalConfig() ) );
                                        edge e1 = G.new_edge( goalNode, n, dist );
                                } // end if within 1 grid box
                        } // end if not goalNode
                } // end for all nodes
        }  // end if not on grid.


        // Initialize for a new search
        InitNewSearch();

}

//=============================================================================
// SetCollisionDetector
//
// Purpose: Assigns the CollisionDetector and extracts the DOF from it to allow
//                  other graph specific parameters to be defined.
//=============================================================================
void PL_Astar::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        // Call the parent's SetCollisionDetector to ensure proper assignment.
        PL_GraphBase::SetCollisionDetector( collisionDetector );


        // Initialize to the default step size if the step size has not already been initialized.
        if ( stepsize.Length() != collisionDetector->DOF() )
        {
                SetDefaultStepSize();
        }

}

//=============================================================================
// Astar_f
//
// Purpose: Estimates the cost of the path based on the given node.
//                      Returns the cost of the path upto the current node (given as a parameter)
//                      and estimates the cost from current node to the goal.
//=============================================================================
double PL_Astar::Astar_f( const node& n, const double& currcost ) const
{
        return (1-weight)*currcost + weight*sqrt(Distance( n, goalNode ));  
}


//=============================================================================
// InvertStepSize
//
// Purpose: Inverts the current step size for each of the joints and assigns
//                      it to invStepSize.
//=============================================================================
void PL_Astar::InvertStepSize()
{
        // Equate the size of the vector.
        int length = stepsize.Length();
        invStepSize.SetLength( length );

        // Get inverse of each of the steps.
        for (int i = 0; i < length; i++ )
        {
                invStepSize[i] = double( 1 / stepsize[i] );
        }

}

//=============================================================================
// SetStepSize
//
// Purpose: Assigns the step size for each joint based on the given parameter
//                      If the new step size has changed, the graph is cleared.
//=============================================================================
void PL_Astar::SetStepSize( const VectorN& newstepsize )
{
        if ( stepsize == newstepsize )
        {
                return; // step size is the same.  Exit early.
        }

        // Assign new stepsize.
        stepsize = newstepsize;

        // Store the inverse of each step
        InvertStepSize();

        // Clear the Graph
        ClearGraph();

}

//=============================================================================
// SetDefaultStepSize
//
// Purpose: Assigns the default step size for each joint based on the joint limits
//=============================================================================
void PL_Astar::SetDefaultStepSize()
{
        int dof = 0;

        if ( collisionDetector != NULL )
        {
                dof = collisionDetector->DOF();
        }
        
        stepsize.SetLength( dof );

        for ( int i = 0; i < dof; i++)
        {
                stepsize[i] = double( (collisionDetector->JointMax(i) - collisionDetector->JointMin(i)) / DEFAULTNUMSTEPS );
        }

        // Store the inverse of each step.
        InvertStepSize();

}

//=============================================================================
// AlignJoint
//
// Purpose: Returns an alligned joint value on a grid that is created from
//                      the step size and start configuration, closest to the given joint value
//=============================================================================
double PL_Astar::AlignJoint( const int& jointNum, const double& jointValue ) const
{
        double start = GetStartConfig()[jointNum];
        double step  = stepsize[jointNum];

        double diff = jointValue - start;

        int numofsteps = diff / step;   // number will truncate towards zero.

        double align = start + numofsteps*step;

        // check if we are within 1/2 a step of proposed joint value
        if ( fabs( jointValue - align ) < ( 0.5 * step ) )
        {
                return align;
        }
        else if ( diff > 0 )            // truncate towards zero, so make sure we step in the right direction to get closest grid allignment
        {
                double align2 = align + step;
                if ( align2 > collisionDetector->JointMax( jointNum ) )  // ensure we do not go over the limits
                {
                        return align;
                }
                else
                {
                        return align2;
                }
        }
        else
        {
                double align2 = align - step;
                if ( align2 < collisionDetector->JointMin( jointNum ) )  // ensure we do not go over the limits
                {
                        return align;
                }
                else
                {
                        return align2;
                }
        }

}

//=============================================================================
// AlignConfig
//
// Purpose: Returns an alligned configuration on a grid that is created from
//                      the step size and start configuration, closest to the given config
//=============================================================================
Configuration PL_Astar::AlignConfig( const Configuration& config ) const
{
        int dof = config.DOF();

        Configuration returnMe;
        returnMe.SetNumDOF( dof );

        for ( int i = 0; i < dof; i++ )
        {
                returnMe[i] = AlignJoint( i, config[i] );
        }

        return returnMe;

}

//=============================================================================
// ExpandNode
//
// Purpose: Expands the node by finding all valid configurations within specified
//                  stepsize and connects to them with valid edges.
//
//                      Only expands nodes that were not previously expanded.
//=============================================================================
void PL_Astar::ExpandNode( const node& n )
{
        // Check if node has already been expanded and return early if TRUE.
        if ( nodeExpanded[n] )
        {
                return;
        }

        int dof = collisionDetector->DOF();

        // Create list of nodes already connected to current node
        node_list neighbours;
        edge e1;
        forall_inout_edges( e1, n)
        {
                node n1 = G.source(e1);
                if ( n1 == n )
                {
                        n1 = G.target(e1);
                }

                neighbours.append(n1);
        }

        // Search all configurations within stepsize of current node.
        Configuration currentconfig = G.inf(n);
        for( int i = 0; i < dof; i++)
        {
                BOOL jointWraps = collisionDetector->JointWraps( i );
                double jmax = collisionDetector->JointMax( i );
                double jmin = collisionDetector->JointMin( i );

                for( int j = -1; j < 2; j+=2 )  // perform check for both forward and backward step.
                {
                        Configuration newconfig = currentconfig;
                        double newjoint = currentconfig[i] + (j*stepsize[i]);
                        

                        // With wrapping joints, need to be able to wrap around the limits but ensure the resulting wrap 
                        // remains alligned to the grid.  Need to check first if the joint wraps, and then if it
                        // exceeds any of the limits, realign it to the grid using the corresponding joint limit.
                        
                        if ( jointWraps )
                        {
                                // Joint Wraps, check limits first and then align.
                                if ( newjoint > jmax )
                                {
                                        // Want to have joint at smallest grid alligned value.
                                        newjoint = AlignJoint( i, jmin );
                                }
                                else if ( newjoint < jmin )
                                {
                                        // Want to have joint at largest grid alligned value.
                                        newjoint = AlignJoint( i, jmax );
                                }
                        }
                        else
                        {       
                                // joint does not wrap.  Hence, if the new joint value exceeds the joint limits, it
                                // is invalid and the resulting configuration should not be added to graph.
                                if ( ( newjoint > jmax ) || ( newjoint < jmin ) )
                                {
                                        // joint exceeds limit
                                        continue;
                                }
                                
                        }

                        // With realigning the joint, there is a chance the new config is now the same as the current config.
                        // To ensure we do not add a duplicate config to the graph a second time with a 0 distance edge, check if the
                        // new joint is the same as the current joint and escape if ture.
                        if ( newjoint == currentconfig[i] )
                        {
                                continue;       
                        }


                        // Update new config with new joint value
                        newconfig[i] = newjoint;

                        // Verify new configuration is valid
                        if ( IsInterfering( newconfig ) )
                        {
                                continue;       // configuration causes collision.  Don't preform further checks, but start next iteration
                        }

                        // Check if new config is already graph connected to node by checking its neighbours
                        node n1;
                        bool configFound = FALSE;
                        forall( n1, neighbours )
                        {
                                // use the compare function using a tolerance to prevent floating point math errors.
                                if ( newconfig.Compare( G.inf(n1) , COMPTOL ) )
                                {
                                        configFound = TRUE;
                                        break;  // break out check of neighbours.
                                }
                        }
                        if ( configFound )
                        {
                                continue;  // this config already exists.  Search next iteration.
                        }

                        // Search for config in existing graph, and add it if it is not found.
                        n1 = FindConfig( newconfig );  // uses the compare function using a tolerance to prevent floating point math errors.
                        if ( n1 == nil )
                        {
                                // Add the node
                                n1 = G.new_node( newconfig );
                                nodeExpanded[ n1 ] = FALSE;                     // new node has not been expanded.
                                pred[ n1 ] = nil;                                       // node has just been initialized.
                                dist[ n1 ] = 0;

                        }

                        // Connect configuration to current node if a collision free edge exists
                        if ( !IsInterfering( newconfig, currentconfig ) )
                        {
                                double dist = sqrt( Distance( newconfig, currentconfig ) );
                                edge e1 = G.new_edge( n1, n, dist );            // edge points to current node.
                        }

                }  // end for j
        }  // end for i


        // If the goal is not on the grid, check to if it is within 1 grid box of current node and connect if valid.
        if ( ( !goalOnGrid ) && 
                 ( Distance( currentconfig, GetGoalConfig(), invStepSize ) < 1 ) && 
                 ( !IsInterfering( currentconfig, GetGoalConfig() ) )  )
        {
                double dist = sqrt( Distance( GetGoalConfig(), currentconfig ) );
                edge e1 = G.new_edge( goalNode, n, dist );              // edge points from goal to current node
        }

        // Current node has just been fully expanded.
        nodeExpanded[ n ] = TRUE;

}

//=============================================================================
// CopySettings
//
// Purpose: copies the settings from a different planner
//=============================================================================
void PL_Astar::CopySettings( PlannerBase* original )
{
        PL_GraphBase::CopySettings( original );

        PL_Astar* originalAstar = dynamic_cast< PL_Astar* >( original );
        if( originalAstar == NULL )
        {
                return;
        }
        this->stepsize = originalAstar->stepsize;
        this->invStepSize = originalAstar->invStepSize;
        this->weight = originalAstar->weight;

}

---