planners/ik_mpep/PL_MPEP.cpp

Go to the documentation of this file.

//
//Contents: Class PL_MPEP
// 
//PL_MPEP: Planner for Motion Planning along given End-effector Path.
//This algorithm is based on the following paper, For more information
//about original consideration and algorithm, please refer to that.
//
//        M.Ottavi, G.Oriolo and M.Vendittelli. Probabilistic motion planning
//        for redundant robots along given end-effector paths. 
//        IEEE Intl. Conf. on Intelligent Robots and Systems, 2002.
//
//Extending from this paper, some improved algorithm is proposed as well.
//
//For original paper, the following algorithms are implemented:
//       Greedy Planner
//       RRT-Like Planner
//       RRT-Connect-Like Planner
//       RRT-Greedy-Like Planner
//       RRT-Greedy-Connect Planner
//      
//For extension, the following algorithms are imlemented:
//       Improved-Greedy Planner
//       Self-motion-Greedy Planner
//       Self-motion-RRT-Greedy-Like Planner
//       Self-motion-RRT-Connect-Like Planner
//
//
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Jan-2002:  Z.Yao     Original algorithms implemented
//     Mar-2003:  Z.Yao     Improve algorithms implemented

#include <stdio.h>

#include "math/math2.h"
#include "math/matrixmxn.h"
#include "math/vector4.h"
#include <synchronization/semaphore.h>
#include "PL_Mpep.h"
#include "opengl/glos.h"
#include <gl/gl.h>
#include "SMGraph.h"
#include "debug/debug.h"
//#define Log(a, b, c) ;
//#define LogMessage(a, b) ;

using namespace std;

#define _FIRST_POSE   (0)
#define _LAST_POSE    (poses.size()-1)
#define DEF_SMG_STEPSIZE        10
#define DEF_ERROR_TOLERANCE     0.01

#define MAX_SMG_EXTEND_RETRY    10

static int numSMGraph=0;
static int numSMGNode=0;
static int numFinalSMGraph=0;
static int numFinalSMGNode=0;
static int numSMGraphInPath=0;
static int numSMGNodeInPath=0;
static int numTotalNode=0;
//------------------------------------------------------------------
//
//Construction and Deconstruction function
//
//------------------------------------------------------------------
PL_MPEP::PL_MPEP()
{
        distTolerance = DEFAULT_TOLERANCE;
        randomRetry = DEFAULT_RANDOM_RETRY;
        algorithm = ALG_GREEDY_PLANNER;
        passiveNum = 2;
        extendNum = DEFAULT_EXTEND_RETRY;
        exploreNum = DEFAULT_EXPLORE_RETRY;
        iterationNum = DEFAULT_ITERATION;
        smgNodeNum = DEFAULT_SMGNODE_NUM;
        usingStartConfig = true;
        usingGoalConfig = false;
        usingGoalTree = false;
        m_pJacobian = NULL;
        usingJacobianExt = false;
        ClearTree();
}

PL_MPEP::~PL_MPEP()
{
        if (m_pJacobian != NULL)
                delete m_pJacobian;
        ClearTree();
}

//------------------------------------------------------------------
//
//Get tool frame by configuration
//
//------------------------------------------------------------------
Frame PL_MPEP::GetToolFrame( const Configuration& config ) const
{
        Frame toolFrame = Matrix4x4();

        redundant.GetDesireFrameByConfiguration(config, toolFrame);

        return toolFrame;
}

//------------------------------------------------------------------
//
//Begin planning
//
//------------------------------------------------------------------
bool PL_MPEP::Plan()
{
        //Set seed of random, it's supposed be done by upper layer, actually
        if (m_pJacobian == NULL)
                m_pJacobian = new CJacobian(*m_pRobot);

        srand( (unsigned)time( NULL ) );

        numTotalNode = 0;

        path.Clear();

        int result=ERROR_OK;

        Configuration startConfig = GetStartConfig();
        Configuration goalConfig = GetGoalConfig();

        if (usingStartConfig)
        {
                if (collisionDetector->IsInterfering(startConfig))
                {
                        result = ERROR_START_CONFIG_INVALID;
                }
        }

        StartTimer();

        if (result == ERROR_OK)
        {
                //AddTrajectory(startConfig, goalConfig);
                switch (algorithm)
                {
                case ALG_GREEDY_PLANNER:
                        result = Planner_Greedy();
                        break;
                case ALG_RRT_LIKE:
                        result = Planner_RRT_Like();
                        break;
                case ALG_RRT_CONNECT_LIKE:
                        result = Planner_RRT_Connect_Like();
                        break;
                case ALG_RRT_GREEDY_LIKE:
                        result = Planner_RRT_Greedy_Like();
                        break;
                case ALG_RRT_GREED_CONNECT:
                        result = Planner_RRT_Greedy_Connect();
                        break;
                case ALG_IMP_GREEDY_PLANNER:
                        result = Planner_IMP_Greedy();
                        break;
                case ALG_SMG_GREEDY_PLANNER:
                        result = Planner_SMG_Greedy();
                        break;
                case ALG_SMG_RRT_PLANNER:
                        result = Planner_SMG_RRT_Greedy_Like();
                        break;
                case ALG_SMG_CONNECT_PLANNER:
                        result = Planner_SMG_RRT_Connect_Like();
                        break;
                default:
                        result = ERROR_UNKNOW_PLANNER;
                        break;
                }
        }

        poses.clear();
        if (result == ERROR_OK)
                return true;

        Display_Error_Message(result);
        path.Clear();
        path.AppendPoint(startConfig);
        return false;
}

//------------------------------------------------------------------
//This function is use to extend from pose "begin" to pose "end",
//      it just for the GREEDY-series planners.
//
//Input Parameter:
//      begin: The index of pose we going to explore from
//      end  : The index of target pose
//      base : The pointer to vertex corresponding to "begin"
//
//Ouput Parameter:
//      begin: The index of pose where we fail to go further
//      base : The pointer to the failure vertex
//
//Return Value:
//      ERROR_OK, if success
//      ERROR_GET_RANDOM_CONF_FAILURE,
//      ERROR_GET_COLLISION_FREE_FAILURE,
//
//Notes:
//      The exploration will fail if:
//       - fail to get a closed configuration, or
//       - fail to get a collision free configuration
//------------------------------------------------------------------
int PL_MPEP::Greedy_Step(int &begin, int end, vertex &base)
{
        int i;
        int retry;
        int last=begin;
        vertex parent=base;
        Pose_Node *node=(Pose_Node *)tree.vertex_inf(base);
        Pose_Node newNode;
        Configuration conf;
        Configuration prevConf=node->conf;

        for(i=begin; i<end; i++)
        {
                retry = 0;
                while (retry < exploreNum)
                {
                        if (GetRandomConfiguration(conf, poses[i+1], prevConf))
                        {
                                if (!collisionDetector->IsInterfering(conf))
                                {
                                        if (!collisionDetector->IsInterferingLinear(prevConf, conf))
                                        {
                                                newNode.conf = conf;
                                                newNode.poses = i+1;
                                                newNode.graph = NULL;
                                                parent = ConnectNode(tree, vertices, parent, newNode);
                                                prevConf = conf;
                                                begin = i+1;
                                                base = parent;
                                                break;
                                        }
                                }
                        }

                        retry ++;
                }//End while
                if ((retry == exploreNum))
                {
                        return ERROR_GET_COLLISION_FREE_FAILURE;
                }
        }//End for

        return ERROR_OK;
}

bool PL_MPEP::AdjustConfiguration(Configuration &config, int nPose)
{
        Frame desiredFrame = poses[nPose];
        Vector4 desiredPos = desiredFrame.GetTranslationVector();

        //LogMessage("reconfig.log", "Start From Here................");
        //LogMatrix("reconfig.log", desiredFrame, "Desired Frame");

        int logRetry=0;
        while (true)
        {
                Frame currentFrame = GetToolFrame(config);
                Vector4 currentPos = currentFrame.GetTranslationVector();

                //LogConfiguration("reconfig.log", config);
                //LogMatrix("reconfig.log", currentFrame, "Current Frame");
                
                Vector4 offset = desiredPos - currentPos;
                double dDistance = offset.Magnitude();
                if (dDistance < DEF_ERROR_TOLERANCE)
                        break;

                logRetry++;

                Matrixmxn mJEnd, mJEndInv;
                m_pJacobian->SetConfiguration(config);
                m_pJacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
                mJEnd = *(m_pJacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                //LogMatrix("reconfig.log", mJEnd, "Jacobian");
                //LogMatrix("reconfig.log", mJEndInv, "Jacobian Inverse");

                VectorN endEffOffset(offset[0], offset[1], offset[2]);
                Configuration jointOffset;
                jointOffset = mJEndInv * endEffOffset *  Rad2Deg(1);
                config += jointOffset;
        }
        //Log("reconfig.log", "It take: %d  iterations to do adjustment", logRetry);
        return true;
}

bool PL_MPEP::IsJointWithinLimits(Configuration& config)
{
        bool bResult = true;
        for (int i = 0; i<m_nDof; i++)
        {
                if ( config[i] < collisionDetector->JointMin(i) )
                {
                        bResult = false;
                }
                else if ( config[i] > collisionDetector->JointMax(i) )
                {
                        bResult = false;
                }
        }
        return bResult;
}

int PL_MPEP::Extend_Node_Self_Motion_Graph_Jacobian(vertex g_vertex, Configuration **nextConf)
{
        void* nodeInGraph=NULL;
        Pose_Node *node = (Pose_Node*) tree.vertex_inf(g_vertex);
        SMGraph *g = node->graph;
        (*nextConf)=NULL;

        //The method use to explore the S-M-G is RRT like exploration method
        //  1. Put a random configuration, as a random direction
        //  2. Get the closest configuration "q" in this graph
        //  3. Make one step further from "q" in this direction, with fixed length
        Configuration randConf, activeConf, newConf;
        Configuration *nearConf;
        bool bFind = false;

        Matrixmxn mJEnd, mJEndInv;
        Matrixmxn mTemp(m_nDof, m_nDof); 
        Matrixmxn matrixIdentity(m_nDof, m_nDof);
        //int logK = 0;
        //for (int k=0; k<MAX_SMG_EXTEND_RETRY; k++)
        //{
        for (int i=0; i<randomRetry; i++)
        {
                //logK++;
                GetRandomConfiguration(randConf);

                //The v here, is actually the node in self-motion graph
                nodeInGraph = GetNearestNodeInGraph(g, &randConf);
                nearConf = g->GetConfiguration(nodeInGraph);
                Configuration dirVel = randConf - (*nearConf);

                mJEnd = *(m_pJacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                mTemp = matrixIdentity;
                mTemp -= mJEndInv * mJEnd;
                dirVel = mTemp * dirVel;
                dirVel *= (DEF_SMG_STEPSIZE/ dirVel.Magnitude());
                newConf = *nearConf + dirVel;
                if (!IsJointWithinLimits(newConf))
                        continue;
                if (collisionDetector->IsInterfering(newConf))
                        continue;

                AdjustConfiguration(newConf, node->poses);
                if (collisionDetector->IsInterferingLinear(newConf, *nearConf))
                        continue;

                bFind = true;
                break;
        }
        
        //}
        if (!bFind)
        {
                //Log("PlannerForTest.log", "Failed to extend SMG after : %d  retries", logK);
                return ERROR_GET_COLLISION_FREE_FAILURE;
        }

        //Log("PlannerForTest.log", "Succeeded to extend SMG after : %d  retries", logK);
        nodeInGraph = g->AddConnection(nodeInGraph, newConf);
        (*nextConf) = g->GetConfiguration(nodeInGraph);


        //Testing for validity, need to be removed later
        Frame currentFrame = GetToolFrame(**nextConf);
        Frame desiredFrame = poses[node->poses];
        Vector4 vec1 = currentFrame.GetTranslationVector();
        Vector4 vec2 = desiredFrame.GetTranslationVector();
        vec2 -= vec1;
        double dDistance = vec2.Magnitude();
        //Log("PlannerForTest.log", "The distance to pose: %d, is %f", node->poses, dDistance);

        return ERROR_OK;
}

//------------------------------------------------------------------
//This function is use to explore self-motion graph for a given pose,
//      it is for the those planners enabled self-motion.
//
//Input Parameter:
//      g_vertex: The vertex in the tree for the given pose
//
//Ouput Parameter:
//      nextConf: The valid configuration generated in this graph
//
//Return Value:
//      ERROR_OK, if success
//      ERROR_GET_RANDOM_CONF_FAILURE,
//
//Notes I:
//      Here, we just consider those node which is connecteable to current graph.
//          those not connectable will be discarded
//
//Notes II:
//      The exploration will fail if:
//       - fail to get a closed configuration, or
//       - fail to get a collision free configuration
//       - the number of node achieve the limit
//------------------------------------------------------------------
int PL_MPEP::Extend_Node_Self_Motion_Graph(vertex g_vertex, Configuration **nextConf)
{
        void* nodeInGraph=NULL;
        Pose_Node *node = (Pose_Node*) tree.vertex_inf(g_vertex);
        SMGraph *g = node->graph;
        (*nextConf)=NULL;

        //The method use to explore the S-M-G is RRT like exploration method
        //  1. Put a random configuration, as a random direction
        //  2. Get the closest configuration "q" in this graph
        //  3. Make one step further from "q" in this direction, with fixed length
        Configuration randConf, activeConf, newConf;
        Configuration *nearConf;
        bool bFind = false;

        //for (int k=0; k<MAX_SMG_EXTEND_RETRY; k++)
        //{
        for (int i=0; i<randomRetry; i++)
        {
                GetRandomConfiguration(randConf);

                //The v here, is actually the node in self-motion graph
                nodeInGraph = GetNearestNodeInGraph(g, &randConf);
                nearConf = g->GetConfiguration(nodeInGraph);

                if (!GetConfigurationByDirection(activeConf, *(nearConf), randConf))
                        continue;

                //I am thinking whether we should change the tolerance to make generating
                //   feasible configurations easier.
                if (!redundant.GetConfigurationByActive(newConf, activeConf, *nearConf,
                                                                                                poses[node->poses], distTolerance))
                //if (!redundant.GetConfigurationByActive(newConf, activeConf, *nearConf,
                //                                        poses[node->poses], activeConf.DOF()*distTolerance))
                        continue;

                if (collisionDetector->IsInterfering(newConf))
                        continue;

                if (collisionDetector->IsInterferingLinear(newConf, *nearConf))
                        continue;

                bFind = true;
                break;
        }
        
        if (!bFind)
                return ERROR_GET_COLLISION_FREE_FAILURE;

        nodeInGraph = g->AddConnection(nodeInGraph, newConf);
        (*nextConf) = g->GetConfiguration(nodeInGraph);
        return ERROR_OK;
}

int PL_MPEP::Establish_Self_Motion_Graph(dynamic_trees &tr, vertex extended)
{
        int result;
        Pose_Node *node = (Pose_Node *)tr.vertex_inf(extended);
        if (node->graph==NULL)
        {
                numSMGraph++;
                numFinalSMGraph++;

                node->graph = new SMGraph(node->conf);

                //for (int i=0; i<5; i++)
                //{
                //      Configuration *nextConf;
                //      result = Extend_Node_Self_Motion_Graph(extended, &nextConf);
                //      if (result != ERROR_OK)
                //      {
                //              return result;
                //      }
                //}
        }
        if (node->graph->Node_Num()<smgNodeNum)
        {
                Configuration *nextConf;

                if (usingJacobianExt)
                        result = Extend_Node_Self_Motion_Graph_Jacobian(extended, &nextConf);
                else
                        result = Extend_Node_Self_Motion_Graph(extended, &nextConf);
                if (result==ERROR_OK)
                {
                        numSMGNode++;
                        numFinalSMGNode++;
                        node->conf = *nextConf;
                }
                /*
                for (int retry=0; retry < exploreNum; retry++)
                {
                        result = Extend_Node_Self_Motion_Graph(extended, &nextConf);
                        if (result==ERROR_OK)
                        {
                                node->conf = *nextConf;
                                break;
                        }
                }*/
        }
        else
        {
                result = ERROR_SMGRAPH_FULL;
        }
        return result;
}

//------------------------------------------------------------------
//This function is use to check the connectivity from a new node
//      to the random tree growing up from the other direction;
//
//Input Parameter:
//      extendedVert: The new node ;
//                tr: The random tree;
//              vert: The arry storing the vertices in the tree;
//               dir: The growing relative direction between the node and the tree
//                     1: node is in the tree from START, tree grows from GOAL;
//                    -1: node is in the tree from GOAL, tree grows from START;
//
//Ouput Parameter:
//     connectedVert: The node connectable to extendedVert in the given random tree.
//
//Return Value:
//      ERROR_OK, if success
//      ERROR_GET_COLLISION_FREE_FAILURE,
//
//Notes I:
//      Here, we just consider those node which is connecteable to current graph.
//          those not connectable will be discarded
//
//Notes II:
//      The exploration will fail if:
//       - fail to get a closed configuration, or
//       - fail to get a collision free configuration
//       - the number of node achieve the limit
//------------------------------------------------------------------
int PL_MPEP::CheckConnectivity(dynamic_trees &tr, std::vector<vertex> &vert, vertex extendedVert, vertex &connectedVert, int dir)
{
        int i;
        Pose_Node *node;
        Pose_Node *extendedNode;
        extendedNode = (Pose_Node *)tree.vertex_inf(extendedVert);

        int index = extendedNode->poses + dir;
        bool bFind = false;

        for (i=0; i<vert.size(); i++)
        {
                node = (Pose_Node *)tr.vertex_inf(vert[i]);
                if (node->poses == index)
                {
                        if (!collisionDetector->IsInterferingLinear(node->conf, extendedNode->conf))
                        {
                                bFind = true;
                                connectedVert = vert[i];
                        }
                        else if (node->graph != NULL)
                        {
                                void *nodeInGraph = node->graph->FirstNode();
                                for (; nodeInGraph != NULL ; )
                                {
                                        Configuration *confInGraph = node->graph->GetConfiguration(nodeInGraph);
                                        if (!collisionDetector->IsInterferingLinear(node->conf, extendedNode->conf))
                                        {
                                                bFind = true;
                                                node->conf = *(confInGraph);
                                                connectedVert = vert[i];
                                                break;
                                        }
                                        nodeInGraph = node->graph->NextNode();
                                }
                        }
                        if (bFind)
                                break;
                }
        } //end for

        if (!bFind)
                return ERROR_GET_COLLISION_FREE_FAILURE;
        else
                return ERROR_OK;
}

//------------------------------------------------------------------
//This function is RRT-Connect Like Planner, with Self-Motion Graph enabled;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_SMGRAPH_FULL, if fail due to self motion graph for a node is full;
//      ERROR_RRT_EXTEND_FAILURE, if fail after a certain number of retry;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Two tree growing up from START and GOAL respectively, and make extension from two tree in turn.
//      After every EXTENTION, we greedily step to the tree growing up from the other direction.
//            in where fail to get further, propagate a Self-Motion-Graph.
//            in where succeed to get further, try to connect to the random tree growing up from the other direction.
//------------------------------------------------------------------
int PL_MPEP::Planner_SMG_RRT_Connect_Like()
{
        int poseIndexFromStart = _FIRST_POSE;
        int poseIndexFromGoal = _LAST_POSE;
        int retry = 0;
        int dir, result;
        bool walkinotherdir = false;
        bool connected = false;
        vertex extendedVert;
        vertex connectedVert = NULL;

        usingGoalTree = true;
        while ((!connected) && (retry<iterationNum))
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        //return result;
                {
                        retry ++;
                        continue;
                }

                int extend_retry = 0;

                do
                {
                        dir = 1;
                        if (RRT_Extend_Like(tree, vertices, poseIndexFromStart, extendedVert, dir)==ERROR_OK)
                        {
                                //----------------------------------------------------------------------
                                //  Walk from Randomtree growing up from START;
                                //----------------------------------------------------------------------
                                walkinotherdir = false;
                                while ((!walkinotherdir) && (!connected))
                                {
                                        result = Step(tree, vertices, poseIndexFromStart, poseIndexFromStart+1, extendedVert);
                                        if (result == ERROR_OK)
                                        {
                                                poseIndexFromStart = poseIndexFromStart + 1;
                                                /*
                                                if (poseIndexFromStart == _LAST_POSE)
                                                {
                                                        connected = true;
                                                        goalPoseVertex = extendedVert;
                                                        goalPoseVertex2 = NULL;
                                                }
                                                else
                                                */
                                                if(extend_retry > (extendNum*0.6))
                                                {
                                                        if (CheckConnectivity(goal_tree, goal_vertices, extendedVert, connectedVert, dir)==ERROR_OK)
                                                        {
                                                                connected = true;
                                                                goalPoseVertex = extendedVert;
                                                                goalPoseVertex2 = connectedVert;
                                                        }
                                                }
                                        }
                                        /*
                                        else
                                        {
                                                //----------------------------------------------------------------------
                                                //  Create or Explore the self-motion-graph for the leaf;
                                                //----------------------------------------------------------------------
                                                result = Establish_Self_Motion_Graph(tree, extendedVert);
                                                if (result != ERROR_OK)
                                                        walkinotherdir = true;
                                        }
                                        */
                                        walkinotherdir = true;
                                } //while ((!walkinother) && (!connected))
                        }

                        if (connected)
                                break;

                        dir = -1;
                        if (RRT_Extend_Like(goal_tree, goal_vertices, poseIndexFromGoal, extendedVert, dir)==ERROR_OK)
                        {
                                //----------------------------------------------------------------------
                                //  Walk from Randomtree growing up from GOAL;
                                //----------------------------------------------------------------------
                                walkinotherdir = false;
                                while ((!walkinotherdir) && (!connected))
                                {
                                        result = Step(goal_tree, goal_vertices, poseIndexFromGoal, poseIndexFromGoal-1, extendedVert, dir);
                                        if (result == ERROR_OK)
                                        {
                                                poseIndexFromGoal = poseIndexFromGoal - 1;
                                                /*
                                                if (poseIndexFromGoal == _FIRST_POSE)
                                                {
                                                        connected = true;
                                                        goalPoseVertex2 = extendedVert;
                                                        goalPoseVertex = NULL;
                                                }
                                                else
                                                */
                                                if(extend_retry > (extendNum*0.6))
                                                {
                                                        if (CheckConnectivity(tree, vertices, extendedVert, connectedVert, dir)==ERROR_OK)
                                                        {
                                                                connected = true;
                                                                goalPoseVertex = connectedVert;
                                                                goalPoseVertex2 = extendedVert;
                                                        }
                                                }
                                        }
                                        /*
                                        else
                                        {
                                                //----------------------------------------------------------------------
                                                //  Create or Explore the self-motion-graph for the leaf;
                                                //----------------------------------------------------------------------
                                                result = Establish_Self_Motion_Graph(goal_tree, extendedVert);
                                                if (result != ERROR_OK)
                                                        walkinotherdir = true;
                                        }
                                        */
                                        walkinotherdir = true;
                                } //while ((!walkinother) && (!connected))
                        }

                        extend_retry +=2;

                        if ( HasTimeLimitExpired() )
                                return ERROR_PLANNING_TIMEOUT;
                } while ((!connected) && (extend_retry<extendNum));

                retry ++;
        } //while ((!connected) && (retry<iterationNum))

        if (! connected)
                return ERROR_RRT_EXTEND_FAILURE;

        RetrievePathFromTree();
        return ERROR_OK;
}

//------------------------------------------------------------------
//This function is RRT-Greedy Like Planner, with Self-Motion Graph enabled;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_SMGRAPH_FULL, if fail due to self motion graph for a node is full;
//      ERROR_RRT_EXTEND_FAILURE, if fail after a certain number of retry;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Only one tree growing up from START.
//      After every EXTENTION, we greedily step to the goal.
//            in where fail to get further, propagate a Self-Motion-Graph.
//------------------------------------------------------------------
int PL_MPEP::Planner_SMG_RRT_Greedy_Like()
{
        int poseIndexFromStart = _FIRST_POSE;
        int poseIndexFromGoal = _LAST_POSE;
        int retry = 0;
        int dir, result;
        bool walkinotherdir = false;
        bool connected = false;
        vertex extendedVert;
        vertex connectedVert = NULL;

        LogMessage("SMGCount.Log", "Start SMG-RRT");
        
        numSMGraph = numSMGNode = 0;

        usingGoalTree = false;
        while ((!connected) && (retry<iterationNum))
        {
                numFinalSMGraph = numFinalSMGNode = 0;

                if ((result=InitializeTree())!=ERROR_OK)
                        return result;

                int extend_retry = 0;

                do
                {
                        dir = 1;
                        if (RRT_Extend_Like(tree, vertices, poseIndexFromStart, extendedVert, dir)==ERROR_OK)
                        {
                                //----------------------------------------------------------------------
                                //  Walk from Randomtree growing up from START;
                                //----------------------------------------------------------------------
                                walkinotherdir = false;
                                while ((!walkinotherdir) && (!connected))
                                {
                                        //Here is a loop; It tries to go one step further each iteration;
                                        //    once it fail, it propogate an SMG.
                                        result = Step(tree, vertices, poseIndexFromStart, poseIndexFromStart+1, extendedVert);
                                        if (result == ERROR_OK)
                                        {
                                                poseIndexFromStart = poseIndexFromStart + 1;
                                                if (poseIndexFromStart == _LAST_POSE)
                                                {
                                                        connected = true;
                                                        goalPoseVertex = extendedVert;
                                                        goalPoseVertex2 = NULL;
                                                }
                                        }
                                        else
                                        {
                                                //----------------------------------------------------------------------
                                                //  Create or Explore the self-motion-graph for the leaf;
                                                //----------------------------------------------------------------------
                                                result = Establish_Self_Motion_Graph(tree, extendedVert);
                                                if (result != ERROR_OK)
                                                        walkinotherdir = true;
                                        }
                                } //while ((!walkinother) && (!connected))
                        }

                        if (connected)
                                break;

                        extend_retry ++;

                        if ( HasTimeLimitExpired() )
                                return ERROR_PLANNING_TIMEOUT;
                } while ((!connected) && (extend_retry<extendNum));

                retry ++;
        } //while ((!connected) && (retry<iterationNum))

        //Log("SMGCount.Log", "%d\t%d\t%d\t%d", numSMGraph, numSMGNode, numFinalSMGraph, numFinalSMGNode);

        if (! connected)
                return ERROR_RRT_EXTEND_FAILURE;

        RetrievePathFromTree();
        return ERROR_OK;
}

//------------------------------------------------------------------
//This function is Greedy Planner, with Self-Motion Graph enabled;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_SMGRAPH_FULL, if fail due to self motion graph for a node is full;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//      ERROR_GET_COLLISION_FREE_FAILURE
//
//Description:
//      Pretty much same as Greedy Planner, the only difference is:
//            in where fail to get further, propagate a Self-Motion-Graph,
//            and then try to connect from the nodes in S-M-G to GOAL.
//------------------------------------------------------------------
int PL_MPEP::Planner_SMG_Greedy()
{
        double timeForSMG=0;
        double timePost=0;
        double timeForGreedy=0;

        int retry = 0;
        int result;
        int poseIndex = _FIRST_POSE;
        vertex extendedVert;
        Configuration conf;

        usingGoalTree = false;

        LogMessage("SMGCount.Log", "Start SMG-Greedy");
        numSMGraph = numSMGNode = 0;

        while (retry < iterationNum)
        {
                numFinalSMGraph = numFinalSMGNode = 0;
                if ((result=InitializeTree())!=ERROR_OK)
                        break;

                extendedVert = vertices[0];

                poseIndex = _FIRST_POSE;
                do
                {
                        timePost = GetTimeElapsedInSeconds();
                        result = Greedy_Step(poseIndex, _LAST_POSE, extendedVert);
                        timeForGreedy += (GetTimeElapsedInSeconds()-timePost);

                        //After return, poseIndex is where it fail and extendedVert is corresponding Vertex in tree.

                        if (result != ERROR_OK)
                        {
                                timePost = GetTimeElapsedInSeconds();
                                result = Establish_Self_Motion_Graph(tree, extendedVert);
                                timeForSMG += (GetTimeElapsedInSeconds()-timePost);
                                if (result != ERROR_OK)
                                        break;
                        }
                        else
                        {
                                break;
                        }

                        if ( HasTimeLimitExpired() )
                        {
                                result = ERROR_PLANNING_TIMEOUT;
                                break;
                        }
                } while (true);

                retry ++;
                if ((result == ERROR_PLANNING_TIMEOUT) || (result == ERROR_OK) )
                        break;
        }

        //Log("SMGCount.Log", "%d\t%d\t%d\t%d", numSMGraph, numSMGNode, numFinalSMGraph, numFinalSMGNode);
        //Log("SMGCount.Log", "Time used in SMG: %.4f \t Time used in Greedy: %.4f", timeForSMG, timeForGreedy);
        if (result==ERROR_OK)
        {
                goalPoseVertex = extendedVert;
                RetrievePathFromTree();
        }

        return result;
}

//------------------------------------------------------------------
//This function is Greedy Planner, with mechanism of back-tracking;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Pretty much same as Greedy Planner, the only difference is:
//            in where fail to get further, propagate a Self-Motion-Graph,
//            and then try to connect from the nodes in S-M-G to GOAL.
//------------------------------------------------------------------
int PL_MPEP::Planner_IMP_Greedy()
{
        int retry = 0;
        int result;
        int poseIndex = _FIRST_POSE;
        vertex extendedVert;
        Configuration conf;

        usingGoalTree = false;

        while (retry < iterationNum)
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        break;

                extendedVert = vertices[0];

                poseIndex = _FIRST_POSE;
                do
                {
                        result = Greedy_Step(poseIndex, _LAST_POSE, extendedVert);
                        if (result != ERROR_OK)
                        {
                                poseIndex --;
                                extendedVert = tree.parent(extendedVert);
                        }
                        else
                        {
                                break;
                        }
                        if ( HasTimeLimitExpired() )
                        {
                                result = ERROR_PLANNING_TIMEOUT;
                                break;
                        }
                } while (poseIndex >= 0);

                if (result != ERROR_OK)
                        retry ++;
                else
                        break;
        }

        if (result==ERROR_OK)
        {
                goalPoseVertex = extendedVert;
                RetrievePathFromTree();
        }

        return result;
}

//------------------------------------------------------------------
//This function is Greedy Planner;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Please refer to Oriolo's paper.
//------------------------------------------------------------------
int PL_MPEP::Planner_Greedy()
{
        int retry = 0;
        int result;
        vertex extendedVert;
        Configuration conf;

        usingGoalTree = false;

        while (retry < iterationNum)
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        break;

                extendedVert = vertices[0];
                result = Step(tree, vertices, _FIRST_POSE, _LAST_POSE, extendedVert);
                if (result != ERROR_OK)
                        retry ++;
                else
                        break;
                if ( HasTimeLimitExpired() )
                {
                        result = ERROR_PLANNING_TIMEOUT;
                        break;
                }
        }

        if (result==ERROR_OK)
        {
                goalPoseVertex = extendedVert;
                RetrievePathFromTree();
        }

        return result;
}

//------------------------------------------------------------------
//This function is RRT-Like Planner;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Please refer to Oriolo's paper.
//      Try to make one step further in every extension,
//            making the tree randomly grow up evenly in every direction;
//------------------------------------------------------------------
int PL_MPEP::Planner_RRT_Like()
{
        int poseIndex = _FIRST_POSE;
        int retry = 0;
        int result;
        vertex extendedVert;

        usingGoalTree = false;

        while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        return result;

                int extend_retry = 0;
                do
                {
                        RRT_Extend_Like(tree, vertices, poseIndex, extendedVert);
                        extend_retry ++;

                        if ( HasTimeLimitExpired() )
                                return ERROR_PLANNING_TIMEOUT;
                } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));

                retry ++;
        }

        if (poseIndex != _LAST_POSE)
                return ERROR_RRT_EXTEND_FAILURE;

        goalPoseVertex = extendedVert;
        RetrievePathFromTree();
        return ERROR_OK;
}

//------------------------------------------------------------------
//This function is RRT-Connect-Like Planner;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Please refer to Oriolo's paper.
//      Try to make one step further in every extension,
//            making the tree randomly grow up evenly in every direction;
//      After that, greedily step to the GOAL
//------------------------------------------------------------------
int PL_MPEP::Planner_RRT_Connect_Like()
{
        int poseIndex = 0;
        int retry = 0;
        int result;
        vertex extendedVert;

        usingGoalTree = false;

        while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        return result;

                int extend_retry = 0;
                do
                {
                        if (RRT_Extend_Like(tree, vertices, poseIndex, extendedVert)==ERROR_OK)
                        {
                                if (Step(tree, vertices, poseIndex, _LAST_POSE, extendedVert)==ERROR_OK)
                                        poseIndex = _LAST_POSE;
                        }
                        extend_retry ++;

                        if ( HasTimeLimitExpired() )
                                return ERROR_PLANNING_TIMEOUT;
                } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));

                retry ++;
        }

        if (poseIndex != _LAST_POSE)
                return ERROR_RRT_EXTEND_FAILURE;

        goalPoseVertex = extendedVert;
        RetrievePathFromTree();
        return ERROR_OK;
}

//------------------------------------------------------------------
//This function is RRT-Connect-Like Planner;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Please refer to Oriolo's paper.
//      Try to make one step further in every extension,
//            making the tree randomly grow up evenly in every direction;
//      After that, greedily try once more step.
//------------------------------------------------------------------
int PL_MPEP::Planner_RRT_Greedy_Like()
{
        int poseIndex = 0;
        int retry = 0;
        int result;
        vertex extendedVert;

        usingGoalTree = false;

        while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        return result;

                int extend_retry = 0;
                do
                {
                        if (RRT_Extend_Like(tree, vertices, poseIndex, extendedVert)==ERROR_OK)
                        {
                                if (poseIndex<_LAST_POSE)
                                {
                                        vertex closestVert = GetClosestPoseVert(tree, vertices);
                                        if (closestVert!=NULL)
                                        {
                                                Pose_Node *closestNode=(Pose_Node *)tree.vertex_inf(closestVert);
                                                if (Step(tree, vertices, closestNode->poses, closestNode->poses+1, extendedVert)==ERROR_OK)
                                                        poseIndex = closestNode->poses + 1;
                                        }
                                }
                        }
                        extend_retry ++;

                        if ( HasTimeLimitExpired() )
                                return ERROR_PLANNING_TIMEOUT;
                } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));

                retry ++;
        }

        if (poseIndex != _LAST_POSE)
                return ERROR_RRT_EXTEND_FAILURE;

        goalPoseVertex = extendedVert;
        RetrievePathFromTree();
        return ERROR_OK;
}

//------------------------------------------------------------------
//This function is RRT-Greedy-Connect-Like Planner;
//
//Input Parameter:
//      N/A;
//
//Ouput Parameter:
//      N/A.
//
//Return Value:
//      ERROR_OK, if success;
//      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
//
//Description:
//      Please refer to Oriolo's paper.
//      Try to make one step further in every extension,
//            making the tree randomly grow up evenly in every direction;
//      After that, greedily step toward the GOAL
//            if fail, then try once more step from the closest node to the GOAL.
//------------------------------------------------------------------
int PL_MPEP::Planner_RRT_Greedy_Connect()
{
        int poseIndex = 0;
        int retry = 0;
        int result;
        vertex extendedVert;

        usingGoalTree = false;

        while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
        {
                if ((result=InitializeTree())!=ERROR_OK)
                        return result;

                int extend_retry = 0;
                do
                {
                        if (RRT_Extend_Like(tree, vertices, poseIndex, extendedVert)==ERROR_OK)
                        {
                                if (Step(tree, vertices, poseIndex, _LAST_POSE, extendedVert)==ERROR_OK)
                                        poseIndex = _LAST_POSE;
                                if (poseIndex<_LAST_POSE)
                                {
                                        vertex closestVert = GetClosestPoseVert(tree, vertices);
                                        if (closestVert!=NULL)
                                        {
                                                Pose_Node *closestNode=(Pose_Node *)tree.vertex_inf(closestVert);
                                                if (Step(tree, vertices, closestNode->poses, closestNode->poses+1, closestVert)==ERROR_OK)
                                                {
                                                        extendedVert = closestVert;
                                                        poseIndex = closestNode->poses + 1;
                                                }
                                        }
                                }
                        }
                        extend_retry ++;

                        if ( HasTimeLimitExpired() )
                                return ERROR_PLANNING_TIMEOUT;
                } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));

                retry ++;
        }

        if (poseIndex != _LAST_POSE)
                return ERROR_RRT_EXTEND_FAILURE;

        goalPoseVertex = extendedVert;
        RetrievePathFromTree();
        return ERROR_OK;
}


//--------------------------------------------------------------
int PL_MPEP::RRT_Extend_Like(dynamic_trees &tr, std::vector<vertex> &vert, int &extPose, vertex &extended, int dir)
{
        vertex nearVert, newVert;
        Configuration randConf, activeConf;
        //Pose_Node *newNode;
        Pose_Node newNode;
        Pose_Node *nearPose;

        bool bFind = false;
        //for (int i=0; i<randomRetry; i++)
        //{
        GetRandomConfiguration(randConf);
        nearVert = GetNearestNodeInTree(tr, vert, randConf);
        nearPose = (Pose_Node *)tr.vertex_inf(nearVert);

        bFind = GetConfigurationByDirection(activeConf, nearPose->conf, randConf);
        if (bFind)
                bFind = redundant.GetConfigurationByActive(newNode.conf, activeConf, nearPose->conf,
                                                    poses[nearPose->poses+dir], distTolerance);
                //if (!redundant.GetConfigurationByActive(newNode.conf, activeConf, nearPose->conf,
                //                                        poses[nearPose->poses+dir], activeConf.DOF()*distTolerance))
        if (bFind)
        {
                bFind = false;
                if (!collisionDetector->IsInterfering(newNode.conf))
                {
                        if (!collisionDetector->IsInterferingLinear(newNode.conf, nearPose->conf))
                                bFind = true;
                }
        }

        if (!bFind)
        {
                return ERROR_GET_COLLISION_FREE_FAILURE;
        }
        else
        {
                extPose = newNode.poses = nearPose->poses + dir;
                newVert = ConnectNode(tr, vert, nearVert, newNode);
                extended = newVert;
                return ERROR_OK;
        }
}

int PL_MPEP::Step(dynamic_trees &tr, std::vector<vertex> &vert, int begin, int end, vertex &base, int dir)
{
        int i;
        int retry;
        vertex parent=base;
        Pose_Node *node=(Pose_Node *)tr.vertex_inf(base);
        Pose_Node newNode;
        Configuration conf;
        Configuration prevConf=node->conf;

        i = begin;
        while (i!=end)
        {
                retry = 0;
                while (retry < exploreNum)
                {
                        if (GetRandomConfiguration(conf, poses[i+dir], prevConf))
                        {
                                //==================*************** key
                                if (!collisionDetector->IsInterfering(conf))
                                {
                                        if (!collisionDetector->IsInterferingLinear(prevConf, conf))
                                        {
                                                newNode.conf = conf;
                                                newNode.poses = i+dir;
                                                newNode.graph = NULL;
                                                parent = ConnectNode(tr, vert, parent, newNode);
                                                prevConf = conf;
                                                base = parent;
                                                break;
                                        }
                                }
                        }

                        retry ++;
                }//End while
                if (retry == exploreNum)
                {
                        //LogMessage("mpep.log", "ERROR_GET_COLLISION_FREE_FAILURE");
                        return ERROR_GET_COLLISION_FREE_FAILURE;
                }

                i += dir;
        }//End while (i!=end)

        return ERROR_OK;
}

//--------------------------------------------------------------
//Get the node in the tree which is closest to given configuration.
//According Oriolo's paper, just the active joints is computed.
//While here, we acutally compute the whole configuration,
//  thinking there will be no much difference
vertex PL_MPEP::GetNearestNodeInTree(dynamic_trees &tr, std::vector<vertex> &vert, Configuration &conf)
{
        //tree.
        if (vert.size() == 0)
                return NULL;

        Pose_Node *node;
        double minDist, dist;
        int minPose=0;
        node = (Pose_Node *)tr.vertex_inf(vert[0]);
        minDist = DistanceInActiveJoints(node->conf, conf);

        for (int i=1; i<vert.size(); i++)
        {
                node = (Pose_Node *)tr.vertex_inf(vert[i]);
                dist = DistanceInActiveJoints(node->conf, conf);
                if (dist<minDist)
                {
                        minDist = dist;
                        minPose = i;
                }
        }

        return vert[minPose];
}

void *PL_MPEP::GetNearestNodeInGraph(SMGraph *graph, Configuration *conf)
{
        void *nodeInGraph=NULL;
        void *minNode=NULL;
        Configuration *confInGraph;

        double dist;
        double minDist = 360.0*conf->DOF();

        nodeInGraph = graph->FirstNode();
        confInGraph = graph->GetConfiguration(nodeInGraph);
        while(nodeInGraph != NULL)
        {
                dist = DistanceInActiveJoints(*confInGraph, *conf);
                if (dist<minDist)
                {
                        minDist = dist;
                        minNode = nodeInGraph;
                }
                nodeInGraph = graph->NextNode();
                if (nodeInGraph)
                        confInGraph = graph->GetConfiguration(nodeInGraph);
        }

        return minNode;
}

int PL_MPEP::InitializeTree()
{
        int i;
        bool bFind;
        Pose_Node firstNode;
        Pose_Node *newNode;
        vertex newVert;
        Semaphore semaphore( guid );

        ClearTree();

        //Create the Random tree growing up from the Start Pose.
        firstNode.poses = _FIRST_POSE;
        if (usingStartConfig)
        {
                firstNode.conf = GetStartConfig();
        }
        else
        {
                bFind = false;
                for (i=0; i<randomRetry; i++)
                {
                        if (!redundant.GetRandomConfiguration(firstNode.conf, poses[_FIRST_POSE]))
                                continue;
                        if (collisionDetector->IsInterfering(firstNode.conf))
                                continue;
                        bFind = true;
                        break;
                }
                if (!bFind)
                        return ERROR_START_CONFIG_INVALID;
        }

        //Create the root of dynamic tree
        newNode = new Pose_Node;
        newNode->conf = firstNode.conf;
        newNode->poses = firstNode.poses;
        newNode->graph = NULL;

        semaphore.Lock();
        newVert = tree.make(newNode);
        vertices.push_back(newVert);
        semaphore.Unlock();

        if (!usingGoalTree)
                return ERROR_OK;

        //Create the Random tree growing up from the Goal Pose.
        firstNode.poses = _LAST_POSE;

        if (usingGoalConfig)
        {
                firstNode.conf = GetGoalConfig();
        }
        else
        {
                bFind = false;
                for (i=0; i<randomRetry; i++)
                {
                        //if (!redundant.GetRandomConfiguration(firstNode.conf, poses[_LAST_POSE]))
                        //  continue;

                        while (!redundant.GetRandomConfiguration(firstNode.conf, poses[_LAST_POSE]));
                        //continue;
                        if (collisionDetector->IsInterfering(firstNode.conf))
                                continue;
                        bFind = true;
                        break;
                }
                if (!bFind)
                        return ERROR_GOAL_CONFIG_INVALID;
        }

        //Create the root of dynamic tree
        newNode = new Pose_Node;
        newNode->conf = firstNode.conf;
        newNode->poses = firstNode.poses;
        newNode->graph = NULL;

        semaphore.Lock();
        newVert = goal_tree.make(newNode);
        goal_vertices.push_back(newVert);
        semaphore.Unlock();

        return ERROR_OK;
}

void PL_MPEP::ClearTree()
{
        numTotalNode += vertices.size();

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

        int i;

        for (i=0; i<vertices.size(); i++)
        {
                Pose_Node *node = (Pose_Node*)tree.vertex_inf(vertices[i]);
                if (node->graph)
                        delete node->graph;
                delete node;
        }
        tree.clear();
        vertices.clear();

        for (i=0; i<goal_vertices.size(); i++)
        {
                Pose_Node *node = (Pose_Node*)goal_tree.vertex_inf(goal_vertices[i]);
                if (node->graph)
                        delete node->graph;
                delete node;
        }
        goal_tree.clear();
        goal_vertices.clear();

        goalPoseVertex = NULL;

        semaphore.Unlock();
}

vertex PL_MPEP::ConnectNode(dynamic_trees &tr, std::vector<vertex> &vert, vertex &parent, Pose_Node &child)
{
        Semaphore semaphore( guid );
        semaphore.Lock();

        Pose_Node *newNode = new Pose_Node;
        newNode->conf = child.conf;
        newNode->poses = child.poses;
        newNode->graph = NULL;

        vertex newVert = tr.make(newNode);
        vert.push_back(newVert);
        tr.link(newVert, parent, 1);

        semaphore.Unlock();

        return newVert;
}

void PL_MPEP::RetrievePathFromTree()
{
        vertex current;

        //ASSERT(goalPoseVertex != NULL);

        itinerary.clear();

        if (goalPoseVertex != NULL)
        {
                current = goalPoseVertex;

                itinerary.push_back(current);

                while ((current = tree.parent(current)) != NULL)
                {
                        itinerary.insert(itinerary.begin(), current);
                }
        }

        if ((usingGoalTree) && (goalPoseVertex2 != NULL))
        {
                current = goalPoseVertex2;
                while((current = goal_tree.parent(current)) != NULL)
                {
                        itinerary.push_back(current);
                }
        }

        numSMGraphInPath=0;
        numSMGNodeInPath=0;
        numTotalNode += vertices.size();

        for (int i=0; i<itinerary.size(); i++)
        {
                Pose_Node *node;
                node = (Pose_Node*)(tree.vertex_inf(itinerary[i]));
                if (node->graph == NULL)
                {
                        path.AppendPoint(node->conf);
                }
                else
                {
                        numSMGraphInPath ++;
                        node->graph->GetPath();
                        Configuration *conf = node->graph->FirstConfigInPath();
                        while (conf != NULL)
                        {
                                numSMGNodeInPath ++;
                                path.AppendPoint(*(conf));
                                conf = node->graph->NextConfigInPath();
                        }
                        numSMGNodeInPath = numSMGNodeInPath - 1; //Get rid of the root
                }
        }
        Log("SMGCount.Log", "%d\t%d\t%d\t%d\t%d\t%d\tTotalNodes:\t%d\t%d", 
                numSMGraph, numSMGNode, numFinalSMGraph, numFinalSMGNode, numSMGraphInPath, numSMGNodeInPath,
                numTotalNode, vertices.size());
}

vertex PL_MPEP::GetClosestPoseVert(dynamic_trees &tr, std::vector<vertex> &vert)
{
        if (vert.size() == 0)
                return NULL;

        Pose_Node *node;
        double maxPose, poseIndex;
        int maxIndex=0;

        node = (Pose_Node *)tr.vertex_inf(vert[0]);
        maxPose = node->poses;

        for (int i=1; i<vert.size(); i++)
        {
                node = (Pose_Node *)tr.vertex_inf(vert[i]);
                poseIndex = node->poses;
                if (poseIndex>maxPose)
                {
                        maxPose = poseIndex;
                        maxIndex = i;
                }
        }

        return vert[maxIndex];
}

//This function computes the distance according the two configurations
double PL_MPEP::Distance(Configuration &conf1, Configuration &conf2)
{
        VectorN jointDiff;
        jointDiff = collisionDetector->JointDisplacement(conf1, conf2);

        double distance = 0;
        for( int i = 0; i < jointDiff.Length(); i++ )
        {
                distance += Sqr(jointDiff[i] );
        }

        return sqrt(distance);
}
//This function computes the distance according the active joints in these two configurations
double PL_MPEP::DistanceInActiveJoints(Configuration &conf1, Configuration &conf2)
{
        VectorN jointDiff;
        jointDiff = collisionDetector->JointDisplacement(conf1, conf2);

        double distance = 0;
        for( int i = 0; i < jointDiff.Length()-passiveNum; i++ )
        {
                distance += Sqr(jointDiff[i] );
        }

        return sqrt(distance);
}

//--------------------------------------------------------------
bool PL_MPEP::GetConfigurationByDirection(Configuration &conf, Configuration &base, Configuration &direct)
{
        int i;
        Configuration activeConf, activeDirect;
        int dof = collisionDetector->DOF();
        int activeJointNum = dof-passiveNum;
        activeConf.SetLength(dof);
        activeDirect.SetLength(dof);
        conf.SetLength(activeJointNum);
        for (i=0; i<activeJointNum; i++)
        {
                activeConf[i] = base[i];
                activeDirect[i] = direct[i];
        }
        double dist = DistanceInActiveJoints(activeConf, activeDirect);
        double landa;
        if (dist==0)
                return false;
        else
                landa = (Rad2Deg(distTolerance)*sqrt(activeJointNum))/dist;

        for (i=0; i<activeJointNum; i++)
        {
                conf[i] = (1-landa)*activeConf[i]+landa*activeDirect[i];
        }

        return true;
}

bool PL_MPEP::GetRandomConfiguration(Configuration &conf)
{
        Configuration randConf;
        if (redundant.GetRandomConfiguration(randConf))
        {
                conf = randConf;
                return true;
        }
        return false;
}

bool PL_MPEP::GetRandomConfiguration(Configuration &conf, Frame &frame)
{
        Configuration randConf;
        if (redundant.GetRandomConfiguration(randConf, frame))
        {
                conf = randConf;
                return true;
        }

        return false;
}

bool PL_MPEP::GetRandomConfiguration(Configuration &conf, Frame &frame, Configuration &bias)
{
        if (distTolerance <= 0)
                return GetRandomConfiguration(conf, frame);

        Configuration randConf;
        if (redundant.GetRandomConfiguration(randConf, bias, frame, distTolerance))
        {
                conf = randConf;
                return true;
        }

        return false;
}

//--------------------------------------------------------------
void PL_MPEP::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        PL_Boolean_Output::SetCollisionDetector(collisionDetector);

        //null is a valid collision detector to set this to
        if( this->collisionDetector != NULL )
        {
                FrameManager *frameManager=this->collisionDetector->GetFrameManager();
                redundant.SetFrameManager(frameManager);
                redundant.SetCollisionDetector(this->collisionDetector);

                int nDOF = collisionDetector->DOF();

                redundant.SetActiveBaseFrame(0);
                redundant.SetActiveFirstJoint(0);
                redundant.SetActiveLastJoint(nDOF-3);

                redundant.SetPassiveBaseFrame(0);
                redundant.SetPassiveFirstJoint(nDOF-2);
                redundant.SetPassiveLastJoint(nDOF-1);

                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 (m_pJacobian != NULL)
                {
                        delete m_pJacobian;
                        m_pJacobian = new CJacobian(*m_pRobot);
                }
        }
}

void PL_MPEP::SetIterationNum(unsigned int num)
{
        iterationNum = num;
}

void PL_MPEP::SetExploreNum(unsigned int num)
{
        exploreNum = num;
}

void PL_MPEP::SetExtendNum(unsigned int num)
{
        extendNum = num;
}

void PL_MPEP::SetPassiveNum(unsigned int num)
{
        passiveNum = num;
        int nDOF = collisionDetector->DOF();

        redundant.SetActiveBaseFrame(0);
        redundant.SetActiveFirstJoint(0);
        redundant.SetActiveLastJoint(nDOF-num-1);

        redundant.SetPassiveBaseFrame(0);
        redundant.SetPassiveFirstJoint(nDOF-num);
        redundant.SetPassiveLastJoint(nDOF-1);
}

void PL_MPEP::SetAlgorithm(unsigned int alg)
{
        algorithm = alg;
}

void PL_MPEP::SetTrajectory(std::vector<Frame> &traj)
{
        poses.clear();
        for (int i=0; i<traj.size(); i++)
        {
                poses.push_back(traj[i]);
        }
}

void PL_MPEP::SetUsingJacobianExt(bool use)
{
        usingJacobianExt = use;
}

void PL_MPEP::SetTolerance(double dist)
{
        distTolerance = dist;
}

void PL_MPEP::SetUsingStartConfig(bool use)
{
        usingStartConfig = use;
}

void PL_MPEP::SetUsingGoalConfig(bool use)
{
        usingGoalConfig = use;
}

void PL_MPEP::SetRandomRetry(unsigned int retry)
{
        randomRetry = retry;
}

void PL_MPEP::SetSMGNodeNum(unsigned num)
{
        smgNodeNum = num;
}

void PL_MPEP::Display_Error_Message(int result)
{
        char *szErrorMsg;
        switch(result)
        {
        case ERROR_GET_RANDOM_CONF_FAILURE:
                szErrorMsg = "Can not get random configuration!";
                break;
        case ERROR_GET_COLLISION_FREE_FAILURE:
                szErrorMsg = "Can not get collisiion free configuration!";
                break;
        case ERROR_RRT_EXTEND_FAILURE:
                szErrorMsg = "Can not get good extension!";
                break;
        case ERROR_START_CONFIG_INVALID:
                szErrorMsg = "The start configuration is not valid!";
                break;
        case ERROR_GOAL_CONFIG_INVALID:
                szErrorMsg = "The goal configuration is not valid!";
                break;
        case ERROR_SMGRAPH_FULL:
                szErrorMsg = "The Self Motion Graph is FULL!";
                break;
        case ERROR_PLANNING_TIMEOUT:
                szErrorMsg = "Time out!";
                break;
        default:
                szErrorMsg = "Unknown Message";
                break;
        }
        //MessageBox(NULL, szErrorMsg, "Motion Planning along End-effect Path", MB_OK);
}

bool PL_MPEP::DrawExplicit () const
{
        // IMPROVE:  Add Open GL code to inser the graph here.
        double Xcor, Ycor, Zcor;        // temp coordinate variables

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

        // Setup GL Environment
        //glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
        glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
        glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
        BOOL lightingState = glIsEnabled( GL_LIGHTING );
        glDisable( GL_LIGHTING );

        // Create Shading Effects that will allow edges to have two end colours.
        //  IMPROVE:  Does this change the shading for others.  If so, is there a way
        //                    to check for current shading mode and save it.
        glShadeModel( GL_SMOOTH );

        // The depth function is set to its default of GL_LESS.  This means that any object closer to the viewport
        // will have its colour displayed for that pixel.  This means closer objects will be drawn on top of further
        // objects.  Any objects that are in the same plane, will have the colour of the object drawn first.

        // Draw all NODES first, as these are to be seen on top of any edges.
        glPointSize(5.0f);      // Set size of points associated with the nodes.

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

                //Draw Self-motion graph
                glBegin(GL_LINES);
                for(i = 1; i<vertices.size(); i++)
                {
                        //here node is data type for graph.
                        vertex vert;
                        Pose_Node *smgNode;
                        edge e;

                        vert = vertices[i];
                        vertex parentVert = ((dynamic_trees &)tree).parent(vert);
                        if (parentVert==NULL) //Don't draw standalone vertices;
                                continue;

                        smgNode = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(vert);
                        SMGraph *graph = smgNode->graph;
                        if (graph==NULL)
                                continue;
                        GRAPH<Configuration *, double> *smgGraph = graph->GetSMGraph();
                        forall_edges(e, *smgGraph)
                        {
                                Configuration *conf1, *conf2;
                                conf1 = conf2 = NULL;
                                graph->GetConfig(e, &conf1, &conf2);

                                ASSERT((conf1!=NULL) && (conf2!=NULL));

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

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

                        }
                }
                glEnd();

                //const dynamic_trees *ptrTree = &tree;
                glBegin(GL_POINTS);
                for(i = 0; i<vertices.size(); i++)
                {
                        vertex vert;
                        Pose_Node *node;
                        vert = vertices[i];
                        vertex parentVert = ((dynamic_trees &)tree).parent(vert);
                        if (parentVert==NULL) //Don't draw standalone vertices;
                                continue;
                        node = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(vert);
                        Configuration conf = node->conf;

                        Xcor = conf[0];
                        Ycor = conf[1];
                        Zcor = conf[2];

                        if ((i==0) || (i==(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<vertices.size(); i++)
                {
                        vertex vert, parentVert;
                        Pose_Node *node, *parentNode;

                        vert = vertices[i];
                        parentVert = ((dynamic_trees &)tree).parent(vert);
                        if (parentVert==NULL) 
                                continue;
                        node = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(vert);
                        parentNode = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(parentVert);
                        Configuration conf = node->conf;
                        Configuration parentConf = parentNode->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();
        }

        //draw goal tree if there is
        if (usingGoalTree)
        {
                if (goal_vertices.size() )
                {
                        int i;

                        //const dynamic_trees *ptrTree = &tree;
                        glBegin(GL_POINTS);
                        for(i = 0; i<goal_vertices.size(); i++)
                        {
                                vertex vert;
                                Pose_Node *node;
                                vert = goal_vertices[i];
                                node = (Pose_Node *)((dynamic_trees &)goal_tree).vertex_inf(vert);
                                Configuration conf = node->conf;

                                Xcor = conf[0];
                                Ycor = conf[1];
                                Zcor = conf[2];

                                if ((i==0) || (i==(goal_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<goal_vertices.size(); i++)
                        {
                                vertex vert, parentVert;
                                Pose_Node *node, *parentNode;

                                vert = goal_vertices[i];
                                parentVert = ((dynamic_trees &)goal_tree).parent(vert);
                                node = (Pose_Node *)((dynamic_trees &)goal_tree).vertex_inf(vert);
                                parentNode = (Pose_Node *)((dynamic_trees &)goal_tree).vertex_inf(parentVert);
                                Configuration conf = node->conf;
                                Configuration parentConf = parentNode->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();
                }
        }//end if (usingGoalTree)

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

        // Unlock Semaphore
        semaphore.Unlock();

        return true;
}

---