planners/neural/PL_Neural.cpp

Go to the documentation of this file.

#pragma warning( disable : 4786 )

#include <synchronization/semaphore.h>
#include "debug/debug.h"
#include "color/colorf.h"
#include "math/math2.h"
#include "planners/neural/pl_neural.h"
#include <gl\gl.h>

//=============================================================================
// PL_Neural::PL_Neural
//=============================================================================
PL_Neural::PL_Neural()
{
}

//=============================================================================
// PL_Neural::~PL_Neural
//=============================================================================
PL_Neural::~PL_Neural()
{
}

//=============================================================================
// PL_Neural::DrawExplicit
//=============================================================================
void PL_Neural::AddObstaclePoint( const Configuration& c ) const
{
    m_ObstacleLocations.push_back( c );
}

//=============================================================================
// PL_Neural::CheckPathForObstacles
//=============================================================================
bool PL_Neural::CheckPathForObstacles()
{
    int n = NodesInCollision();
    int e = EdgeInCollision();

    if( ( n == -1 ) && ( e == -1 ) )
    {
        return false;
    }
    return true;
}

//=============================================================================
// PL_Neural::ClampAllPointsToDofLimits
//=============================================================================
void PL_Neural::ClampAllPointsToJointLimits()
{
    int i;
    int size = m_Path.Size();
    for( i = 0; i < size; ++i )
    {
        Configuration& c = m_Path[ i ];
        c = ClampToJointLimits( c );
    }
}

//=============================================================================
// PL_Neural::ClampAllPointsToDofLimits
//=============================================================================
Configuration PL_Neural::ClampToJointLimits( const Configuration& c ) const
{
    int i;
    int size = c.DOF();
    Configuration returnMe = c;
    for( i = 0; i < size; ++i )
    {
        double min = collisionDetector->JointMin( i );
        double max = collisionDetector->JointMax( i );
        returnMe[ i ] = Clamp( returnMe[ i ], min, max );
    }
    return returnMe;
}

//=============================================================================
// PL_Neural::ClosestObstacleToEdge
//=============================================================================
int PL_Neural::ClosestObstacleToEdge ( const unsigned int edgeNum ) const
{
    int size = m_ObstacleLocations.size();
    if( size == 0 )
    {
        return -1;
    }
    const Configuration& c0 = m_Path[ edgeNum + 0 ];
    const Configuration& c1 = m_Path[ edgeNum + 1 ];

    double bestDist = DistanceSquared( c0, c1, m_ObstacleLocations[ 0 ] );
    int    bestObstacle = 0;
    int i;
    for( i = 0; i < size; ++i )
    {
        const Configuration& p = m_ObstacleLocations[ i ];
        double distanceSqr = DistanceSquared( c0, c1, p );
        if( distanceSqr < bestDist )
        {
            bestDist = distanceSqr;
            bestObstacle = i;
        }
    }
    return bestObstacle;
}

//=============================================================================
// PL_Neural::ClosestObstacleToPoint
//=============================================================================
int PL_Neural::ClosestObstacleToPoint( const Configuration& c ) const
{
    int size = m_ObstacleLocations.size();
    if( size == 0 )
    {
        return -1;
    }

    double closestDistanceSquared = ( m_ObstacleLocations[ 0 ] - c ).MagSquared();
    int bestPoint = 0;
    int i;
    for( i = 0; i < size; ++i )
    {
        double distanceSqr = ( m_ObstacleLocations[ i ] - c ).MagSquared();
        if( distanceSqr < closestDistanceSquared )
        {
            closestDistanceSquared = distanceSqr;
            bestPoint = i;
        }
    }
    return bestPoint;
}

//=============================================================================
// PL_Neural::DrawExplicit
//=============================================================================
bool PL_Neural::DrawExplicit() const
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();
    Color::yellow_50.GlDraw();
    Draw( m_Path );

    //
    // Draw obstacle locations
    //
    Color::red.GlDraw();
    glBegin( GL_POINTS );
    int i;
    int size = m_ObstacleLocations.size();
    for( i = 0; i < size; ++i )
    {
        const Configuration& c = m_ObstacleLocations[ i ];
        Draw( c );
    }
    glEnd();

    semaphore.Unlock();
    return false;
}

//=============================================================================
// PL_Neural::DrawUniversePortion
//=============================================================================
void PL_Neural::DrawUniversePortion() const
{
        Semaphore semaphore( guid );
        semaphore.Lock();

        glDisable( GL_LIGHTING );
        glShadeModel( GL_SMOOTH );
    glPointSize( 2.0 );

    Color::yellow_50.GlDraw();
    glPointSize( 2.0f );
    glLineWidth( 1.0f );
    DrawOverlay( m_Path );

    glDisable( GL_DEPTH_TEST );
    //
    // Draw obstacle locations
    //
    glBegin( GL_POINTS );
    Color::red.GlDraw();
    int i;
    int size = m_ObstacleLocations.size();
    for( i = 0; i < size; ++i )
    {
        const Configuration& c = m_ObstacleLocations[ i ];
        DrawOverlay( c );
    }
    glEnd();

    //
    // draw repulsion vectors
    //
/*
    int x;
    int y;
    Color::cyan.GlDraw();
    for( x = -10; x < 10; ++x )
    {
        for( y = -10; y < 10; ++y )
        {
            Configuration config( VectorN( x, y, 0 ) );
            Configuration r = RepulsiveVector( config );
            r.Normalize();
            r *= 0.5;
            DrawArrorOverlay( config, r );
        }
    }
*/
    semaphore.Unlock();
}

//=============================================================================
// PL_Neural::EdgeInCollision
//=============================================================================
int PL_Neural::EdgeInCollision() const
{
    int i;
    int size = m_Path.Size() - 1;
    int returnMe = -1;
    for( i = 0; i < size; ++i )
    {
        const Configuration& c0 = m_Path[ i + 0 ];
        const Configuration& c1 = m_Path[ i + 1 ];
        bool collision = collisionDetector->IsInterferingLinear( c0, c1 );
        if( collision )
        {
            Configuration c = collisionDetector->GetLastIntersection();
            AddObstaclePoint( c );
            returnMe = i;
        }
    }
    return returnMe;
}

//=============================================================================
// PL_Neural::GenerateRandomConfig 
//=============================================================================
Configuration PL_Neural::EdgeRepulsion( const unsigned int edgeNum ) const
{
    Configuration repulsion = m_Path[ 0 ] * 0.0;
    Configuration c0 = m_Path[ edgeNum + 0 ];
    Configuration c1 = m_Path[ edgeNum + 1 ];

    int i;
    int size = m_ObstacleLocations.size();
    for( i = 0; i < size; ++i )
    {
        Configuration obstacleLocation = m_ObstacleLocations[ i ];
        VectorN closestPoint = ClosestPoint( c0, c1, obstacleLocation );
        closestPoint -= obstacleLocation;
        double magSquared = closestPoint.MagSquared();
        if( magSquared == 0.0 )
        {
            closestPoint *= 0.0;
        }
        else
        {
            closestPoint /= magSquared;
        }
        repulsion += closestPoint;
    }

    return repulsion;
}

//=============================================================================
// PL_Neural::GenerateRandomConfig 
//=============================================================================
Configuration PL_Neural::GenerateRandomConfig () const
{
    Configuration returnMe = GetStartConfig();
    int i;
    int size = returnMe.Size();
    for( i = 0; i < size; ++i )
    {
        double max = collisionDetector->JointMax( i );
        double min = collisionDetector->JointMin( i );
        double random = rand() / static_cast< double >( RAND_MAX );
        random = min + random * ( max - min );
        returnMe[ i ] = random;
    }
    return returnMe;
}

//=============================================================================
// PL_Neural::NodesInCollision
//=============================================================================
int PL_Neural::NodesInCollision() const
{
    int nodeInCollision = -1;
    int i;
    int size = m_Path.Size();
    for( i = 1; i < size - 1; ++i )
    {
        const Configuration& c = m_Path[ i ];
        bool collision = collisionDetector->IsInterfering( c );
        if( collision )
        {
            AddObstaclePoint( c );
            nodeInCollision = i;
        }
    }
    return nodeInCollision;
}

//=============================================================================
// PL_Neural::Perturb
//=============================================================================
void PL_Neural::Perturb( const unsigned int pointNum )
{
    if( pointNum == 0 )
    {
        return;
    }
    unsigned int size = m_Path.Size();
    if( pointNum == size - 1 )
    {
        return;
    }
    
    Configuration randomDirection = GenerateRandomConfig();
    m_Path[ pointNum ] += randomDirection * 0.5;
}

//=============================================================================
// PL_Neural::Plan 
//=============================================================================
bool PL_Neural::Plan()
{
    //
    // Initialize the planner
    //
    const Configuration& start = GetStartConfig();
    const Configuration& goal =  GetGoalConfig();

    int i;
    int size = 10;
    for( i = 0; i <= size; ++i )
    {
        double percent = static_cast< double >( i ) / size;
        m_Path.AppendPoint( start + ( goal - start ) * percent );
    }

    for( i = 0; i <= size; ++i )
    {
        Perturb( i );
    }


        StartTimer();
        while( !HasTimeLimitExpired() )
        {
                Semaphore semaphore( guid );
                semaphore.Lock();
            RelaxUntilSettled();
            bool obstaclesPresent = CheckPathForObstacles();        
                semaphore.Unlock();

        if( !obstaclesPresent )
        {
            path = m_Path;
            return true;
        }
        else
        {
            int i;
            int size = m_Path.Size() - 1;
            for( i = 1; i < size; ++i )
            {
                //Perturb( i );
            }
        }
        }
        return false;
}

//=============================================================================
// PL_Neural::Relax
//=============================================================================
void PL_Neural::Relax( const unsigned int pointNum )
{
    if( pointNum == 0 )
    {
        return;
    }
    unsigned int size = m_Path.Size();
    if( pointNum == size - 1 )
    {
        return;
    }

    double length = m_Path.PhysicalLength();
    length /= size;
    length *= 0.8;
    
    const Configuration& p0 = m_Path[ pointNum - 1 ];
    const Configuration& p1 = m_Path[ pointNum - 0 ];
    const Configuration& p2 = m_Path[ pointNum + 1 ];

    const Configuration v0 = p1 - p0;
    const Configuration v1 = p1 - p2;

    double d0 = ( v0 ).Magnitude();
    double d1 = ( v1 ).Magnitude();
    d0 -= length;
    d1 -= length;
    Configuration repulsiveVector = v0 * ( -d0 ) + v1 * ( -d1 );
    //const Configuration repulsiveForce = RepulsiveVector( p1 ) * 0.0;
    //Configuration attractionForce = 0.5 * p0 - p1 + 0.5 * p2;
    Configuration offset = 0.01 * repulsiveVector;
    m_Path[ pointNum ] += offset;
}

//=============================================================================
// PL_Neural::RelaxUntilSettled
//=============================================================================
void PL_Neural::RelaxUntilSettled()
{
    //
    // relax all the nodes until they settle down
    //
    int count;
    for( count = 0; count < 20; ++count )
    {
        ClampAllPointsToJointLimits();
        int size = m_Path.Size();
        int i;
        for( i = 1; i < size; ++i )
        {
            Relax( i );
        }

        size = m_Path.Size() - 1;
        for( i = 1; i < size; ++i )
        {
            Configuration repulsion = EdgeRepulsion( i ) * 0.01;
            double repusiveForce = repulsion.Magnitude();
            if( i != 0 )
            {
                m_Path[ i + 0 ] += repulsion;
            }
            if( i != size - 1 )
            {
                m_Path[ i + 1 ] += repulsion;
            }
        }
    }
}

//=============================================================================
// PL_Neural::RepulsiveVector
//=============================================================================
Configuration PL_Neural::RepulsiveVector( const Configuration& c ) const
{
    Configuration returnMe = c * 0.0;
    int size = m_ObstacleLocations.size();
    int i;
    for( i = 0; i < size; ++i )
    {
        Configuration ob = m_ObstacleLocations[ i ];
        ob -= c;
        ob *= -1;
        double mag = ob.MagSquared();
        if( mag != 0.0 )
        {
            ob /= mag;   
        }
        returnMe += ob;
    }
    return returnMe;
}

---