planners/prm/PL_PrmIjg.cpp

Go to the documentation of this file.

#pragma warning( disable : 4786 )

#include <synchronization/semaphore.h>
#include <algorithm>
#include "CollisionDetectors/CD_Bool.h"
#include "color/colorf.h"
#include "debug/debug.h"
#include "math/astar.h"
#include "PL_PrmIjg.h"
#include <gl\gl.h>
#include "smoothers\SmootherBase.h"

PL_PrmIjg::PL_PrmIjg():
    m_PlanComplete( false ),
    m_NumberOfTimesContinued( 0 )
{
    m_Graphs.reserve( 10000 );
}

PL_PrmIjg::~PL_PrmIjg()
{
}

//=============================================================================
// AddNodeToGraph
//
// generates a random configuraiton in CSpace
//=============================================================================
bool PL_PrmIjg::AddNodeToGraph( const int graphNum, const Configuration& config )
{
    bool configInCollision = collisionDetector->IsInterfering( config );
    bool graphInCollision = !m_GraphInFreeSpace[ graphNum ];
    if( configInCollision != graphInCollision )
    {
        return false;
    }

    //
    // Get the graph
    //
    PrmGraph& graph = m_Graphs[ graphNum ];

    //
    // Connect the node to the closest other node in the graph
    //
    int closestNode = -1;
    double closestNodeValue = 9999999999; //make this DBL_MAX?
    unsigned size = graph.GetNumNodes();
    unsigned i;
    for( i = 0; i < size; ++i )
    {
        const Configuration& c = graph.GetNodeData( i );
        double distance = ( c - config ).MagSquared();
        if( distance < closestNodeValue )
        {
            closestNodeValue = distance;
            closestNode = i;
        }
    }
/*
    //
    // Find the closest edge in the graph to this point - more often than
    // not, new points will connect best to interior points of existing
    // edges
    //
    int closestEdge = -1;
    double closestEdgeValue = 9999999999;
    size = graph.GetNumEdges();
    for( i = 0; i < size; ++i )
    {
        const Graph::Edge& e = graph.GetEdge( i );
        int n0 = e.m_Node0;
        int n1 = e.m_Node1;
        const Configuration& c0 = graph.GetNodeData( n0 );
        const Configuration& c1 = graph.GetNodeData( n1 );
        double distance = DistanceSquared( c0, c1, config );
        if( distance < closestEdgeValue )
        {
            closestEdgeValue = distance;
            closestEdge = i;
        }
    }

    if( closestEdgeValue < closestNodeValue )
    //if( false )
    {
        //
        // We need to subdivide an edge in the graph
        //
        const Graph::Edge& e = graph.GetEdge( closestEdge );
        int n0 = e.m_Node0;
        int n1 = e.m_Node1;
        const Configuration& c0 = graph.GetNodeData( n0 );
        const Configuration& c1 = graph.GetNodeData( n1 );
        Configuration& closestPoint = ClosestPoint( c0, c1, config );
        bool completelyFree = !collisionDetector->IsInterferingLinear( config, closestPoint );
        if( completelyFree )
        {
            int subdividedNode = graph.AddNode( closestPoint );
            int newNode = graph.AddNode( config );
            graph.AddEdge( n0, subdividedNode );
            graph.AddEdge( n1, subdividedNode );
            graph.AddEdge( newNode, subdividedNode );
            return true;
        }
    }
*/
    //
    // Add an edge connection to the closest node
    //
    if( closestNode != -1 )
    {
        Configuration closestConfig = graph.GetNodeData( closestNode );
        IJG_Assert( config.DOF() != 0 );
        IJG_Assert( closestConfig.DOF() != 0 );
        //IJG_Assert( completelyFree != completelyInCollision );
        if( graphInCollision )
        {   
            bool completelyInCollision = collisionDetector->IsCompletelyWithinObstacles( config, closestConfig );
            if( completelyInCollision )
            {
                int nodeNum = graph.AddNode( config );
                graph.AddEdge( nodeNum, closestNode );
                return true;
            }
        }

        if( !graphInCollision )
        {
            bool completelyFree = !collisionDetector->IsInterferingLinear( config, closestConfig );
            if( completelyFree )
            {
                int nodeNum = graph.AddNode( config );
                graph.AddEdge( nodeNum, closestNode );
                return true;
            }
        }
    }
    return false;
}

//=============================================================================
// CreateNewGraph
//
// Creates a new graph with this configuration as a seed
//=============================================================================
int PL_PrmIjg::CreateNewGraph( const Configuration& config )
{
    //
    // Add a graph to the array of graphs
    //
    int currentSize = m_Graphs.size();
    m_Graphs.resize( currentSize + 1 );
    int size = currentSize + 1;
    PrmGraph& graph = m_Graphs[ size - 1 ];
    graph.AddNode( config );

    //
    // Add a boolean flag to the list of bools
    //
    bool interfering = collisionDetector->IsInterfering( config );
    m_GraphInFreeSpace.push_back( !interfering );
    return size - 1;
}

//=============================================================================
// GenerateRandomConfig
//
// generates a random configuraiton in CSpace
//=============================================================================
bool PL_PrmIjg::DrawExplicit() const
{
        // Lock Graph for Drawing
        Semaphore semaphore( this->guid );
        semaphore.Lock();

    //
    // Draw all the nodes
    //
        glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        glDisable( GL_LIGHTING );
        glShadeModel( GL_SMOOTH );
    glPointSize( 2.0 );

    Color::Colorf freeNodeColor     = Color::green;
    Color::Colorf obstacleNodeColor = Color::red;
    Color::Colorf freeEdgeColor     = freeNodeColor - Color::opaque * 0.5;
    Color::Colorf obstacleEdgeColor = obstacleNodeColor - Color::opaque * 0.5;


    int i;
    int size = m_Graphs.size();
    for( i = 0; i < size; ++i )
    {
        //
        // Get the graph
        //
        const PrmGraph& graph = m_Graphs[ i ];
        const bool isInFreeSpace = m_GraphInFreeSpace[ i ];
        if( isInFreeSpace )
        {
            freeNodeColor.GlDraw();
        }
        else
        {
            obstacleNodeColor.GlDraw();
        }
        SetColor( i );

        //
        // Draw all the points
        //
        glBegin( GL_POINTS );
        {
            const int size = graph.GetNumNodes();
            int i;
            for( i = 0; i < size; ++i )
            {
                const Configuration& node = graph.GetNodeData( i );
                Draw( node );
            }
        }
        glEnd();

        //
        // Draw all the edges
        //
        if( isInFreeSpace )
        {
            Color::green;
            freeEdgeColor.GlDraw();
        }
        else
        {
            obstacleEdgeColor.GlDraw();
        }
        SetColor( i );


        glLineWidth( 1.0f );
        glBegin( GL_LINES );
        {
            const int size = graph.GetNumEdges();
            int i;
            for( i = 0; i < size; ++i )
            {
                const Graph::Edge& e = graph.GetEdge( i );
                const Configuration& c0 = graph.GetNodeData( e.m_Node0 );
                const Configuration& c1 = graph.GetNodeData( e.m_Node1 );
                Draw( c0 );
                Draw( c1 );
            }
        }
        glEnd();
    }

        // Unlock Semaphore
        semaphore.Unlock();
        Sleep( 15 );

    return true;
}

//=============================================================================
// DrawOverlay
//
// draws a configuration in "world" space
//=============================================================================
void PL_PrmIjg::DrawOverlay( const Configuration& config ) const
{
    float x = 0.0f;
    float y = 0.0f;
    float z = 0.0f;
    const unsigned int DOF = config.DOF();
    if( DOF > 0 )
    {
        x = config[ 0 ];
    }
    if( DOF > 1 )
    {
        y = config[ 1 ];
    }
    if( DOF > 2 )
    {
        z = config[ 2 ];
    }
    glVertex3f( y, -x, z );
}

//=============================================================================
// DrawUniversePortion
//
// Gets the path out of the graph, now that we know there is one
//=============================================================================
void PL_PrmIjg::DrawUniversePortion() const
{
    //
    // I've only written this for 2dof translational robots
    //
    unsigned int dof = collisionDetector->DOF();
    if( dof != 2 )
    {
        return;
    }

        // Lock Graph for Drawing
        Semaphore semaphore( this->guid );
        semaphore.Lock();

    //
    // Draw all the nodes
    //
        glDisable( GL_LIGHTING );
        glShadeModel( GL_SMOOTH );
    glPointSize( 2.0 );

    Color::Colorf freeNodeColor     = Color::green;
    Color::Colorf obstacleNodeColor = Color::red;
    Color::Colorf freeEdgeColor     = freeNodeColor - Color::opaque * 0.5;
    Color::Colorf obstacleEdgeColor = obstacleNodeColor - Color::opaque * 0.5;


    int i;
    int size = m_Graphs.size();
    for( i = 0; i < size; ++i )
    {
        //
        // Get the graph
        //
        const PrmGraph& graph = m_Graphs[ i ];
        const bool isInFreeSpace = m_GraphInFreeSpace[ i ];
        if( isInFreeSpace )
        {
            freeNodeColor.GlDraw();
        }
        else
        {
            obstacleNodeColor.GlDraw();
        }
        SetColor( i );

        //
        // Draw all the points
        //
        glBegin( GL_POINTS );
        {
            const int size = graph.GetNumNodes();
            int i;
            for( i = 0; i < size; ++i )
            {
                const Configuration& node = graph.GetNodeData( i );
                DrawOverlay( node );
            }
        }
        glEnd();

        //
        // Draw all the edges
        //
        if( isInFreeSpace )
        {
            Color::green;
            freeEdgeColor.GlDraw();
        }
        else
        {
            obstacleEdgeColor.GlDraw();
        }
        SetColor( i );


        glLineWidth( 1.0f );
        glBegin( GL_LINES );
        {
            const int size = graph.GetNumEdges();
            int i;
            for( i = 0; i < size; ++i )
            {
                const Graph::Edge& e = graph.GetEdge( i );
                const Configuration& c0 = graph.GetNodeData( e.m_Node0 );
                const Configuration& c1 = graph.GetNodeData( e.m_Node1 );
                DrawOverlay( c0 );
                DrawOverlay( c1 );
            }
        }
        glEnd();
    }

        // Unlock Semaphore
        semaphore.Unlock();
        Sleep( 15 );
}

//=============================================================================
// GenerateRandomConfig
//
// generates a random configuraiton in CSpace
//=============================================================================
Configuration PL_PrmIjg::GenerateRandomConfig() const
{
        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 ;
}

//=============================================================================
// MergeGraphs
//
// merges two formerly disjoint subgraphs together
//=============================================================================
void PL_PrmIjg::MergeGraphs( const int base, const int merge, const Configuration& config )
{
    //
    // Check if we're done
    //
    if( ( base == 0 ) && ( merge == 1 ) )
    {
        m_PlanComplete = true;
    }

    PrmGraph& baseGraph = m_Graphs[ base ];
    PrmGraph& mergeGraph = m_Graphs[ merge ];

    baseGraph.Add( mergeGraph );
    mergeGraph.Clear();     //really aught to erase this graph outright

    //
    // find the nodes that match config
    //
    int firstMatch = -1;
    int i;
    int size = baseGraph.GetNumNodes();
    for( i = 0; i < size; ++i )
    {
        const Configuration& baseConfig = baseGraph.GetNodeData( i );
        double difference = ( baseConfig - config ).Magnitude();
        if( baseConfig == config )
        {
            firstMatch = i;
            break;
        }
    }

    bool graphsConnected = false;
    IJG_Assert( firstMatch != -1 );
    for( ++i; i < size; ++i )
    {
        if( baseGraph.GetNodeData( i ) == config )
        {
            graphsConnected = true;
            baseGraph.AddEdge( firstMatch, i );
        }
    }
    IJG_Assert( graphsConnected );
}

//=============================================================================
// Plan
//
// does all the palnning work
//=============================================================================
bool PL_PrmIjg::Plan ()
{
        // Lock Graph for Drawing
        Semaphore semaphore( this->guid );
        semaphore.Lock();

    //
    // check if we're starting or continuing
    //
    if( m_Graphs.size() == 0 )
    {

        //
        // starting
        //
        CreateNewGraph( GetStartConfig() );
        CreateNewGraph( GetGoalConfig() );
    }
    else
    {
        //
        // continuing
        //
        ++m_NumberOfTimesContinued;
        srand( m_NumberOfTimesContinued );
    }

    // Start the timer
        StartTimer();

    while( ( !m_PlanComplete ) && ( !HasTimeLimitExpired() ) )
    {
        semaphore.Unlock();
        semaphore.Lock();
        Configuration config = GenerateRandomConfig();

        //
        // Try adding the new config to all the graphs 1 by 1
        //
        std::vector< int > graphsAddedTo;
        int i;
        int size = m_Graphs.size();
        bool successfullyAdded = false;
        for( i = 0; i < size; ++i )
        {
            bool addedToThisGraph = AddNodeToGraph( i, config );
            successfullyAdded |= addedToThisGraph;
            if( addedToThisGraph )
            {
                IJG_Assert( m_Graphs[ i ].ContainsNode( config ) );
                graphsAddedTo.push_back( i );
            }
        }

        //
        // This caused a join of 2 or more subgraphs
        //
        if( graphsAddedTo.size() > 1 )
        {
            std::sort( graphsAddedTo.begin(), graphsAddedTo.end() );
            int baseGraphNum = graphsAddedTo[ 0 ];
            int i;
            int size = graphsAddedTo.size();
            for( i = 1; i < size; ++i )
            {
                int mergeGraphNum = graphsAddedTo[ i ];
                MergeGraphs( baseGraphNum, mergeGraphNum, config );
                Graph::GraphBase& g = m_Graphs[ baseGraphNum ];
                bool ok = g.CheckIntegrity();
                if( !ok )
                {
                    this->PrintGraph( baseGraphNum );
                }
                IJG_Assert( ok );
            }
        }

        //
        // This is an entirely new subgraph
        //
        if( !successfullyAdded )
        {
            if( !collisionDetector->IsInterfering( config ) )
            {
                CreateNewGraph( config );
            }
        }
    }

    //
    // Do a graph search from the start to the goal, to actually get the path
    //
    if( !HasTimeLimitExpired() )
    {
        //
        // which is the node that we're searching for?
        //
        int goalNodeNum = -1;
        int i;
        int size = m_Graphs[ 0 ].GetNumNodes();
        for( i = 0; i < size; ++i )
        {
            const Configuration& c = m_Graphs[ 0 ].GetNodeData( i );
            if( c == GetGoalConfig() )
            {
                goalNodeNum = i;
                break;
            }
        }
        IJG_Assert( goalNodeNum != -1 );

        Graph::Astar search;
        search.Search( m_Graphs[ 0 ], 0, goalNodeNum );
        std::vector< int > results = search.GetPath();

        size = results.size();
        for( i = 0; i < size; ++i )
        {
            int nodeNumber = results[ i ];
            const Configuration& c = m_Graphs[ 0 ].GetNodeData( nodeNumber );
            path.AppendPoint( c );
        }
    }

        if( this->m_Smoother != NULL )
        {
                this->m_Smoother->SetPath( &this->path );
                this->m_Smoother->SetCollisionDetector( this->collisionDetector );
                m_Smoother->Smooth();
                this->path = m_Smoother->GetPath();
                m_Smoother->SetPath( NULL );
        }

    semaphore.Unlock();
    if( HasTimeLimitExpired() )
    {
        return false;
    }
        return true ;
}

//=============================================================================
// PlanMultiThread
//
// does all the palnning work
//=============================================================================
void PL_PrmIjg::PlanMultiThread (void* data)
{
    //nothing???
}

//=============================================================================
// PrintGraph
//
// prints out the graph for debugging purposes
//=============================================================================
void PL_PrmIjg::PrintGraph( const int graphNum ) const
{
    const PrmGraph& graph = m_Graphs[ graphNum ];

    //
    // Print all the node data values
    //
    int i;
    int size = graph.GetNumNodes();
    for( i = 0; i < size; ++i )
    {
        const Configuration& config = graph.GetNodeData( i );
        printf( "%d\t", i );
        config.Output();
        printf( "\n" );
    }

    //
    // Print all of the edges
    //
    printf( "Edges\n" );
    size = graph.GetNumEdges();
    for( i = 0; i < size; ++i )
    {
        const Graph::Edge& e = graph.GetEdge( i );
        printf( "[%d]\t%d\t%d\n", i, e.m_Node0, e.m_Node1 );
    }

    //
    // Print which nodes contain which edges
    //
    printf( "Nodes\n" );
    size = graph.GetNumNodes();
    for( i = 0; i < size; ++i )
    {
        printf( "[%d]\t", i );
        const Graph::Node& n = graph.GetNode( i );
        int edgeSize = n.GetNumEdges();
        int j;
        for( j = 0; j < edgeSize; ++j )
        {
            int edge = n.GetEdgeNumber( j );
            printf( "%d\t", edge );
        }
        printf( "\n" );
    }
}

//=============================================================================
// SetColor
//
// sets the GL color to a distinct color
//=============================================================================
void  PL_PrmIjg::SetColor( const int i )
{
    int modulo = i % 4;
    switch( modulo )
    {
    case 0:
    {
        Color::red_50.GlDraw();
        return;
    }
    case 1:
    {
        Color::green_50.GlDraw();
        return;
    }
    case 2:
    {
        Color::yellow_50.GlDraw();
        return;
    }
    case 3:
    {
        Color::cyan_50.GlDraw();
        return;
    }
    default:
        Color::white.GlDraw();
        return;
    }
}

---