planners/closedchain/PL_RRT_ClosedChain.cpp

Go to the documentation of this file.

#include "math/vector4.h"
#include "math/matrixmxn.h"
#include "synchronization/semaphore.h"
#include "robots/R_OpenChain.h"
#include "CollisionDetectors/CD_Swiftpp.h"
#include <assert.h>
#include "planners/inversekin/Redundant.h"
#include "planners/ik_mpep/IK_Jacobian.h"
#include "PL_RRT_ClosedChain.h"
#include <gl/gl.h>
#include <opengl/gl_sphere.h>
#include <opengl/gl_group.h>

#define DEF_NEIGHBOR         (2)         //For CompareLocalPlanners();
#define DEF_RESOLUTION       (15)
#define MIN_DIST_CHANGE      (0.5)

#define LEN_STEP_SIZE   30
//static double LEN_STEP_SIZE=30;
//#define MAX_CONF_RETRY        10
#define _MULTIPLE_STEP

typedef struct
{
        Configuration conf;
        PA_Points edge;
} VertexInfoInTree;

#define ERR_FAIL            0xff
#define ERR_SUCCESS         0x00
#define ERR_TIMEOUT         0x10
#define ERR_ILLPARAMETER    0x20


PL_RRT_ClosedChain::PL_RRT_ClosedChain()
{
        m_rootVert = NULL;
        m_goalVert = NULL;
}

PL_RRT_ClosedChain::~PL_RRT_ClosedChain()
{
        if ( m_rootVert != NULL )
                ClearTree();
}

void PL_RRT_ClosedChain::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        PL_PRM_ClosedChain::SetCollisionDetector(collisionDetector);
/*
        collisionDetector->DeactivateFrames(1, collisionDetector->DOF());
        this->collisionDetector = collisionDetector;
        m_nDof = collisionDetector->DOF();
        m_pRobot = dynamic_cast<R_OpenChain*>(collisionDetector->GetRobot(0));
        m_nToolFrame = m_pRobot->GetToolFrame(0);
        m_frEndEffector = Matrix4x4::Identity();
        if (m_nToolFrame != -1)
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                m_frEndEffector = *(frameManager->GetFrameRef(m_nToolFrame));
        }
        if (jacobian != NULL)
        {
                delete jacobian;
                jacobian = new CJacobian(*m_pRobot);
        }
*/
}

bool PL_RRT_ClosedChain::DrawExplicit () const
{
        double Xcor, Ycor, Zcor;        // temp coordinate variables

        Semaphore semaphore( guid );
        semaphore.Lock();

        // Setup GL Environment
        glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        BOOL lightingState = glIsEnabled( GL_LIGHTING );
        glDisable( GL_LIGHTING );
        glShadeModel( GL_SMOOTH );
        glPointSize(5.0f);      // Set size of points associated with the nodes.

        if (m_vertices.size() )
        {
                int i;

                //const dynamic_trees *ptrTree = &tree;
                glBegin(GL_POINTS);
                for(i = 0; i<m_vertices.size(); i++)
                {
                        vertex vert;
                        VertexInfoInTree *info;
                        vert = m_vertices[i];
                        info = (VertexInfoInTree *)((dynamic_trees &)m_trajTree).vertex_inf(vert);

                        Configuration conf = info->conf;
                        Xcor = conf[0];
                        Ycor = conf[1];
                        Zcor = conf[2];

                        if ((i==0) || (i==(m_vertices.size()-1)))
                                glColor3f(0.0f,0.0f,1.0f);    //Set the start and end to be blue
                        else
                                glColor3f(1.0f,1.0f,0.0f);              // set colour to yellow
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
                }
                glEnd();

                glBegin(GL_LINES);
                for(i = 1; i<m_vertices.size(); i++)
                {
                        vertex vert, parentVert;
                        VertexInfoInTree *info, *parentInfo;

                        vert = m_vertices[i];
                        parentVert = ((dynamic_trees &)m_trajTree).parent(vert);
                        info = (VertexInfoInTree *)((dynamic_trees &)m_trajTree).vertex_inf(vert);
                        parentInfo = (VertexInfoInTree *)((dynamic_trees &)m_trajTree).vertex_inf(parentVert);

                        Configuration conf = info->conf;
                        Configuration parentConf = parentInfo->conf;

                        Xcor = conf[0];
                        Ycor = conf[1];
                        Zcor = conf[2];
                        glColor3f(1.0f,0.5f,0.0f);      // set colour to orange
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line

                        Xcor = parentConf[0];
                        Ycor = parentConf[1];
                        Zcor = parentConf[2];
                        glColor3f(1.0f,0.0f,0.0f);      // set colour to red
                        glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line
                }
                glEnd();
        }

        // Clean up Environment
        if( lightingState != FALSE )
        {
                glEnable( GL_LIGHTING );
        }

        semaphore.Unlock();

        Sleep( 50 );

        return true;
}

bool PL_RRT_ClosedChain::Plan ()
{
        srand( (unsigned)time( NULL ) );

        path.Clear();

        // Start the timer
        StartTimer();

        //============== Test for Local Planner ========================
        CompareLocalPlanners();
        return false;
        //==============================================================

        Configuration startConf = GetStartConfig();
        CreateTree(startConf);

        int result = ERR_FAIL;
        bool pathFound = false;
        m_goalVert = NULL;
        
        //if (m_nAlgorithm == ALG_CLOSEDCHAIN_RGD)
        //      LEN_STEP_SIZE = 10;
        //else if (m_nAlgorithm == ALG_CLOSEDCHAIN_APD)
        //      LEN_STEP_SIZE = 20;
        //else if (m_nAlgorithm == ALG_CLOSEDCHAIN_JACOBIAN)
        //      LEN_STEP_SIZE = 30;

        int repeatNum = 0;
        while ((!pathFound) && (result != ERR_TIMEOUT))
        {
                PA_Points localPath;
                Configuration randConf = PL_PRM::GenerateRandomConfig();
                //Configuration randConf = PL_PRM_ClosedChain::GenerateRandomConfig();
                vertex fromVert = FindClosestInTree(randConf);
                Configuration fromConf = GetConfigurationFromTree(fromVert);
                Configuration toConf;

                //int nCurrentAlgorithm = m_nAlgorithm;
                //m_nAlgorithm = ALG_CLOSEDCHAIN_APD;
                if (m_nAlgorithm == ALG_CLOSEDCHAIN_JACOBIAN)
                        result = ExtendJacobian(fromConf, toConf, randConf, localPath);
                else
                        result = Extend(fromConf, toConf, randConf, localPath);
                //m_nAlgorithm = (ClosedChainAlgorithm) nCurrentAlgorithm;

                if (result == ERR_SUCCESS)
                {
                        Log("ExtendDistance.log", "%f", Distance(fromConf, toConf));
                        fromVert = AddNodeInTree(fromVert, toConf, localPath);
                        //if (ConnectToGoal() == ERR_SUCCESS)
                        //int nCurrentAlgorithm = m_nAlgorithm;
                        //m_nAlgorithm = ALG_CLOSEDCHAIN_APD;
                        if (ConnectToGoal(fromVert) == ERR_SUCCESS)
                        {
                                pathFound = true;
                        }
                        //m_nAlgorithm = (ClosedChainAlgorithm) nCurrentAlgorithm;
                }

                //semaphore.Unlock();
                //semaphore.Lock();

                if ( HasTimeLimitExpired() )
                {
                        break;
                }

        } //End of While(...)

        //semaphore.Unlock();
        if (pathFound)
        {
                assert(m_goalVert != NULL);
                RetrievePath(m_goalVert);
        }
        
        OutputStatistics();
        return pathFound;
}

int PL_RRT_ClosedChain::Extend(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &localPath)
{
        std::vector<Frame> edgeTraj;
        localPath.Clear();

        double pathLen = 0;
        bool trajExtracted = true;

        //Extracted a end-effector path as an edge in physical space.
        double dist = Distance(fromConf, dirConf);
        if (dist > LEN_STEP_SIZE)
                toConf = fromConf * (1-LEN_STEP_SIZE/dist) + dirConf * (LEN_STEP_SIZE/dist);
        else
                toConf = dirConf;

        //bool result = false;
        //int retry = 0;
        //while ( (!result) && (retry < MAX_CONF_RETRY) )
        //{
        //      result = MakeItClosed( toConf );
        //      retry ++;
        //}     
        bool result = MakeItClosed( toConf );

        if (!result)
        {
                return ERR_FAIL;
        }
        
        if ( IsInterfering(fromConf, toConf) == false)
        {
                for (int i=0; i<edgeFrag->size(); i++)
                {
                        list_item itt = edgeFrag->get_item(i);
                        Configuration conf = edgeFrag->contents(itt);
                        localPath.AppendPoint(conf);
                }
                edgeFrag->clear();
                //localPath = *edgeFrag;
                return ERR_SUCCESS;
        }

        return ERR_FAIL;
}

int PL_RRT_ClosedChain::ExtendJacobian(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &localPath)
{
        std::vector<Frame> edgeTraj;
        localPath.Clear();

        double pathLen = 0;
        bool trajExtracted = true;

        Configuration dirVel = dirConf;
        dirVel -= fromConf;
        toConf = fromConf;

        //compute self-motion
        jacobian->SetConfiguration(fromConf);
        
        Matrixmxn mJEnd, mJEndInv;
        Matrixmxn mTemp(m_nDof, m_nDof); 
        Matrixmxn matrixIdentity(m_nDof, m_nDof);
        double distTravel=0;
        localPath.AppendPoint(fromConf);
        bool bCollisionFree = true;
        Configuration lastConf = fromConf;
#ifdef _MULTIPLE_STEP
        while(distTravel<LEN_STEP_SIZE)
        {
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                mTemp = matrixIdentity;
                mTemp -= mJEndInv * mJEnd;
                dirVel = mTemp * dirVel;
                dirVel *= (DEF_RESOLUTION / dirVel.Magnitude());
                toConf = toConf + dirVel;
                if (MakeItClosed(toConf))
                {
                        if (IsInterfering(toConf, fromConf))
                        {
                                bCollisionFree = false;
                                localPath.Clear();
                                break;
                        }
                        else
                        {
                                //localPath.AppendPoint(toConf);
                                for (int i=0; i<edgeFrag->size(); i++)
                                {
                                        list_item itt = edgeFrag->get_item(i);
                                        Configuration conf = edgeFrag->contents(itt);
                                        localPath.AppendPoint(conf);
                                }
                                edgeFrag->clear();
                                jacobian->SetConfiguration(toConf);
                                distTravel += dirVel.Magnitude();
                                dirVel = dirConf - toConf;
                                lastConf = toConf;
                                if (distTravel < MIN_DIST_CHANGE)
                                        break;
                        }
                }
                else
                {
                        bCollisionFree = false;
                        localPath.Clear();
                        break;
                }
        }
#else
        jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
        mJEnd = *(jacobian->GetMatrix());
        mJEnd.Inverse(mJEndInv);
        mTemp = matrixIdentity;
        mTemp -= mJEndInv * mJEnd;
        dirVel = mTemp * dirVel;
        dirVel *= (DEF_RESOLUTION / dirVel.Magnitude());
        toConf = toConf + dirVel;
        MakeItClosed(toConf);
        
        if (IsInterfering(toConf, fromConf))
        {
                bCollisionFree = false;
                localPath.Clear();
        }
        else
        {
                localPath.AppendPoint(toConf);
        }
#endif

        if (bCollisionFree)
        {
                //Log("closedchain.log", "The number of intermediate node in an extension: %d", localPath.Size());
                return ERR_SUCCESS;
        }
        //double timeInterval = (LEN_STEP_SIZE)/dirVel.Magnitude();
        //toConf = fromConf + dirVel * timeInterval;

        //if (AdjustConfiguration(toConf))
        //{
        //      if ( collisionDetector->IsInterferingLinear(toConf, fromConf) == false)
        //      {
        //              return ERR_SUCCESS;
        //      }
        //}

        return ERR_FAIL;
}

//int PL_RRT_ClosedChain::ConnectToGoal(vertex fromVert)
int PL_RRT_ClosedChain::ConnectToGoal()
{
        Configuration goalConf = GetGoalConfig();
        Frame goalPose = GetToolFrame(goalConf);

        vertex fromVert = fromVert = FindClosestInTree(goalConf);
        return ConnectToGoal(fromVert);
}

//int PL_RRT_ClosedChain::ConnectToGoal(vertex fromVert)
int PL_RRT_ClosedChain::ConnectToGoal(vertex fromVert)
{
        Configuration goalConf = GetGoalConfig();
        if (fromVert==NULL)
        {
                return ERR_FAIL;
        }

        Configuration fromConf = GetConfigurationFromTree(fromVert);
        if (!IsInterfering(fromConf, goalConf))
        {
                PA_Points localPath;
                for (int i=0; i<edgeFrag->size(); i++)
                {
                        list_item itt = edgeFrag->get_item(i);
                        Configuration conf = edgeFrag->contents(itt);
                        localPath.AppendPoint( conf );
                }
                edgeFrag->clear();
                m_goalVert = AddNodeInTree(fromVert, goalConf, localPath);
                localPath.Clear();
                return ERR_SUCCESS;
        }
        return ERR_FAIL;
}

bool PL_RRT_ClosedChain::CreateTree(Configuration &conf)
{
        if (m_rootVert != NULL)
        {
                ClearTree();
        }

        VertexInfoInTree *newNode = new VertexInfoInTree;
        newNode->conf = conf;

        Semaphore semaphore( guid );
        semaphore.Lock();
        m_rootVert = m_trajTree.make(newNode);
        m_vertices.push_back(m_rootVert);
        semaphore.Unlock();

        return true;
}

void PL_RRT_ClosedChain::ClearTree()
{
        Semaphore semaphore( guid );
        semaphore.Lock();
        for (int i=0; i<m_vertices.size(); i++)
        {
                VertexInfoInTree *info;
                info = (VertexInfoInTree *)m_trajTree.vertex_inf(m_vertices[i]);
                delete info;
        }

        m_trajTree.clear();
        m_vertices.clear();
        m_rootVert = NULL;
        semaphore.Unlock();
}

vertex PL_RRT_ClosedChain::AddNodeInTree(vertex parentVertex, Configuration &childConf, PA_Points &localPath)
{
        VertexInfoInTree *newNode = new VertexInfoInTree;
        newNode->conf = childConf;
        CopyPath(newNode->edge, localPath);

        Semaphore semaphore( guid );
        semaphore.Lock();
        vertex newVert = m_trajTree.make(newNode);
        m_vertices.push_back(newVert);
        m_trajTree.link(newVert, parentVertex, 1);
        semaphore.Unlock();

        return newVert;
}

vertex PL_RRT_ClosedChain::FindClosestInTree(Configuration &conf)
{
        if (m_vertices.size() == 0)
                return NULL;

        VertexInfoInTree *info;
        double minDist, currDist;
        int minIndex=0;

        info = (VertexInfoInTree *)m_trajTree.vertex_inf(m_vertices[0]);
        minDist = Distance(info->conf, conf);
        minIndex = 0;

        for (int i=1; i<m_vertices.size(); i++)
        {
                info = (VertexInfoInTree *)m_trajTree.vertex_inf(m_vertices[i]);
                currDist = Distance(info->conf, conf);
                if (currDist < minDist)
                {
                        minDist = currDist;
                        minIndex = i;
                }
        }

        return m_vertices[minIndex];
}

Configuration& PL_RRT_ClosedChain::GetConfigurationFromTree(vertex vert)
{
        VertexInfoInTree *info;
        info = (VertexInfoInTree *)m_trajTree.vertex_inf(vert);
        return info->conf;
}

double PL_RRT_ClosedChain::Distance(const Configuration &conf1, const Configuration &conf2)
{
        double dist = 0;
        VectorN jointDiff = collisionDetector->JointDisplacement(conf1, conf2);
        for(int i = 0; i < jointDiff.Length(); i++)
        {
                dist += (jointDiff[i])*(jointDiff[i]);
        }
        return sqrt(dist);
}

PA_Points& PL_RRT_ClosedChain::GetPathFromTree(vertex vert)
{
        VertexInfoInTree *info;
        info = (VertexInfoInTree *)m_trajTree.vertex_inf(vert);
        return info->edge;
}

void PL_RRT_ClosedChain::RetrievePath(vertex &goalVert)
{
        vertex currVert = goalVert;
        while (currVert != NULL)
        {
                InsertPath(path, GetPathFromTree(currVert));
                //LogPath("APDErrorPath.log", path);
                currVert = m_trajTree.parent(currVert);
        }//end of while
}

void PL_RRT_ClosedChain::AppendPath(PA_Points &collect, PA_Points &local)
{
        for (int i=0; i<local.Size(); i++)
        {
                collect.AppendPoint(local[i]);
        }
}

void PL_RRT_ClosedChain::AppendPath(PA_Points &collect, PA_Points *local)
{
        for (int i=0; i<local->Size(); i++)
        {
                collect.AppendPoint(local->GetPoint(i));
        }
}

void PL_RRT_ClosedChain::InsertPath(PA_Points &target, PA_Points &source)
{
        int nLen = source.Size()-1;
        if ( target.Size() != 0 )
        {
                nLen = nLen - 1;
        }
        for( int i = nLen; i >= 0; i-- )
        {
                target.AppendPointToBeginning( source[ i ] ) ;
        }
}

void PL_RRT_ClosedChain::OutputStatistics()
{
        double dTimeElapsed = GetTimeElapsedInSeconds();
        int nLinearCD = collisionDetector->GetNumberOfTimesCalledLinear();
        int nBoolCD = collisionDetector->GetNumberOfTimesCalledPoint();
        int nNodeNum = m_vertices.size();
        int nPathLen = path.Size();
        bool bSuccess = (nPathLen != 0);

        Log("ClosedChainRRT.log", "%d, %f, %d, %d, %d, %d", 
                        bSuccess, dTimeElapsed, nLinearCD, nBoolCD, nNodeNum, nPathLen);
}

//==============================================================================
//== The following functions are used to compare Jacobian local planner 
//==      and the Randomized Gradient Descent methos by Lavalle and Kavaraki
//==============================================================================
bool PL_RRT_ClosedChain::TestJacobianConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath)
{
        ClosedChainAlgorithm currentAlg = m_nAlgorithm;
        m_nAlgorithm = ALG_CLOSEDCHAIN_JACOBIAN;
        localPath.Clear();
        bool bValid = !(PL_PRM_ClosedJacobian::IsInterfering(testStartConf, testGoalConf));
        if (bValid)
        {
                for (int i=0; i<edgeFrag->size(); i++)
                {
                        list_item itt = edgeFrag->get_item(i);
                        Configuration conf = edgeFrag->contents(itt);
                        localPath.AppendPoint(conf);
                }
                edgeFrag->clear();
        }
  
        m_nAlgorithm = currentAlg;
        return bValid;
}

bool PL_RRT_ClosedChain::TestRGDConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath)
{
        ClosedChainAlgorithm currentAlg = m_nAlgorithm;
        m_nAlgorithm = ALG_CLOSEDCHAIN_RGD;
        localPath.Clear();
        bool bValid = !(PL_PRM_ClosedRGD::IsInterfering(testStartConf, testGoalConf));
        if (bValid)
        {
                for (int i=0; i<edgeFrag->size(); i++)
                {
                        list_item itt = edgeFrag->get_item(i);
                        Configuration conf = edgeFrag->contents(itt);
                        localPath.AppendPoint(conf);
                }
                edgeFrag->clear();
        }
  
        m_nAlgorithm = currentAlg;
        return bValid;
}

bool PL_RRT_ClosedChain::TestAPDecompConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath)
{
        ClosedChainAlgorithm currentAlg = m_nAlgorithm;
        m_nAlgorithm = ALG_CLOSEDCHAIN_APD;
        localPath.Clear();
        bool bValid = !(PL_PRM_ClosedAPDecomp::IsInterfering(testStartConf, testGoalConf));
        if (bValid)
        {
                for (int i=0; i<edgeFrag->size(); i++)
                {
                        list_item itt = edgeFrag->get_item(i);
                        Configuration conf = edgeFrag->contents(itt);
                        localPath.AppendPoint(conf);
                }
                edgeFrag->clear();
        }
  
        m_nAlgorithm = currentAlg;
        return bValid;
}

void PL_RRT_ClosedChain::CompareLocalPlanners()
{
        int numTest = 100;
        Configuration testStart, testGoal;
        if (jacobian==NULL)
                jacobian = new CJacobian(*m_pRobot);

        testStart = GetStartConfig();
        testGoal = GetGoalConfig();

        Log("CompareLocalPlanners.log", "===============Begin===============");
        Log("CompareLocalPlanners.log", "Planner: S/F, Time, Len, Max, Min");
        for (int i=0; i<numTest; i++)
        {
#if 1
                do{
                        testStart = GenerateRandomConfig(testStart, DEF_NEIGHBOR*0.5);
                } while(!IsClosed(testStart));
                do
                {
                        testGoal = GenerateRandomConfig(testGoal, DEF_NEIGHBOR*0.5);
                } while(!IsClosed(testGoal));
#endif
                Log("PathByJacobian.log", "Start Configuration");
                LogConfiguration("PathByJacobian.log", testStart);
                Log("PathByJacobian.log", "Goal Configuration");
                LogConfiguration("PathByJacobian.log", testGoal);

                //Log("PathByRGD.log", "Start Configuration");
                //LogConfiguration("PathByRGD.log", testStart);
                //Log("PathByRGD.log", "Goal Configuration");
                //LogConfiguration("PathByRGD.log", testGoal);

                int nDof = testStart.DOF();
                PA_Points foundPath;

                //===========================================================================
                //For Jacobian-based 
                bool bJacobian = false;
                double timeForJacobian = 0;
                int nPathLenForJacobian = 0;
                int nCDBoolForJacobian, nCDLinearForJacobian;
                nCDBoolForJacobian = nCDLinearForJacobian = 0;
                double dMaxForJacobian, dMinForJacobian;
                dMaxForJacobian = dMinForJacobian = 0;
#if 1
                collisionDetector->ResetStats();
                timeForJacobian = GetTimeElapsedInSeconds();
                bJacobian = TestJacobianConnection(testStart, testGoal, foundPath);
                timeForJacobian =  GetTimeElapsedInSeconds() - timeForJacobian;
                nCDLinearForJacobian = collisionDetector->GetNumberOfTimesCalledLinear();
                nCDBoolForJacobian = collisionDetector->GetNumberOfTimesCalledPoint();
                nPathLenForJacobian = foundPath.Size();

                if ((bJacobian) && (nPathLenForJacobian>1))
                {       
                        Configuration diff=foundPath[1]-foundPath[0];
                        double distance = diff.Magnitude();
                        dMaxForJacobian = dMinForJacobian = distance;
                        for(int k=2; k<nPathLenForJacobian; k++)
                        {
                                diff=foundPath[k]-foundPath[k-1];
                                distance = diff.Magnitude();
                                if (distance > dMaxForJacobian)
                                        dMaxForJacobian = distance;
                                if (distance < dMinForJacobian)
                                        dMinForJacobian = distance;
                        }

                        Log("PathByJacobian.log", "Path: %d [out of %d]", i, numTest);
                        LogPath("PathByJacobian.log", foundPath);
                }

                foundPath.Clear();
#endif

                //===========================================================================
                //For Randomized Gradient Descent
                bool bRGD=false;
                double timeForRGD = 0;
                int nPathLenForRGD = 0;
                int nCDBoolForRGD, nCDLinearForRGD;
                nCDBoolForRGD = nCDLinearForRGD = 0;
                double dMaxForRGD, dMinForRGD;
                dMaxForRGD = dMinForRGD = 0;
#if 1
                collisionDetector->ResetStats();
                timeForRGD = GetTimeElapsedInSeconds();
                bRGD = TestRGDConnection(testStart, testGoal, foundPath);
                timeForRGD =  GetTimeElapsedInSeconds() - timeForRGD;
                nCDLinearForRGD = collisionDetector->GetNumberOfTimesCalledLinear();
                nCDBoolForRGD = collisionDetector->GetNumberOfTimesCalledPoint();
                nPathLenForRGD = foundPath.Size();

                if ((bRGD) && (nPathLenForRGD>1))
                {
                        Configuration diff=foundPath[1]-foundPath[0];
                        double distance = diff.Magnitude();
                        dMaxForRGD = dMinForRGD = distance;
                        for(int k=2; k<nPathLenForRGD; k++)
                        {
                                diff=foundPath[k]-foundPath[k-1];
                                distance = diff.Magnitude();
                                if (distance > dMaxForRGD)
                                        dMaxForRGD = distance;
                                if (distance < dMinForRGD)
                                        dMinForRGD = distance;
                        }

                        Log("PathByRGD.log", "Path: %d [out of %d]", i, numTest);
                        LogPath("PathByRGD.log", foundPath);
                }

                foundPath.Clear();
#endif

                //===========================================================================
                //For Active-Passive link-decomposition
                bool bAPDecomp = false;
                double timeForAPDecomp = 0;
                int nPathLenForAPDecomp = 0;
                int nCDBoolForAPDecomp, nCDLinearForAPDecomp;
                nCDBoolForAPDecomp = nCDLinearForAPDecomp = 0;
                double dMaxForAPDecomp, dMinForAPDecomp;
                dMaxForAPDecomp = dMinForAPDecomp = 0;
#if 1
                collisionDetector->ResetStats();
                timeForAPDecomp = GetTimeElapsedInSeconds();
                bAPDecomp = TestAPDecompConnection(testStart, testGoal, foundPath);
                timeForAPDecomp =  GetTimeElapsedInSeconds() - timeForAPDecomp;
                nCDLinearForAPDecomp = collisionDetector->GetNumberOfTimesCalledLinear();
                nCDBoolForAPDecomp = collisionDetector->GetNumberOfTimesCalledPoint();
                nPathLenForAPDecomp = foundPath.Size();

                if ((bAPDecomp) && (nPathLenForAPDecomp>1))
                {
                        Configuration diff=foundPath[1]-foundPath[0];
                        double distance = diff.Magnitude();
                        dMaxForAPDecomp = dMinForAPDecomp = distance;
                        for(int k=2; k<nPathLenForAPDecomp; k++)
                        {
                                diff=foundPath[k]-foundPath[k-1];
                                distance = diff.Magnitude();
                                if (distance > dMaxForAPDecomp)
                                        dMaxForAPDecomp = distance;
                                if (distance < dMinForAPDecomp)
                                        dMinForAPDecomp = distance;
                        }

                        Log("PathByAPDecomp.log", "Path: %d [out of %d]", i, numTest);
                        LogPath("PathByAPDecomp.log", foundPath);
                }

                foundPath.Clear();
#endif

                Log("CompareLocalPlanners.log", "Jacobian: %d, %.3f, %d, %.3f, %.3f, %d, %d;\nRGD: %d, %.3f, %d, %.3f, %.3f, %d, %d;\nAPDecomp: %d, %.3f, %d, %.3f, %.3f, %d, %d;  ", 
                        bJacobian, timeForJacobian, nPathLenForJacobian, dMaxForJacobian, dMinForJacobian, nCDLinearForJacobian, nCDBoolForJacobian,
                        bRGD, timeForRGD, nPathLenForRGD, dMaxForRGD, dMinForRGD, nCDLinearForRGD, nCDBoolForRGD,
                        bAPDecomp, timeForAPDecomp, nPathLenForAPDecomp, dMaxForAPDecomp, dMinForAPDecomp, nCDLinearForAPDecomp, nCDBoolForAPDecomp);
        }

}

---