planners/atace/PL_RGD_Constraint.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 "planners/closedchain/PL_PRM_Closedchain.h"
#include "PL_RGD_Constraint.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-1)

//For sphere constraint
#define RGD_SPHERE_CX           15
#define RGD_SPHERE_CY           0  
#define RGD_SPHERE_CZ           5
#define RGD_SPHERE_RADIUS       12
#define SHPERE_ORIENTATION

double (PL_RGD_PRM::*costFunc)(const Configuration &p1, const Frame &pose) const = &PL_RGD_PRM::Error1;

PL_RGD_PRM::PL_RGD_PRM()
{
        edgePath.init(G);
        edgeFrag = new Fragment;
        //int dof = collisionDetector->DOF();
        //distTolerance = DEF_NEIGHBOR * dof;
        planner = new LocalPlannerClosed;
        pCollisionDetector = NULL;
        useJacobian = false;
        useGoalPose = false;
        typeOfConstraint = CONSTRAINT_COLSURE;
}       

PL_RGD_PRM::~PL_RGD_PRM()
{
        if(pCollisionDetector)
        {
                delete pCollisionDetector;
        }
        delete planner;

        edgeFrag->clear();
        delete edgeFrag;
}

void PL_RGD_PRM::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        // Call Parent's Collision Detector and assign the collision detector
        PL_PRM::SetCollisionDetector( collisionDetector );

        if (pCollisionDetector != NULL)
                delete pCollisionDetector;

        pCollisionDetector = new CD_Swiftpp( *(collisionDetector->GetUniverse()) );
        planner->SetCollisionDetector(pCollisionDetector);
        pCollisionDetector->DeactivateFrames(1, pCollisionDetector->DOF());
}

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

        StartTimer();
        if (typeOfConstraint == CONSTRAINT_PLANAR)
        {
                UpdatePlanarConstraint();
        }
        else if (typeOfConstraint == CONSTRAINT_ORIENTATION)
        {
                UpdateOrientConstraint();
        }
        else if (typeOfConstraint == CONSTRAINT_SPHERE)
        {
                UpdateShpereConstraint();
        }

        if (useJacobian)
        {
                planner->SetTimeLimitInSeconds(100);
                Configuration q1 = GetStartConfig();
                Configuration q2 = GetGoalConfig();
                planner->SetStartConfig(q1);
                planner->SetGoalConfig(q2);

                double dObstacleTol = 0.01;
                double dHomogain = 2.0;
                double dPathTol = 0.5;
                int nObstacleNum = 2;

                planner->SetObstacleTolerance(dObstacleTol);
                planner->SetNumObstaclePt(nObstacleNum);
                planner->SetHomogeneousGain(dHomogain);
                planner->SetPathTolerence(dPathTol);
                planner->SetOrientation(false);
                planner->SetPosition(true);

                //if (planner->Plan())
                bool result = planner->Plan();

                PA_Points *plannerPath = (PA_Points *)planner->GetPath();
                path = *plannerPath;

                //To take a look at where planner fail to go on,
                //the path will be returned no matter the planing is success or not.
                return result;
        }
        else if (useGoalPose)
        {
                Configuration conf;
                Frame goalPose = GetToolFrame( GetGoalConfig() );
                bool Result = false;
                double oldTimeLimit = GetTimeLimitInSeconds();
                double timeLimit = 100;
                
                while (Result == false)
                {
                        Result = GenerateRandomConfigForPose(conf, goalPose);
                        if (Result)
                        {
                                timeLimit = GetTimeElapsedInSeconds() + 20;
                                oldTimeLimit = GetTimeLimitInSeconds();
                                if (timeLimit > oldTimeLimit)
                                        timeLimit = oldTimeLimit;
                                SetTimeLimitInSeconds( timeLimit );
                                PL_PRM::SetGoalConfig(conf);
                                Result = Plan_As_Usual();
                                if (GetTimeLimitInSeconds() != timeLimit)
                                        break;
                                else
                                        SetTimeLimitInSeconds( oldTimeLimit );
                        }
                        if (Result)
                                return true;
                        if ( HasTimeLimitExpired() )
                        {
                                return false;
                        }
                }
                return false;
                //return Plan_As_Usual();
                //return PL_PRM::Plan();
        }
        else
        {
                return Plan_As_Usual();
        }

/*
        Configuration q_last = q1;
        Configuration q_end = q2;
        path.AppendPoint(q1);
        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, q2));
        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);
                if (MakeItSatisfied(q_mid))  //if (Error(q_mid)<DEF_ERR_TOLERANCE)
                {
                        j = 0; k++;
                        if (Distance(q_mid, q_end) < Distance(q_last, q_end))
                        {
                                k = 0;
                                path.AppendPoint(q_mid);
                                q_last = q_mid;
                                distToLast = sqrt(Distance(q_last, q2));
                        }
                }
        }

        if (sqrt(Distance(q_last, q_end))<DEF_DIST_TOLERANCE)
        {
                path.AppendPoint(q2);
                return true;
        }
        else
        {
                return false;
        }
*/
//      return PL_PRM::Plan();
}

//****************************************************************************
//             Function to Get Tool Frame: End-effector Frame.
//****************************************************************************
Frame PL_RGD_PRM::GetToolFrame( const Configuration& config ) const
{
        Frame toolFrame = Matrix4x4();
        if ( collisionDetector != NULL )
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                collisionDetector->SetConfiguration( config );
                R_OpenChain * robot = (R_OpenChain *)collisionDetector->GetRobot(0);
                int toolFrameID = robot->GetToolFrame(0);
                if (toolFrameID == -1)
                {
                        toolFrameID = collisionDetector->JointFrameNum(collisionDetector->DOF()-1);
                        toolFrame = frameManager->GetWorldFrame( toolFrameID );
                }
                else
                {
                        toolFrame = *(frameManager->GetFrameRef(toolFrameID));
                        toolFrame = frameManager->GetTransformRelative(collisionDetector->DOF(), 0) * toolFrame;
                }
        }
        return toolFrame;
}

bool PL_RGD_PRM::Plan_As_Usual ()
{
        PRM_StateMachineType currentState = lastPlanningState;

        // Nodes may have been added since last plan.  Allow planner to check roadmap again
        // IMPROVE:  Is this still needed?
        if ( lastPlanningState == PRM_FAILURE )
        {
                currentState = PRM_START;
        }

        // Start the timer
        //StartTimer();

        // Establish the semaphore
        Semaphore semaphore( guid );
        if( this->m_UseSemaphores == true )
        {
                semaphore.Lock();
        }
        

        // Some quick checks to determine if we really need to plan
        if ( (currentState != PRM_DONE ) && (currentState != PRM_FAILURE) ) 
        {
                if ( startNode == goalNode )
                {
                        // start and goal are the same.  Do not plan.
                        currentState = PRM_DONE;

                        // Assign a small path.
                        path.Clear();
                        path.AppendPoint( GetStartConfig() );
                }
                else if ( (PL_PRM::IsInterfering(startNode)) || (PL_PRM::IsInterfering(goalNode)) )
                {
                        // either the start or goal are already interfering.  Plan will
                        // automatically fail.
                        currentState = PRM_FAILURE;
                }
        }
        if( this->m_UseSemaphores == true )
        {
                semaphore.Unlock();
        }

        // Ensure the diagonal length is correct
        diagonal_squared = GetCspaceRange();
        
        // State Machine
        while (( currentState != PRM_DONE ) && ( currentState != PRM_FAILURE )) 
                // NOTE:  The PRM_FAILURE state should eventually be removed!
        {
                SuccessResultType success;

                if( this->m_UseSemaphores == true )
                {
                        semaphore.Lock();
                }
                // Determine the State function to call for this iteration
                switch ( currentState )
                {
                        case PRM_BUILD_INIT_ROADMAP :
                                success = BuildInitRoadMap();
                                break;
                        case PRM_FIND_PATH :
                                success = FindPath();
                                break;
                        case PRM_VERIFY_PATH :
                                success = VerifyPath();
                                break;
                        case PRM_TRANSLATE_PATH :
                                success = TranslatePath();
                                break;
                        case PRM_ENHANCE_ROADMAP :
                                success = EnhanceRoadMap();
                                break;
                        default :
                                // No state functions are associated with this state.
                                // Do nothing except prepare for next iteration.
                                success = PASS;
                }  // end SWITCH


                // Check if timer has expired and one of the state functions had returned early.
                if ( success == TIMER_EXPIRED )
                {
                        // Escape the state machine.
                        break;
                }

                // Determine the State Function to call for the next iteration
                switch ( currentState )
                {
                        case PRM_START :
                                // Start by building the initial roadmap.
                                currentState = PRM_BUILD_INIT_ROADMAP;  
                                break;
                        case PRM_BUILD_INIT_ROADMAP :
                                // Search current road map for a short path.
                                currentState = PRM_FIND_PATH;                   
                                break;
                        case PRM_FIND_PATH :
                                if ( success == PASS )
                                {
                                        // A path has been found, verify it has no collision.
                                        currentState = PRM_VERIFY_PATH;
                                }
                                else
                                {
                                        // No path was found.  Enhance some of the trouble areas.
                                        currentState = PRM_ENHANCE_ROADMAP;
                                        //currentState = PRM_FAILURE;           // NOTE:  Once EnhanceRoadmap is working,
                                                                                                        //                the PRM_FAILURE state should be removed.
                                }
                                break;
                        case PRM_VERIFY_PATH :
                                if ( success == PASS )
                                {
                                        // Path represented by valid nodes is successful. Translate it so the
                                        // main program can implement it.
                                        currentState = PRM_TRANSLATE_PATH;
                                }
                                else
                                {
                                        // Path was not collision free.  Need to find a new path now that the troublesome
                                        // nodes/edges have been removed.
                                        currentState = PRM_FIND_PATH;
                                }
                                break;
                        case PRM_TRANSLATE_PATH :
                                // We are done.  Escape state machine and report success.
                                currentState = PRM_DONE;
                                break;
                        case PRM_ENHANCE_ROADMAP :
                                // Try finding a path again with the new nodes and edges added.
                                currentState = PRM_FIND_PATH;
                                break;
                        default :
                                // an unexpected or invalid state has occurred.  Crash the program!
                                ASSERT( FALSE );
                } // end SWITCH.

                // Release semaphore temporary.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }
        }  // END State Machine


        // save the current state of the machine for the next time the planner is called.
        lastPlanningState = currentState;

        // Report the success of the planner.
        if ( currentState == PRM_DONE )
        {
                // planner has completed.  A path has been found and saved.
                        
                // Release semaphore permanetly.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }

                return true;
        }
        else
        {
                // planner has not completed and has terminated early
                graphPath.clear();

                // Assign a small path.
                path.Clear();
                path.AppendPoint( GetStartConfig() );

                // Release semaphore permanetly.
                if( this->m_UseSemaphores == true )
                {
                        semaphore.Unlock();
                }

                return false;
        }
}  // END Plan

bool PL_RGD_PRM::GetSatisfactoryConfiguration(Configuration &conf, Frame &pose)
{
        int i,j;
        Configuration q;
        q = conf;
        i = 0; j = 0;
        int MAX_I = MAX_ITERATION * MAX_RETRY;
        int MAX_J = MAX_RETRY*2;
        double err_q = (this->*costFunc)(q, pose);
        while ((i<MAX_I) && (j<MAX_J) && (err_q>DEF_ERR_TOLERANCE))
        {
                i++;   j++;
                double deviation = DEF_NEIGHBOR;
                if (err_q < 1)
                        deviation *= err_q;
                Configuration q_next = PL_PRM::GenerateRandomConfig(q, deviation);
                if ((this->*costFunc)(q_next, pose) < err_q)
                {
                        j = 0; 
                        q = q_next;
                        err_q = (this->*costFunc)(q, pose);
                }
                if ( HasTimeLimitExpired() )
                {
                        return false;
                }
        }

        char msg[200];
        sprintf(msg, "I=%d, J=%d \t %f", i, j, err_q);
        //LogMessage("_generateConf.log", msg);
        conf = q;
        return (err_q <= DEF_ERR_TOLERANCE);
}

bool PL_RGD_PRM::GetSatisfiedConfiguration(Configuration &conf)
{
        UpdatePlanarConstraint();
        UpdateOrientConstraint();
        UpdateShpereConstraint();

        int i,j;
        Configuration q;
        q = conf;
        i = 0; j = 0;
        int MAX_I = MAX_ITERATION * MAX_RETRY;
        int MAX_J = MAX_RETRY;
        double err_q = Error(q);
        while ((i<MAX_I) && (j<MAX_J) && (err_q>DEF_ERR_TOLERANCE))
        {
                i++;   j++;
                double deviation = DEF_NEIGHBOR;
                if (err_q < 1)
                        deviation *= err_q;
                Configuration q_next = PL_PRM::GenerateRandomConfig(q, deviation);
                if (Error(q_next) < err_q)
                {
                        j = 0; 
                        q = q_next;
                        err_q = Error(q);
                }
        }

        conf = q;
        return (err_q <= DEF_ERR_TOLERANCE);
}

bool PL_RGD_PRM::GenerateRandomConfigForPose (Configuration &conf, Frame &endEffPose)
{
        conf = PL_PRM::GenerateRandomConfig();
        costFunc = &PL_RGD_PRM::Error1;

        bool Result = GetSatisfactoryConfiguration(conf, endEffPose);
        if (Result)
        {
                Result = !IsInterfering(conf);
        }
        return Result;
}

Configuration PL_RGD_PRM::GenerateRandomConfig ()
{
        Configuration conf;
        Frame endEffPose;
        conf = PL_PRM::GenerateRandomConfig();
        if (typeOfConstraint == CONSTRAINT_PLANAR)
        {
                endEffPose = planeDesired;
                costFunc = &PL_RGD_PRM::Error3;
        }
        else if (typeOfConstraint == CONSTRAINT_ORIENTATION)
        {
                endEffPose = orientDesired;
                costFunc = &PL_RGD_PRM::Error4;
        }
        else if (typeOfConstraint == CONSTRAINT_SPHERE)
        {
                costFunc = &PL_RGD_PRM::Error5;
        }
        else if (typeOfConstraint == CONSTRAINT_OTHER)
        {
        //At current moment, I use it for orientation constraint.
                endEffPose = orientDesired; 
                costFunc = &PL_RGD_PRM::Error6;
        }
        else
        {
                costFunc = &PL_RGD_PRM::Error2;
        }

        GetSatisfactoryConfiguration(conf, endEffPose);
        //MakeItSatisfied(conf);
        return conf;
}

Configuration PL_RGD_PRM::GenerateRandomConfig ( const Configuration& seed, const double& std_dev )
{
        Configuration conf;
        Frame endEffPose;
        conf = PL_PRM::GenerateRandomConfig(seed, std_dev);
        if (typeOfConstraint == CONSTRAINT_PLANAR)
        {
                endEffPose = planeDesired;
                costFunc = &PL_RGD_PRM::Error3;
        }
        else if (typeOfConstraint == CONSTRAINT_ORIENTATION)
        {
                endEffPose = orientDesired;
                costFunc = &PL_RGD_PRM::Error4;
        }
        else if (typeOfConstraint == CONSTRAINT_SPHERE)
        {
                costFunc = &PL_RGD_PRM::Error5;
        }
        else
        {
                costFunc = &PL_RGD_PRM::Error2;
        }

        GetSatisfactoryConfiguration(conf, endEffPose);
        return conf;
}

void PL_RGD_PRM::ConnectEdgesFull( const node& newnode, const double& radius_squared )
{
        // verify connectIDp is valid.
        assert( connectIDp != NULL );

        // Save calculation time by determining configuration of the node once intstead of in each iteration
        Configuration nodeconfig = G.inf(newnode);

        node n1;

        // create a list of neighbour nodes to connect to within the given radius.
        node_pq<double> neighbours(G);
        forall_nodes( n1, G)
        {
                if ( newnode != n1)
                {
                        double edgelength_squared = Distance( nodeconfig, G.inf(n1) );  // returns the squared distance
                        // Check if the two nodes are within radius
                        if (  edgelength_squared <= radius_squared )  // can compare squares since a^2<b^2 if |a|<|b|
                        {
                                // node is within radius, add it as a neighbour with its distance as its priority.
                                neighbours.insert( n1, edgelength_squared );
                        }
                }
        }

        // New node is currently not connected to anyone, assign it the NIL_ID.
        (*connectIDp)[newnode] = NIL_ID;

        // Create a running list of graph connected components new node is attached to.
        list<int> connectIDs;

        // While the list of neighbour is not empty, connect the node to the closests nodes, not already graph
        // connected to.
        while ( !neighbours.empty() )
        {
                // get closest neighbour and remove it from the list
                node neighbour = neighbours.find_min();
                double dist_sq = neighbours.inf( neighbour );
                neighbours.del( neighbour );

                // connect the neighbour to the new node if it is not already graph connected.
                // A node is graph connected if its current connectID is in the running list of
                // connected graph components or has the same connectID as the new node.
                if ( ((*connectIDp)[neighbour] == NIL_ID) ||
                         ( ((*connectIDp)[neighbour] != (*connectIDp)[newnode]) &&
                           ( !NodeInConnectionList(neighbour, connectIDs) ) )
                   )
                {
                        edgeFrag->clear();
                        // nodes are not aleady connected.  Connect them now if there is a collision free path.
                        if ( !IsInterfering( nodeconfig, G.inf(neighbour) ) )
                        {
                                double edgelength = sqrt( dist_sq );
                                edge newedge = G.new_edge( newnode, neighbour, edgelength );
                                edgeChecked[newedge] = TRUE;
                                edgePath[newedge] = *edgeFrag;

                                //LogMessage("closedchain.log", "Configurations checked in fragment");
                                //for (int testj=0; testj<edgePath[newedge].size(); testj++)
                                //{
                                //      Fragment& E= edgePath[newedge];
                                //      list_item itt = E.get_item(testj);
                                //      Configuration conf = (E[itt]);
                                //      LogVector("closedchain.log", conf, "Conf: ");
                                //}

                                // update the connectionIDs to show these two nodes are now graph connected.
                                if ( (*connectIDp)[neighbour] != NIL_ID )
                                // neighbour node has been previously connected to a graph component.
                                {
                                        if ( (*connectIDp)[newnode] == NIL_ID )
                                        {
                                                // node has not been previously connected, assign it the connect ID of its new neighbour.
                                                (*connectIDp)[newnode] = (*connectIDp)[neighbour];
                                        }
                                        else if ( (*connectIDp)[neighbour] < (*connectIDp)[newnode] )
                                        {
                                                // the neighbour node has the smaller ID.  Select this as the new ID for the two
                                                // newly connected graph components (one from the new neighbour, one from the new node).

                                                // save the old connectionID into the running list to be updated later.
                                                connectIDs.push( (*connectIDp)[newnode] );

                                                // assign the lower connection ID to the new node.
                                                (*connectIDp)[newnode] = (*connectIDp)[neighbour];
                                        }
                                        else
                                        {
                                                // the new node has the smaller ID.  Select this as the new ID for the two components.

                                                // save the old connectionID into the running list to be updated later.
                                                connectIDs.push( (*connectIDp)[neighbour] );

                                                // assign the lower connection ID to the neighbour.
                                                (*connectIDp)[neighbour] = (*connectIDp)[newnode];
                                        }
                                }  // end if neighbour != NIL_ID
                                else
                                // neighbour has not been previously connected.
                                {
                                        if ( (*connectIDp)[newnode] != NIL_ID )
                                        {
                                                // new node has been previously connected, assign the neighbour its ID.
                                                (*connectIDp)[neighbour] = (*connectIDp)[newnode];
                                        }
                                        else
                                        {
                                                // both nodes have not been previously connected.  Create a new graph connected
                                                // component by assigning these two nodes the baseConnectID.
                                                (*connectIDp)[newnode] = baseConnectID;
                                                (*connectIDp)[neighbour] = baseConnectID;

                                                // Update the base connect ID to ensure a new unique ID is available for next new node.
                                                baseConnectID++;
                                        }
                                }  // end update connectIDs.

                        } // end if interfering
                        else
                        {
                                edgeFrag->clear();
                                // nodes have a collision path.  Use the midpoint between these two nodes as a seed for enhancing.
                                // Add the midpoint of this potential edge as a seed, if requested
                                if ( useMidPts )
                                {
                                        Configuration midpt = GetMidPoint( nodeconfig, G.inf(neighbour) );
                                        config_seeds.append( midpt );
                                }
                        }  // end if interfering else.

                }       // end if not already graph connected.

        } // end while !neighbours.empty().

        // Search the entire graph, if required, updating connectIDs to reflect how new node has graph connected
        // different components of the graph.
        if ( ((*connectIDp)[newnode] != NIL_ID) && ( !connectIDs.empty() ) )
        {
                // new node is connected to more than one graph connected component.  Update the connect IDs of
                // affected nodes.
                int newID = (*connectIDp)[newnode];

                forall_nodes( n1, G )
                {
                        if ( ((*connectIDp)[n1] != newID ) && (NodeInConnectionList( n1, connectIDs )) )
                        {
                                (*connectIDp)[n1] = newID;
                        }
                }
        }  // end update connectIDs of entire graph

        return;
}

bool PL_RGD_PRM::MakeItSatisfied(Configuration &conf)
{
        int i,j;
        Configuration q;
        q = conf;
        i = 0; j = 0;
        int MAX_I = MAX_ITERATION * MAX_RETRY;
        int MAX_J = MAX_RETRY;
        double err_q = Error(q);
        while ((i<MAX_I) && (j<MAX_J) && (err_q>DEF_ERR_TOLERANCE))
        {
                i++;   j++;
                double deviation = DEF_NEIGHBOR;
                if (err_q < 1)
                        deviation *= err_q;
                Configuration q_next = PL_PRM::GenerateRandomConfig(q, deviation);
                if (Error(q_next) < err_q)
                {
                        j = 0; 
                        q = q_next;
                        err_q = Error(q);
                }
                if ( HasTimeLimitExpired() )
                {
                        return false;
                }
        }

        conf = q;
        return (err_q <= DEF_ERR_TOLERANCE);
}

bool PL_RGD_PRM::IsSatisfied(const Configuration &conf) const
{
        return (Error(conf) <= DEF_ERR_TOLERANCE);
}

bool PL_RGD_PRM::IsInterfering ( const Configuration& q1, const Configuration& q2 )
{
        int i, j, k;
        i = 0;  j = 0;  k = 0;
        Configuration q_last = q1;
        //Configuration q_end = q2;
        edgeFrag->append(q1);
        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, q2));
        bool result = false;
        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)) + q2*(DEF_DIST_TOLERANCE/distToLast);
                if (MakeItSatisfied(q_mid))  //if (Error(q_mid)<DEF_ERR_TOLERANCE)
                {
                        j = 0; k++;
                        if (PL_PRM::IsInterfering(q_mid))
                        {
                                result = true;
                                break;
                        }
                        if (Distance(q_mid, q2) < distToLast*distToLast)
                        {
                                k = 0;
                                edgeFrag->append(q_mid);
                                q_last = q_mid;
                                distToLast = sqrt(Distance(q_last, q2));
                        }
                }
                if ( HasTimeLimitExpired() )
                {
                        result = true;
                        break;
                }
        }

        //LogMessage("closedchain.log", "Configurations added into fragment");
        //for (int testj=0; testj<edgeFrag->size(); testj++)
        //{
        //      list_item itt = edgeFrag->get_item(testj);
        //      Configuration conf = edgeFrag->contents(itt);
        //      LogVector("closedchain.log", conf, "Conf: ");
        //}

        if ((!result) && (distToLast<=DEF_DIST_TOLERANCE))
        {
                edgeFrag->append(q2);
                return false;
        }
        else
        {
                edgeFrag->clear();
                return true;
        }
}

bool PL_RGD_PRM::IsInterfering ( const Configuration& c1 )
{
        if (!IsSatisfied(c1)) 
                return true;
        if (PL_PRM::IsInterfering(c1))
                return true;
        return false;
}


void PL_RGD_PRM::UpdatePlanarConstraint() 
{
        planeDesired = GetToolFrame( GetStartConfig() );
}

void PL_RGD_PRM::UpdateOrientConstraint()
{
        //Make it same as the start configuration.
        //orientDesired = GetToolFrame( GetStartConfig() );
        
        //Make it upward
        if (typeOfConstraint == CONSTRAINT_OTHER)
        {
                orientDesired = Matrix4x4::Identity();
                orientDesired.Rotate2(Vector4(0, 1, 0), 90);//For Rightward
        }
        else
        {
                orientDesired = Matrix4x4::Identity();
                orientDesired.Rotate2(Vector4(1, 0, 0), 90);//For Upward
                //orientDesired.Rotate2(Vector4(0, 1, 0), 90);//For Rightward

                //Or, you can choose desired Z-axis fixed to wherever you want.
                //orientDesired.Rotate(Vector4(0, 0, 1), 90);
                //orientDesired.Rotate(Vector4(1, 0, 0), 90);
    }
}

void PL_RGD_PRM::UpdateShpereConstraint() 
{
        return;
}

//Distance between a configuration to a pose, and it is used to generate goal configurations
double PL_RGD_PRM::Error1(const Configuration &p1, const Frame &pose) const
{
        double distance;

        Frame toolFrame = GetToolFrame( p1 );

        if (typeOfConstraint == CONSTRAINT_ORIENTATION)
        {
                Vector4 offset = pose.GetTranslationVector() - toolFrame.GetTranslationVector();
                distance = offset.Magnitude();

                //=== Basically, this is the metrics used in IK-ACA;
                //Vector4 PX(1, 0, 0);
                //Vector4 PY(0, 1, 0);
                Vector4 PZ(0, 0, 1);

                // Generate vectors from base frame to each of the unit vector of the given frames.
                //Vector4 Pax = toolFrame * PX;
                //Vector4 Pay = toolFrame * PY;
                Vector4 Paz = toolFrame * PZ;

                //Vector4 Pbx = pose * PX;
                //Vector4 Pby = pose * PY;
                Vector4 Pbz = pose * PZ;

                // Generate difference vectors that connect the corresponding unit vectors of the given frames
                //Vector4 dx = Pax - Pbx;
                //Vector4 dy = Pay - Pby;
                Vector4 dz = Paz - Pbz;

                // Calculate the value of the distance metric (square the magitude of each difference vectors and sum.
                //double distance_sq = dx.MagSquared() + dy.MagSquared() + dz.MagSquared();
                //distance = sqrt(distance_sq);
                distance += dz.Magnitude(); //need to make sure the origin of tool frame concide.
        }       
        else if (typeOfConstraint == CONSTRAINT_SPHERE)
        {
                Vector4 offset = pose.GetTranslationVector() - toolFrame.GetTranslationVector();
                distance = offset.Magnitude();

                Vector4 center(RGD_SPHERE_CX, RGD_SPHERE_CY, RGD_SPHERE_CZ);
                //Get desired Z-axis
                Vector4 pos1 = pose.GetTranslationVector();
                Vector4 desiredZ = center;
                desiredZ -= pos1;
                desiredZ.Normalize();
                //desiredZ *= -(RGD_SPHERE_CX - 1);
                //desiredZ += center;
                //Get current Z-axis
                Vector4 pos2 = toolFrame.GetTranslationVector();
                Vector4 currentZ;
                currentZ[0] = toolFrame(0, 2);
                currentZ[1] = toolFrame(1, 2);
                currentZ[2] = toolFrame(2, 2);
                //currentZ += pos2;
                
                desiredZ -= currentZ;
                distance += desiredZ.Magnitude();
        }
        else
        {
                Vector4 offset = pose.GetTranslationVector() - toolFrame.GetTranslationVector();
                distance = offset.Magnitude();
        }

        return distance;
}

//Distance between a configuration to a closure constraint
double PL_RGD_PRM::Error2(const Configuration &p1, const Frame &pose) const
{
        //This can only be use for OUR planar robot.
        //The reason to set the offset[2] to be 0 is because the thickness of the
        Frame toolFrame = GetToolFrame( p1 );
        Vector4 offset = toolFrame.GetTranslationVector();
        offset[2] = 0;
        return offset.Magnitude();
}

//Distance between a configuration to a plane
double PL_RGD_PRM::Error3(const Configuration &p1, const Frame &pose) const
{
        //Simplify the code by assuming the plane is parrallel to X-Z plane
        Frame toolFrame = GetToolFrame( p1 );
        return fabs(toolFrame(1, 3)-pose(1, 3));
}

//Distance between a configuration to a desired orientation
//To change the constraint, please modify the function "UpdateOrientConstraint"
double PL_RGD_PRM::Error4(const Configuration &p1, const Frame &pose) const
{
        //=== Take the rotation angle between current Z-axis
        //===    and desired Z-axis as the distance.
        //===    I set the unit to degree for better precision
        Frame toolFrame = GetToolFrame( p1 );
        Vector4 desiredZ, currentZ;
        desiredZ = orientDesired*Vector4(0, 0, 1);  
        currentZ[0] = toolFrame(0, 2);  
        currentZ[1] = toolFrame(1, 2);
        currentZ[2] = toolFrame(2, 2);  //X axis;
        double anglecos = currentZ.Dot(desiredZ);
        if (anglecos > 1)
                anglecos = 1;
        else if (anglecos < -1)
                anglecos = -1;
        //return sqrt(1.-anglecos*anglecos); //when theta is small; sin(theta)=theta;
    return fabs(acos(anglecos));
    //return Rad2Deg(acos(anglecos));
        
        //=== There another ways to do this: 
        //===     Compare RPY-angle, which doesn't work well in
        //====        cases where Z-axis is required to be awry.
        //double yaw1, pitch1, roll1;
        //double yaw2, pitch2, roll2;
        //GetRotAngles(toolFrame, roll1, pitch1, yaw1);
        //GetRotAngles(pose, roll2, pitch2, yaw2);
        //return ((roll1-roll2)*(roll1-roll2) + (yaw1-yaw2)*(yaw1-yaw2); //Upward
        //return ((roll1-roll2)*(roll1-roll2) + (pitch1-pitch2)*(pitch1-pitch2)); //Rightward
}

//For a sphere constraint, with the end-effector pointing toward the shpere center.
double PL_RGD_PRM::Error5(const Configuration &p1, const Frame &pose) const
{
        Vector4 center(RGD_SPHERE_CX, RGD_SPHERE_CY, RGD_SPHERE_CZ);
        //Vector4 pos1 = pose.GetTranslationVector(); //pose is not used here
    Frame toolFrame = GetToolFrame(p1); 
        Vector4 pos1 = toolFrame.GetTranslationVector();

#ifdef SHPERE_ORIENTATION //sphere constraint with constraint
        Vector4 desiredZ = center;
        desiredZ -= pos1;       //Get desired Z-axis
        desiredZ.Normalize();
        desiredZ *= -(RGD_SPHERE_RADIUS-1);
        desiredZ += center;
        //Get current Z-axis
        Vector4 pos2 = toolFrame.GetTranslationVector();
        Vector4 currentZ;
        currentZ[0] = toolFrame(0, 2);
        currentZ[1] = toolFrame(1, 2);
        currentZ[2] = toolFrame(2, 2);
        currentZ += pos2;

        desiredZ -= currentZ;
        return desiredZ.Magnitude();

#else //Without orientation
        pos1 -= center;
        return sqrt((pos1.Magnitude() - radius)*(pos1.Magnitude() - radius));
#endif
}

double PL_RGD_PRM::Error6(const Configuration &p1, const Frame &pose) const
{
        //Pending for future.
        return Error4(p1, pose);
}

double PL_RGD_PRM::Error(const Configuration &p1) const
{
        Frame toolFrame = GetToolFrame( p1 );

        if (typeOfConstraint == CONSTRAINT_PLANAR)
        {
                return Error3(p1, planeDesired); //Planar constraint
        }
        else if (typeOfConstraint == CONSTRAINT_ORIENTATION)
        {
                return Error4(p1, orientDesired); //Orientation constraint.
        }
        else if (typeOfConstraint == CONSTRAINT_SPHERE)
        {
                return Error5(p1, toolFrame); //Sphere constraint
        }
        else if (typeOfConstraint == CONSTRAINT_OTHER)
        {
                return Error6(p1, toolFrame); //Constraint for testing
        }
        else
        {
                return Error2(p1, toolFrame); //Closure constraint
        }
}

void PL_RGD_PRM::GetRotAngles( const Matrix4x4& frame, double& roll, double& pitch, double& yaw ) const
{
        // Declare the fixed angle variables.
        double alpha, beta, gamma;
        
        // Calculate beta and cache the cosine.
        beta = atan2( -frame(2,0) , sqrt( Sqr(frame(0,0)) + Sqr(frame(1,0)) ) );
        double cB = cos ( beta );

        // Calculate the other two rotations
        if ( cB == 0.0 )
        { 
                alpha = 0;
                if ( beta > 0 ) // if beta == PI/2
                {
                        gamma = atan2( frame(0,1) , frame(1,1) );
                }
                else
                {
                        gamma = -atan2( frame(0,1), frame(1,1) );
                }
        }
        else
        {
                alpha = atan2( frame(1,0) / cB , frame(0,0) / cB );
                gamma = atan2( frame(2,1) / cB , frame(2,2) / cB );
        }

        // Convert angles to degrees and assign to appropriate parameters
        // NOTE:  These angles must correspond to same angles used in SetGoalFrame
        yaw   = Rad2Deg( alpha );
        pitch = Rad2Deg( beta );
        roll  = Rad2Deg( gamma );
}

SuccessResultType PL_RGD_PRM::TranslatePath()
{
        // Clear any current path.
        path.Clear();

        // Check if a graph path is available.
        if ( graphPath.empty() )
        {
                // Path is empty.  Set start as the beginning of the path, and return.
                path.AppendPoint( GetStartConfig() );

                // no path exists.
                return FAIL;
        }

        // If still in the function, there is a graphPath.
        // Go through all nodes stored in graphPath and assign the configuration they correspond to the
        // actual path.
        node n1;
        node n2;
        forall( n1, graphPath )
        {
                Configuration conf = G.inf(n1);
                path.AppendPoint( conf );
                n2 = graphPath.succ(n1);
                if (n2 == NULL)
                        continue;

                edge e;
                forall_inout_edges(e, n1)
                //edge e = G.first_in_edge(n1);
                //while(e)
                {
                        if (G.opposite(n1, e) == n2)
                        {
                                int nStep = 0;
                                int dir = -1;
                                Fragment& frag = edgePath[e];
                                int nFirst = frag.size();
                                if (conf == frag[frag[0]])
                                {
                                        nFirst = 0;
                                        dir = 1;
                                }
                                for (int i=1; i<(frag.size()-1); i++)
                                {
                                        nStep += dir;
                                        list_item it = frag[nFirst+nStep];
                                        conf = frag[it];
                                        //->get_item(i);//((*frag)[nFirst+dir]);
                                        path.AppendPoint( conf );
                                }
                                break;
                        }
                }
        }

        // Path has been translated and is ready to be animated.
        return PASS;
}

//===========================================================================================

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_RGD_RRT::PL_RGD_RRT()
{
        m_rootVert = NULL;
        m_goalVert = NULL;
}

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

bool PL_RGD_RRT::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_RGD_RRT::Plan ()
{
        srand( (unsigned)time( NULL ) );

        path.Clear();

        // Start the timer
        StartTimer();

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

        int result = ERR_FAIL;
        bool pathFound = false;
        m_goalVert = NULL;
        
        int repeatNum = 0;
        while ((!pathFound) && (result != ERR_TIMEOUT))
        {
                PA_Points localPath;
                Configuration randConf = PL_RGD_PRM::GenerateRandomConfig();
                vertex fromVert = FindClosestInTree(randConf);
                Configuration fromConf = GetConfigurationFromTree(fromVert);
                Configuration toConf;

                result = Extend(fromConf, toConf, randConf, localPath);
                if (result == ERR_SUCCESS)
                {
                        fromVert = AddNodeInTree(fromVert, toConf, localPath);
                        //if (ConnectToGoal(fromVert) == ERR_SUCCESS)
                        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;
}

#define LEN_STEP_SIZE   10
#define MAX_CONF_RETRY  10
int PL_RGD_RRT::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 = GetSatisfiedConfiguration( 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_RGD_RRT::ConnectToGoal(vertex fromVert)
int PL_RGD_RRT::ConnectToGoal()
{
        Configuration goalConf = GetGoalConfig();
        Frame goalPose = GetToolFrame(goalConf);
        bool result = true;
        if (useGoalPose)
        {
                result = false;
                int retry = 0;
                while ( (!result) && (retry++<MAX_CONF_RETRY) && (!HasTimeLimitExpired()))
                {
                        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);
                        return ERR_SUCCESS;
                }
        }
        return ERR_FAIL;
}

bool PL_RGD_RRT::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_RGD_RRT::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_RGD_RRT::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_RGD_RRT::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_RGD_RRT::GetConfigurationFromTree(vertex vert)
{
        VertexInfoInTree *info;
        info = (VertexInfoInTree *)m_trajTree.vertex_inf(vert);
        return info->conf;
}

double PL_RGD_RRT::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_RGD_RRT::GetPathFromTree(vertex vert)
{
        VertexInfoInTree *info;
        info = (VertexInfoInTree *)m_trajTree.vertex_inf(vert);
        return info->edge;
}

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

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

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

void PL_RGD_RRT::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 ] ) ;
        }
}

PL_RGD_Constraint::PL_RGD_Constraint()
{
        m_strategy = STRATEGY_RRT;
}

void PL_RGD_Constraint::SetStrategy(int strategy)
{
        if (m_strategy != strategy)
        {
                path.Clear();
                if (m_strategy == STRATEGY_RRT)
                {
                        this->ClearTree();
                }
                else
                {
                        this->ClearGraph();
                }
        }
        m_strategy = strategy;
}

int PL_RGD_Constraint::GetStrategy()
{
        return m_strategy;
}

bool PL_RGD_Constraint::DrawExplicit () const
{
        if (m_strategy == STRATEGY_RRT)
                return PL_RGD_RRT::DrawExplicit();
        else
                return PL_RGD_PRM::DrawExplicit();
}

bool PL_RGD_Constraint::Plan ()
{
        if (m_strategy == STRATEGY_RRT)
                return PL_RGD_RRT::Plan();
        else
                return PL_RGD_PRM::Plan();
}

---