planners/atace/PL_RRT_Constrained.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/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 MAX_ITERATION        30
#define MAX_RETRY            30
#define MAX_RETRY2           30
#define DEF_NEIGHBOR         (2)
#define DEF_DIST_TOLERANCE   (3)
#define DEF_ERR_TOLERANCE    (1e-3)
#define DEF_TIME_INTERVAL    (1e-1)
#define DEF_INCREMENT        (15)
#define MIN_DIST_CHANGE      (0.5)

#define LEN_STEP_SIZE   20
#define MAX_CONF_RETRY  10

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;
        jacobian = NULL;
        m_bUseJacobian = true;

}

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

void PL_RRT_ClosedChain::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        PL_PRM::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 ========================
        //CompareJacobianAndRGD();
        //return false;
        //==============================================================

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

        int result = ERR_FAIL;
        bool pathFound = false;
        m_goalVert = NULL;
        
        if (m_bUseJacobian)
        {
                if (jacobian == NULL)
                        jacobian = new CJacobian(*m_pRobot);
        }

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

                if (m_bUseJacobian)
                        result = ExtendJacobian(fromConf, toConf, randConf, localPath);
                else
                        result = Extend(fromConf, toConf, randConf, localPath);

                if (result == ERR_SUCCESS)
                {
                        fromVert = AddNodeInTree(fromVert, toConf, localPath);
                        if (m_bUseJacobian)
                        {
                                if (ConnectToGoalJacobian(fromVert) == ERR_SUCCESS)
                                //if (ConnectToGoal() == ERR_SUCCESS)
                                {
                                        pathFound = true;
                                }
                        }
                        else
                        {
                                if (ConnectToGoal() == ERR_SUCCESS)
                                {
                                        pathFound = true;
                                }
                        }
                }

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

                if ( HasTimeLimitExpired() )
                {
                        break;
                }

        } //End of While(...)

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

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 = GetClosedConfiguration( toConf );
                retry ++;
        }       

        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;
#ifdef _MULTIPLE_STEP
        while(distTravel<LEN_STEP_SIZE)
        {
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, useOrientConstraint);
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                mTemp = matrixIdentity;
                mTemp -= mJEndInv * mJEnd;
                dirVel = mTemp * dirVel;
                dirVel *= (DEF_INCREMENT / dirVel.Magnitude());
                toConf = toConf + dirVel;
                AdjustConfiguration(toConf);
                if (collisionDetector->IsInterfering(toConf))
                {
                        bCollisionFree = false;
                        localPath.Clear();
                        break;
                }
                else
                {
                        localPath.AppendPoint(toConf);
                        jacobian->SetConfiguration(toConf);
                        distTravel += dirVel.Magnitude();
                        dirVel = dirConf - toConf;
                }
        }
#else
        mJEnd = *(jacobian->GetMatrix());
        mJEnd.Inverse(mJEndInv);
        mTemp = matrixIdentity;
        mTemp -= mJEndInv * mJEnd;
        dirVel = mTemp * dirVel;
        dirVel *= (DEF_INCREMENT / dirVel.Magnitude());
        toConf = toConf + dirVel;
        AdjustConfiguration(toConf);
        
        if (collisionDetector->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;
}

bool PL_RRT_ClosedChain::AdjustConfiguration(Configuration &config)
{
        if (IsClosed(config))
                return true;
        else
        {
                Frame toolFrame = GetToolFrame( config );
                Vector4 offset = toolFrame.GetTranslationVector();
                offset[2] = 0;
                VectorN endEffOffset(-offset[0], -offset[1], -offset[2]); 
                //Assuming closed configuration has end-effector pose at (0, 0, 0);

                //compute self-motion
                jacobian->SetConfiguration(config);
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, useOrientConstraint);

                Matrixmxn mJEnd, mJEndInv;
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);

                Configuration jointOffset;
                jointOffset = mJEndInv * endEffOffset * Rad2Deg(1);
                config += jointOffset;

        }
        return true;
}

int PL_RRT_ClosedChain::ConnectToGoal2()
{
        Configuration goalConf = GetGoalConfig();
        Frame goalPose = GetToolFrame(goalConf);
        bool result = true;
        if (useGoalPose)
        {
                result = false;
                int retry = 0;
                while ( (!result) && (retry++<MAX_CONF_RETRY))
                {
                        result = GenerateRandomConfigForPose(goalConf, goalPose);
                }
        }

        vertex fromVert = NULL;
        if (result)
        {
                fromVert = FindClosestInTree(goalConf);
        }
        if (fromVert == NULL)
        {
                result = false;
        }

        if (result) 
        {
                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;
}

//int PL_RRT_ClosedChain::ConnectToGoal(vertex fromVert)
int PL_RRT_ClosedChain::ConnectToGoal()
{
        Configuration goalConf = GetGoalConfig();
        Frame goalPose = GetToolFrame(goalConf);
        bool result = true;
        if (useGoalPose)
        {
                result = false;
                int retry = 0;
                while ( (!result) && (retry++<MAX_CONF_RETRY))
                {
                        result = GenerateRandomConfigForPose(goalConf, goalPose);
                }
        }

        vertex fromVert = NULL;
        if (result)
        {
                fromVert = FindClosestInTree(goalConf);
        }
        if (fromVert == NULL)
        {
                result = false;
        }

        if (result) 
        {
                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;
}

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

        bool result = true;

        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;
}

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

        Configuration fromConf = GetConfigurationFromTree(fromVert);
        double distChanged1 = 360*goalConf.DOF(); //distance to goal;
        //double distChanged2 = 360*goalConf.DOF(); //distance from last configuration;
        Configuration dirVel = goalConf - fromConf;
        Configuration nextConf = fromConf;
        double newDistance = 0;
        double oldDistance = dirVel.Magnitude();
        PA_Points localPath;
        localPath.AppendPoint(fromConf);

        jacobian->SetConfiguration(fromConf);

        Matrixmxn mJEnd, mJEndInv;
        Matrixmxn mTemp(m_nDof, m_nDof); 
        Matrixmxn matrixIdentity(m_nDof, m_nDof);

        bool bValid=true;
        while((bValid == true) && (oldDistance > DEF_DIST_TOLERANCE))
        {
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, useOrientConstraint);
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                mTemp = matrixIdentity;
                mTemp -= mJEndInv * mJEnd;
                dirVel = mTemp * dirVel;
                if (oldDistance < DEF_INCREMENT)
                        dirVel *= (oldDistance / dirVel.Magnitude());
                else
                        dirVel *= (DEF_INCREMENT / dirVel.Magnitude());
                nextConf = nextConf + dirVel;
                AdjustConfiguration(nextConf);
                if (collisionDetector->IsInterfering(nextConf))
                {
                        bValid = false;
                }
                else
                {
                        //distChanged2 = dirVel.Magnitude();
                        dirVel = goalConf - nextConf;
                        newDistance = dirVel.Magnitude();
                        distChanged1 = oldDistance - newDistance;
                        oldDistance = newDistance;
                        if ((newDistance > DEF_DIST_TOLERANCE) && (distChanged1<MIN_DIST_CHANGE))
                        {
                                bValid = false;
                                Log("closedchain.log", "Jacobian local failed with newDistance=%f, and distChanged1=%d", newDistance, distChanged1);
                        }
                }

                if (bValid==false)
                {
                        localPath.Clear();
                        break;
                }
                else
                {
                        localPath.AppendPoint(nextConf);
                        jacobian->SetConfiguration(nextConf);
                }
        }

        if (bValid)
        {
                m_goalVert = AddNodeInTree(fromVert, goalConf, localPath);
                Log("closedchain.log", "The number of intermediate node to goal: %d", localPath.Size());
                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));
                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 ] ) ;
        }
}

//==============================================================================
//== The following functions are used to compare Jacobian local planner 
//==      and the Randomized Gradient Descent methos by Lavalle and Kavaraki
//==============================================================================
//This function is copied from ConnectToGoalJacobian()
bool PL_RRT_ClosedChain::TestJacobianConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath)
{
        double distChanged1 = 360*testGoalConf.DOF(); //distance to goal;
        //double distChanged2 = 360*testGoalConf.DOF(); //distance from last configuration;
        Configuration dirVel = testGoalConf - testStartConf;
        Configuration nextConf = testStartConf;
        double newDistance = 0;
        double oldDistance = dirVel.Magnitude();
        localPath.AppendPoint(testStartConf);

        jacobian->SetConfiguration(testStartConf);

        Matrixmxn mJEnd, mJEndInv;
        Matrixmxn mTemp(m_nDof, m_nDof); 
        Matrixmxn matrixIdentity(m_nDof, m_nDof);

        bool bValid=true;
        while((bValid == true) && (oldDistance > DEF_DIST_TOLERANCE))
        {
                jacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, useOrientConstraint);
                mJEnd = *(jacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                mTemp = matrixIdentity;
                mTemp -= mJEndInv * mJEnd;
                dirVel = mTemp * dirVel;
                if (oldDistance < DEF_INCREMENT)
                        dirVel *= (oldDistance / dirVel.Magnitude());
                else
                        dirVel *= (DEF_INCREMENT / dirVel.Magnitude());
                nextConf = nextConf + dirVel;
                AdjustConfiguration(nextConf);
                if (collisionDetector->IsInterfering(nextConf))
                {
                        bValid = false;
                }
                else
                {
                        //distChanged2 = dirVel.Magnitude();
                        dirVel = testGoalConf - nextConf;
                        newDistance = dirVel.Magnitude();
                        distChanged1 = oldDistance - newDistance;
                        oldDistance = newDistance;
                        //if ((newDistance > DEF_DIST_TOLERANCE) && (distChanged1<MIN_DIST_CHANGE) && (distChanged2<MIN_DIST_CHANGE))
                        //{
                        //      bValid = false;
                        //}
                }

                if (bValid==false)
                {
                        localPath.Clear();
                        break;
                }
                else
                {
                        localPath.AppendPoint(nextConf);
                        jacobian->SetConfiguration(nextConf);
                }
        }

        return (bValid);
}

//This function is copied from PL_PRM_ClosedChain::IsInterfering(Configuration &p1, Configuration &p2)
bool PL_RRT_ClosedChain::TestRGDConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath)
{
        int i, j, k;
        i = 0;  j = 0;  k = 0;
        Configuration q_last = testStartConf;
        //Configuration q_end = testGoalConf;
        localPath.AppendPoint(testStartConf);
        int MAX_I = MAX_ITERATION * MAX_RETRY * MAX_RETRY2;
        int MAX_J = MAX_RETRY;
        int MAX_K = MAX_RETRY2;
        double distToLast = sqrt(Distance(q_last, testGoalConf));
        bool bValid = true;
        while ((i<MAX_I) && (j<MAX_J) && (k<MAX_K) && (distToLast>DEF_DIST_TOLERANCE))
        {
                i++;  j++;
                //Configuration q_mid = PL_PRM::GenerateRandomConfig(q_last, DEF_NEIGHBOR*0.2);
                Configuration q_mid = q_last*(1-(DEF_DIST_TOLERANCE/distToLast)) + testGoalConf*(DEF_DIST_TOLERANCE/distToLast);
                if (MakeItClosed(q_mid))  //if (Error(q_mid)<DEF_ERR_TOLERANCE)
                {
                        j = 0; k++;
                        if (PL_PRM::IsInterfering(q_mid))
                        {
                                bValid = false;
                                break;
                        }
                        if (Distance(q_mid, testGoalConf) < distToLast*distToLast)
                        {
                                k = 0;
                                localPath.AppendPoint(q_mid);
                                q_last = q_mid;
                                distToLast = sqrt(Distance(q_last, testGoalConf));
                        }
                }
        }

        if ((bValid) && (distToLast<=DEF_DIST_TOLERANCE))
        {
                localPath.AppendPoint(testGoalConf);
        }
        else
        {
                localPath.Clear();
        }
        
        return bValid;
}

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

        Log("CompareJacobianAndRGD.log", "===============Begin===============");
        Log("CompareJacobianAndRGD.log", "Jacobian: S/F, Time, Len, Max, Min;  RGD: S/F, Time, Len, Max, Min");
        for (int i=0; i<numTest; i++)
        {
                do{
                        testStart = GenerateRandomConfig(GetStartConfig(), DEF_NEIGHBOR*0.5);
                } while(!IsClosed(testStart));
                do
                {
                        testGoal = GenerateRandomConfig(GetGoalConfig(), DEF_NEIGHBOR*0.5);
                } while(!IsClosed(testGoal));
                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;

                bool bJacobian;
                double timeForJacobian;
                int nPathLenForJacobian;
                double dMaxForJacobian, dMinForJacobian;
                dMaxForJacobian = dMinForJacobian = 0;

                timeForJacobian = GetTimeElapsedInSeconds();
                bJacobian = TestJacobianConnection(testStart, testGoal, foundPath);
                timeForJacobian =  GetTimeElapsedInSeconds() - timeForJacobian;
                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();
                
                bool bRGD;
                double timeForRGD;
                int nPathLenForRGD;
                double dMaxForRGD, dMinForRGD;
                dMaxForRGD = dMinForRGD = 0;
                timeForRGD = GetTimeElapsedInSeconds();
                bRGD = TestRGDConnection(testStart, testGoal, foundPath);
                timeForRGD =  GetTimeElapsedInSeconds() - timeForRGD;
                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);
                }

                Log("CompareJacobianAndRGD.log", "Jacobian: %d, %.3f, %d, %.3f, %.3f;  RGD: %d, %.3f, %d, %.3f, %.3f", 
                        bJacobian, timeForJacobian, nPathLenForJacobian, dMaxForJacobian, dMinForJacobian,
                        bRGD, timeForRGD, nPathLenForRGD, dMaxForRGD, dMinForRGD);
        }

}

---