planners/atace/PL_ATACE.cpp

Go to the documentation of this file.

//
//Contents: Class PL_ATACE
// 
//PL_ATACE: Alternate Physical-space And C-space Exploration
//      is a path planning algorithm for PPGEEC, Path Planning problem
//      with General End-Effector Constraints.
//
//This is the topic of Zhenwang Yao's Master thesis, for more information
//please refer to that. The basic idea of APACE is: Instead of finding a path
//in C-space directly, APACE explore the physical space(or, the task space) 
//first for feasible end-effector poses and paths; then track these paths in 
//C-space with trajectory tracking planners.
//
//Revision History:
//       Date     Author    Description
//      ---------------------------------------------------------
//     Jun-2004:  Z.Yao     First version implemented: PL_JPRM
//     Oct-2004:  Z.Yao     Lazy version implemented
//                          Rename the planner to PL_ATACE.
//     Nov-2004:  Z.Yao     Comments added
//  
//Notes:
//There are serveral things may affect the performance:
//   1. m_stepSize/m_timeInterval: extension step size and sampling time;
//   2. m_localPlanner: local planner, any of trajectory tracking planners;
//   3. m_metrics: which metrics used
//   4. m_useLazy: whether Lazy strategy or not
//   5. "fake.wrl": In current version[Nov-2004], the anticipatory collision detector,
//      which detects collision along the end-effector path, loads "fake.wrl" as 
//      the artificial end-effector. Its size and shape may affect the performance A-LOT.
//

#include <synchronization/semaphore.h>
#include <assert.h>
#include "math/Vector4.h"
#include "opengl/glos.h"
#include <gl/gl.h>
#include "LocalPlanner.h"
#include "PL_ATACE.h"
#include "CollisionDetectors/CD_Vcollide.h"
#include "Universe/universe.h"
#include <opengl/gl_sphere.h>
#include <opengl/gl_group.h>
#include <geometry/vrml_20_reader.h>
#include <serializable/MPK_Dir.h>

#define NUM_MAX_ITERATION   10
#define LEN_ENDEFF_SIZE     0.5
#define LEN_RESOLUTION      (0.1)
#define LEN_TOL_ERROR       (1e-5)
#define LEN_ERROR_RADIUS    0.005

#if 1
        #include "math\Matrixmxn.h"
        #define LogMatrix(a, b, c) 
        #define LogVector(a, b, c)
        #define LogMessage(a, b)
#endif
//****************************************************************************
//              Construction and Deconstruction Functions
//****************************************************************************
PL_ATACE::PL_ATACE()
{
        m_robot = NULL;
        m_toolFrame = -1;
        m_rootVert = NULL;
        m_trajCollisionDetector = NULL;
        m_trajUniverse = NULL;

        IsPoseSatified = NULL;
        AdjustPoseToSatisfy = NULL;
        GetVelocity = NULL;
        GetDirectedVelocity = NULL;

        m_isGoalConfSet = m_isGoalPoseSet = false;
        m_isStartConfSet = m_isStartPoseSet = false;
        
        //The following parameters could be set by users
        m_useLazy = false;
        m_useGoalConf = m_useGoalPose = false;
        m_useStartConf = m_useStartPose = false;
        m_useOrientation = false;
        m_useOrientationAlways = false;
        m_drawCSpace = false;
        m_metrics = FLAG_P_METRICS;
        m_stepSize = 1;                 //This will be changed in SetCollisionDetector()
        m_timeInterval = 0.2;   //This will be changed in SetCollisionDetector()
        m_nLocalPlanner = LOCAL_JACOBIAN;

        m_localPlanner = new Jacobian_TrajPlanner;//RRT_TrajPlanner;//TrajPlanner;
}

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

        if (m_localPlanner != NULL)
        {
                delete m_localPlanner;
        }

        DeleteTrajectoryCD();
}

bool PL_ATACE::Plan()
{
        srand( (unsigned)time( NULL ) );
        path.Clear();
        StartTimer();  // Start the timer

        Node rootNode;
        rootNode.pose = m_startPose;
        rootNode.conf = GetStartConfig();
        CreateTree(rootNode);

        m_goalVert = NULL;
        int result = ERR_FAIL;
        bool pathFound = false;
        int repeatNum = 0;
        Vertex fromVert = m_rootVert;
        while ((!pathFound) && (result != ERR_TIMEOUT))
        {
                //Try to greedily extend from latest node to the goal
                if (fromVert != NULL)
                {
                        Pose extendToGoal = GetPoseFromTree(fromVert);
                        if (!IsInterfering(extendToGoal, m_goalPose))
                        {
                                result = ExtendToGoal(fromVert, m_goalPose, m_goalVert);
                        }
                        if (m_goalVert != NULL)
                        {
                                pathFound = true;
                                break;
                        }
                }

                Node dirNode;
                Node toNode;
                Node fromNode;
                Path localPath;
                EEPath endeffPath;

                //Pick up a node that is closest to a randomly-chosen landmark duo
                GenerateRandomNode(dirNode);
                fromVert = FindClosestInTree(dirNode);
                fromNode.conf = GetConfigurationFromTree(fromVert);
                fromNode.pose = GetPoseFromTree(fromVert);

                endeffPath.clear();
                localPath.Clear();
                //Extend the tree in direction of dirNode
                result = ExtendWithConstraint(fromNode, dirNode.pose, toNode, localPath, endeffPath, false);
                if (result == ERR_SUCCESS) 
                {
                        fromVert = AddNodeInTree(fromVert, toNode, dirNode.pose, localPath, endeffPath);
                }
                else
                {
                        fromVert = NULL;
                }
                endeffPath.clear();
                localPath.Clear();

                if ( HasTimeLimitExpired() )
                {
                        break;
                }
        } //End of While(...)

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

//------------------------------------------------------------------
//ExtendWithConstraint(...)
//
//This function is used to extend the random tree in a given direction.
//
//Input Parameter:
//      fromNode : (config., pose) of the node from which to extend the tree
//      dirPose  : (config., pose) of the direction
//      greedy   : use greedy extension or not.
//
//Ouput Parameter:
//      toNode   : (config., pose) of the extened node
//      edgeTraj : the end-effector path in physical space
//      edgePath : the path in C-space to track $edgeTraj$
//
//Return Value:
//      ERR_SUCCESS, if success; 
//      ERR_FAIL   , otherwise;
//
//Notes:
//   1. The caller is responsible for connecting the extended node into the radnom tree.
//   2. edgeTraj/edgePath is allocated by the caller. If extension succeeds, 
//      edgeTraj/edgePath will be emptied by the caller, after the node is connected
//      to the tree; if extension fails, then edgeTraj/edgePath return as empty queues.
//
//------------------------------------------------------------------
int PL_ATACE::ExtendWithConstraint(Node &fromNode, Pose &dirPose, Node &toNode, Path &edgePath, EEPath &edgeTraj, bool greedy)
{
        Pose startPose = fromNode.pose;

        double pathLen = 0;
        bool trajExtracted = true;

        //Extracted a end-effector path as an edge in physical space.
        edgeTraj.push_back(startPose);
        //Note: m_stepSize>0, extraction ends when pathLen exceeds m_stepSize;
        //      m_stepSize<0, extraction ends only when the goal is achieved.
        while((m_stepSize<0) || (pathLen < m_stepSize) ) 
        {
                Pose toPose;
                GetNextPose(startPose, dirPose, toPose, greedy);
                double dist = Distance(startPose, toPose);
                if (dist < m_timeInterval*(1e-5))
                {
                        if (pathLen < 1.e-10)
                        {
                                trajExtracted = false;
                        }
                        break;
                }
                if (!IsInterfering(startPose, toPose))
                {
                        pathLen += dist;
                        edgeTraj.push_back(toPose);
                        startPose = toPose;
                }
                else
                {
                        trajExtracted = false;
                        break;
                }
                if ( HasTimeLimitExpired() )
                {
                        trajExtracted = false;
                        break;
                }
        } //END OF WHILE

        bool pathExtracted = true;
        if (trajExtracted)
        {
                if (m_useLazy)   //using LAZY strategy, trach the path later
                {
                        int lastIndex = edgeTraj.size()-1;
                        toNode.pose = edgeTraj[lastIndex];
                }
                else //Not using LAZY strategy, have to track the E.E. path right away.
                {
                        m_localPlanner->SetStartConfig(fromNode.conf);
                        m_localPlanner->SetTrajectory(edgeTraj);
                        m_localPlanner->SetUsingOrientation((greedy && m_useOrientation) || (m_useOrientation && m_useOrientationAlways));
                        m_localPlanner->SetUseGoalConfig(false);
                        if (m_useGoalConf && greedy)
                        {
                                m_localPlanner->SetGoalConfig(GetGoalConfig());
                                m_localPlanner->SetUseGoalConfig(true);
                        }
                        if (m_localPlanner->Plan())
                        {
                                PA_Points *localPath;
                                localPath = (PA_Points *)m_localPlanner->GetPath();
                                AppendPath(edgePath, localPath);
                                int lastIndex = edgePath.Size() - 1;
                                toNode.conf = edgePath.GetPoint(lastIndex);
                                toNode.pose = GetToolFrame(toNode.conf);
                        }
                        else
                        {
                                pathExtracted = false;
                //if (greedy)
                {
                    Matrixmxn logMatrix;
                    int lastIndex = edgeTraj.size()-1;
                    logMatrix = edgeTraj[0];
                    char szMessage[100];
                    sprintf(szMessage, "The length of the path: %d", lastIndex+1);
                    LogMessage("greedy.log", szMessage);
                    LogMatrix("greedy.log", logMatrix, "First");
                    logMatrix = edgeTraj[1];
                    LogMatrix("greedy.log", logMatrix, "Second");
                    logMatrix = edgeTraj[lastIndex-1];
                    LogMatrix("greedy.log", logMatrix, "Second Last");
                    logMatrix = edgeTraj[lastIndex];
                    LogMatrix("greedy.log", logMatrix, "Last");
                }
                        }
                }
        }

        if ((trajExtracted == false) || (pathExtracted == false))
        {
                return ERR_FAIL;
        }

        return ERR_SUCCESS;
}

//------------------------------------------------------------------
//LazyTrackEEPath
//
//This function is used to track an entire end-effector path in the physical space.
//
//Input Parameter: N/A.
//Ouput Parameter: N/A.
//
//Return Value:
//      ERR_SUCCESS, if success; 
//      ERR_FAIL   , otherwise;
//
//Notes:
//   1. The caller is responsible for connecting the extended node into the radnom tree.
//   2. edgeTraj/edgePath is allocated by the caller. If extension succeeds, 
//      edgeTraj/edgePath will be emptied by the caller, after the node is connected
//      to the tree; if extension fails, then edgeTraj/edgePath return as empty queues.
//   3. The prerequisite of calling this function is (m_goalVert!=NULL), which means
//      a candidate end-effector path joint the start and the goal has been found.
//
//------------------------------------------------------------------
int PL_ATACE::LazyTrackEEPath()
{
        assert( m_goalVert!= NULL);
        bool pathExtracted = true;
        bool greedy = false;
        int failureIndex = 0;
        std::vector<vertex> trajectory;
        Vertex currVert = m_goalVert;
        //The following while-loop extract the end-effector path into a queue:
        //      starting from the goal-pose, ending at the strat-pose
        while (currVert != NULL)
        {
                trajectory.push_back(currVert);
                currVert = m_trajTree.parent(currVert);
        }//end of while

        int nNumOfVert = trajectory.size();
        for (int i=nNumOfVert-1; i>=1; i--)
        {
                Vertex trackingVert = trajectory[i-1];
                Vertex parent = trajectory[i];

                VertexInfo *info;
                info = (VertexInfo *)m_trajTree.vertex_inf(trackingVert);

                if (info->collisionChecked)
                        continue;

                if  (i==1)   //Deal with the last one, greedy to the goal
                        greedy = true;

                Configuration conf = GetConfigurationFromTree(parent);
                m_localPlanner->SetStartConfig(conf);
                m_localPlanner->SetTrajectory(info->eeEdge);
                m_localPlanner->SetUsingOrientation((greedy && m_useOrientation) || (m_useOrientation && m_useOrientationAlways));
                m_localPlanner->SetUseGoalConfig(false);

                if (m_useGoalConf && greedy)
                {
                        m_localPlanner->SetGoalConfig(GetGoalConfig());
                        m_localPlanner->SetUseGoalConfig(true);
                }

                if (m_localPlanner->Plan())
                {
                        PA_Points *localPath;
                        localPath = (PA_Points *)m_localPlanner->GetPath();
                        int lastIndex = localPath->Size() - 1;
                        info->conf = localPath->GetPoint(lastIndex);

                        if ( m_useGoalConf && greedy )
                        {
                                Configuration goalConf = GetGoalConfig();
                                if ( collisionDetector->IsInterferingLinear(info->conf, goalConf) == false )
                                {
                                        pathExtracted = false;
                                        failureIndex = i;
                                        break;
                                }
                                else
                                {
                                        InterpolatePath(*localPath, info->conf, goalConf);
                                }
                        }

                        info->edge.Clear();
                        AppendPath(info->edge, localPath);
                        info->pose = GetToolFrame(info->conf);   //Maybe I should check whether this configuration get to the right pose
                        info->collisionChecked = true;
                        //lastIndex = edgeTraj.size()-1;
                        //toNode.pose = edgeTraj[lastIndex];
                }
                else
                {
                        pathExtracted = false;
                        failureIndex = i;
                        break;
                }
        }

        if (pathExtracted==false)
        {
                TrimTreeFrom( trajectory[failureIndex-1] );
                m_goalVert = NULL;
                return ERR_FAIL;
        }

        return ERR_SUCCESS;
}

//------------------------------------------------------------------
//ExtendToGoal
//
//This function is used to track an entire end-effector path in the physical space.
//
//Input Parameter:
//      fromVert: The newly-extended node in the random tree
//      dirPose : The expected extending direction
// 
//Ouput Parameter:
//      endVert : A new node will be added into the tree, endVert is the pointer
//
//Return Value:
//      ERR_SUCCESS, if success; 
//      ERR_FAIL   , otherwise;
//
//Notes:
//   1. In this function, ExtendWithConstraint() will be called with $greedy$ strategy, 
//      which means the extension ends only after getting to the $dirPose$. 
//   2. It also deals with both CONFIG-TO-POSE and CONFIG-TO-CONFIG problem.
//   3. CONFIG-TO-CONFIG problem: in this type of problem, we simple treat it as
//      a CONFIG-TO-POSE problem. Only when the last step, ExtendToGoal, we bring
//      the goal configuration into consideration.
//      a. With constraints. It needs to be done by the local planner.
//      b. Without constraints. It can be handled by a simple linear configurations connection. 
//      -. In current implmentation [Nov-12-2004], we simply ignore the (a) part, due to the 
//         limitation of our current local planners. So basicaly, we just deal with the basic
//         motion planning problem in (b) part.
//
//------------------------------------------------------------------
int PL_ATACE::ExtendToGoal(Vertex &fromVert, Pose &dirPose, Vertex &endVert)
{
        Vertex currVert;
        Node currNode;

        endVert = NULL;
        currVert = fromVert;
        currNode.pose = GetPoseFromTree(fromVert);
        currNode.conf = GetConfigurationFromTree(fromVert);

        Node toNode;
        Path localPath;
        EEPath endeffPath;
        double oldStepSize;
        int result = ERR_SUCCESS;

        oldStepSize = m_stepSize;
        m_stepSize = -1.0; //Trigger the greedy extension at ExtendWithConstraint()

        localPath.Clear();
        result = ExtendWithConstraint(currNode, dirPose, toNode, localPath, endeffPath, true);
        if ( (result == ERR_SUCCESS ) && (m_useGoalConf) && (!m_useLazy))
        {
                //Deal with the basic motion planning problem
                Configuration goalConf = GetGoalConfig();
                if ( collisionDetector->IsInterferingLinear(currNode.conf, goalConf) == false )
                {
                        result = ERR_FAIL;
                }
                else
                {
                        InterpolatePath(localPath, toNode.conf, goalConf);
                }
        }
        if (result == ERR_SUCCESS)
        {
                Vertex newVert = AddNodeInTree(currVert, toNode, dirPose, localPath, endeffPath);
                localPath.Clear();
                endeffPath.clear();
                endVert = newVert;
        }

        if ((m_useLazy) && (result==ERR_SUCCESS))
        {
                result = LazyTrackEEPath();
        }

        m_stepSize = oldStepSize;

        return result;
}

//****************************************************************************
//              Functions for metrics
//****************************************************************************
//Using physical-space metric.
double PL_ATACE::Distance(const Pose &pos1, const Pose &pos2)
{
        Vector4 diff = pos1.GetTranslationVector() - pos2.GetTranslationVector();
        double dist = (diff[0]*diff[0])+(diff[1]*diff[1])+(diff[2]*diff[2]);
        return sqrt(dist);
}

//Using C-space metric
double PL_ATACE::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);
}

//****************************************************************************
//              All functions related to Trajectory Tree
//****************************************************************************
//Create a random tree
bool PL_ATACE::CreateTree(Node &rootNode)
{
        if (m_rootVert != NULL)
        {
                ClearTree();
        }

        VertexInfo *newNode = new VertexInfo;
        newNode->flag = FLAG_NEW;
        newNode->pose = rootNode.pose;
        newNode->conf = rootNode.conf;

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

        return true;
}

//Destroy a random tree
void PL_ATACE::ClearTree()
{
        Semaphore semaphore( guid );
        semaphore.Lock();
        for (int i=0; i<m_vertices.size(); i++)
        {
                VertexInfo *info;
                info = (VertexInfo *)m_trajTree.vertex_inf(m_vertices[i]);
                if (info->flag != FLAG_OBSOLETE)
                {
                        info->edge.Clear();
                        info->eeEdge.clear();
                }
                delete info;
        }

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

//------------------------------------------------------------------
//TrimTreeFrom
//
//This function is used to delete a whole branch of the random tree.
//
//Input Parameter:
//      failVert: The branch to be deleted
// 
//Ouput Parameter: 
//      N/A.
//
//Return Value:
//      N/A.
//
//Notes: 
//This function will be used in Lazy mode of APACE. We explore the end-effector paths
//first in the task space and save it as edges in the tree. When we fail to track some of
//the edge, we need to delete the entire tree branch after this point. For example, 
//after exploratin in the task space, we get the following tree:
//        ---N1---N2---N3---N4
//                |     |
//                N6   N5
//                      |
//                     N7
//when we fail to track edge(N1,N2), then the branch after N1 should be deleted.
//This can be done by calling TrimTreeFrom($N2$)
//
//We use LEDA dynamic_tree to store our random tree, It would be expensive to physically 
//delete the nodes/vertices. So what we did is to delete the corresponding edges and disable the node.
// 
//------------------------------------------------------------------
void PL_ATACE::TrimTreeFrom(Vertex failVert)
{
        assert(failVert != NULL);

        Semaphore semaphore( guid );
        semaphore.Lock();
        Vertex currVert = m_goalVert;

        std::vector<Vertex> deletingVert;
        deletingVert.push_back(failVert);

        int testStartFrom = 0;
        int currentSize;
        bool bFound = true;
        while (bFound == true)
        {
                bFound = false;
                currentSize = deletingVert.size();
                for (int i=0; i<m_vertices.size(); i++)
                {
                        Vertex testingVert = m_vertices[i];
                        Vertex testingParent = m_trajTree.parent(testingVert);

                        for (int j=testStartFrom; j<currentSize; j++)
                        {
                                if (testingParent == deletingVert[j])
                                {
                                        deletingVert.push_back(testingVert);
                                        bFound = true;
                                }
                        }
                }
                testStartFrom = currentSize;
        }
        //After the above while-loop, taking the example above, TrimTreeFrom(N2).
        //the sequence in $deletingVert$ will be: N2-N3-N6-N4-N5-N7
        //Maybe this is not the best way to delete a branch, aparently,
        //in the worst scenario, it cost $O(n^2)$, but this is the best I can come up with
        //using the LEDA data structure: dynamic_tree.
        //
        //Then, in the following for-loop, $deleteingVert$ will be traversed backwords,
        //and deleted one after another.

        currentSize = deletingVert.size()-1;
        for (int i=currentSize; i>=0; i--)
        {
                currVert = deletingVert[i];
                VertexInfo *info;
                info = (VertexInfo *)m_trajTree.vertex_inf(currVert);
                info->edge.Clear();
                info->eeEdge.clear();
                //delete info;
                info->flag = FLAG_OBSOLETE;  //All vertices is store in a std:vector<vertex>, 
                                             //It could be expensive to delete a node in the middle,
                                             //So what I did is to disable the node by setting this flag.
                m_trajTree.cut(currVert);
        }//end of for
        semaphore.Unlock();
}


Vertex PL_ATACE::AddNodeInTree(Vertex parentVertex, Node &childNode, Pose &dirPose, Path &localPath, EEPath &endeffPath)
{
        VertexInfo *newNode = new VertexInfo;
        newNode->flag = FLAG_NEW;
        newNode->pose = childNode.pose;
        newNode->conf = childNode.conf;
        newNode->dirPose = dirPose;
        if (m_useLazy)
        {
                newNode->collisionChecked = false;
                CopyEEPath(newNode->eeEdge, endeffPath);
        }
        else
        {
                newNode->collisionChecked = true;
                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_ATACE::FindClosestInTree(Pose &pose)
{
        if (m_vertices.size() == 0)
                return NULL;

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

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

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

        return m_vertices[minIndex];
}

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

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

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

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

        return m_vertices[minIndex];
}

Vertex PL_ATACE::FindClosestInTree(Node node)
{
        if (m_metrics == FLAG_C_METRICS)
                return FindClosestInTree(node.conf);
        else if (m_metrics == FLAG_P_METRICS)
                return FindClosestInTree(node.pose);
        else
        {
                if (rand() % 2)
                        return FindClosestInTree(node.conf);
                else
                        return FindClosestInTree(node.pose);
        }
}

Configuration& PL_ATACE::GetConfigurationFromTree(Vertex vert)
{
        VertexInfo *info;
        info = (VertexInfo *)m_trajTree.vertex_inf(vert);
        return info->conf;
}

Pose& PL_ATACE::GetPoseFromTree(Vertex vert)
{
        VertexInfo *info;
        info = (VertexInfo *)m_trajTree.vertex_inf(vert);
        return info->pose;
}

Path& PL_ATACE::GetPathFromTree(Vertex vert)
{
        VertexInfo *info;
        info = (VertexInfo *)m_trajTree.vertex_inf(vert);
        return info->edge;
}

//****************************************************************************
//             Functions about constraints
//****************************************************************************
//------------------------------------------------------------------
//GetNextPose
//
//This function is used to get next pose when extracting an end-effector path.
//
//Input Parameter:
//      prePose : Current end-effector pose.
//      dirPose : Stepping direction.
//      greedy  : Using greedy or step with a fixed size.
// 
//Ouput Parameter: 
//      postPose: The very next end-effector pose.
//
//Return Value:
//      N/A.
//
//Notes: 
//   1. Some call-back functions set by users will be called here, including
//      - IsPoseSatified(...)
//      - AdjustPoseToSatisfy(...)
//      - GetDirectedVelocity(...)
//   2. GetDirectedVelocity(...) returns the feasible linear and angular velocity
//      according to user constraints. For the comments about parameters of 
//      GetDirectedVelocity() please refer to UI-level, dialog for planner APACE.
//       
//------------------------------------------------------------------
void PL_ATACE::GetNextPose(Pose &prePose, Pose &dirPose, Pose &postPose, bool greedy)
{
        VectorN velocity;
        Frame projection;

        postPose = prePose;
        if ( ! IsPoseSatified(postPose) )
        {
                AdjustPoseToSatisfy(postPose);
                //Need to update to position
        }

        GetDirectedVelocity(postPose, dirPose, velocity, projection, greedy, m_timeInterval);

        //Translation, Velocity assumed normalized already
        Vector4 linearVel;
        linearVel[0] = velocity[0];
        linearVel[1] = velocity[1];
        linearVel[2] = velocity[2];
    //Extension won't exceed $projection$
        Vector4 offset = linearVel * m_timeInterval;
        Vector4 offset2 = projection.GetTranslationVector() - prePose.GetTranslationVector();
        if (offset.Magnitude() > offset2.Magnitude())
                offset = offset2;
        postPose.Translate(offset);

        //Roatation, only apply when using orientation
        //Rotate, this is the cases where you need to consider the orientation
        //Please note that although linear velocity returned by GetDirectedVelocity()
        //  has been normalized, angular velocity hasn't. In another world, the returned 
        //  angular velocity has information about axis and the total rotating angle.
        //  We need to compute how much angle we need to roate within $m_timeInterval$.
        if ((m_useOrientation && m_useOrientationAlways) || (greedy && m_useOrientation))
        {
                //Get the equivalent axis and rotated angle
                double angle;
                Vector4 angularVel;
                angularVel[0] = velocity[3];
                angularVel[1] = velocity[4];
                angularVel[2] = velocity[5];
                double magnitude = angularVel.Magnitude();
                if ( !IsZero(magnitude) )
                {
                        angularVel = angularVel/magnitude;
                        //Now angularVel is the equivalent axis of rotation, required to be unitary.
                }
                else
                {
                        angularVel[0] = angularVel[1] = 0;
                        angularVel[2] = 1;
                }
                angle = magnitude*m_timeInterval;

                //double distance, ratio;
                //distance = Distance(prePose, dirPose);
                //if ( IsZero(distance) )
                //{
                //      ratio = 0;
                //}
                //else
                //{
                //      ratio = Distance(prePose, postPose)/distance;
                //}
                //angle = angle * ratio;

                Frame rotated;
                rotated.Rotate2(angularVel, Rad2Deg(angle));

                offset = postPose.GetTranslationVector();
                postPose = rotated * postPose;
                postPose.SetTranslationVector(offset);
        //Matrixmxn logMatrix=postPose;
        //LogMatrix("adjust.log", logMatrix, "Next Pose:");
        }
}

void PL_ATACE::SetPoseChecker(ProcIsStaified proc)
{
        IsPoseSatified = proc;
}

void PL_ATACE::SetPoseAdjuster(ProcAdjustToSatisfy proc)
{
        AdjustPoseToSatisfy = proc;
}

void PL_ATACE::SetVelocityChecker(ProcGetVelocity proc)
{
        GetVelocity = proc;
}

void PL_ATACE::SetDirectedVelocityChecker(ProcGetDirectedVelocity proc)
{
        GetDirectedVelocity = proc;
}

//****************************************************************************
//             Collision Detection
//****************************************************************************
//------------------------------------------------------------------
//CreateTrajectoryCD
//
//This function is used to create anticipatory collision detector in APACE.
//
//Input Parameter:
//      collisionDetector: default collisionDetector.
// 
//Ouput Parameter: 
//      N/A.
//
//Return Value:
//      N/A.
//
//Note:
//      An anticipatory collision detector will be created by
//      using current obstacle and replacing the robot manipulator
//      with a rigid body, "fake.wrl".
//
//------------------------------------------------------------------
void PL_ATACE::CreateTrajectoryCD(CD_BasicStyle* collisionDetector)
{
        m_trajUniverse = new Universe;
        std::vector<Entity *> entities = collisionDetector->GetAllElements();

        //Add all obstacles into the virtual universe.
        for( int i = 0; i < entities.size(); i++ )
        {
                Entity* addme = entities[ i ];
                if( dynamic_cast<RobotBase*>( addme ))
                {
                        continue;
                }
                if (addme->BaseFrame() == 0)
                {
                        m_trajUniverse->AddEntity(addme);
                }
        }

        //Note::::
        //    The geomtry of "fake.wrl", especially its size, may significant affect the performance,
        //Improvement expected:
        //    To adaptively load different rigid body for different application,
        //       it's better to create this body dynamically, using "GL_Mesh" or things like that....
        VRML_20_Reader reader;
        const char *workingPath = GetMPKWorkingDirectory();
        reader.SetRootPath(workingPath);
        reader.SetFilename( "vrml models\\fake.wrl" );
        reader.Read();
        const ObjectBase* theObject = reader.GetTheObject();
        assert( theObject != NULL );
        if( theObject != NULL )
        {
                ObjectBase* fakeEnd = dynamic_cast< ObjectBase* >( theObject->Clone() );
                assert( fakeEnd != NULL );
                fakeEnd->SetBaseFrame( 1 );
                m_trajUniverse->AddEntity( fakeEnd );
                delete fakeEnd;
        }

        if( dynamic_cast<CD_Vcollide*>(collisionDetector) )
        {
                CD_Vcollide *detector = new CD_Vcollide(*m_trajUniverse);
                detector->PermActivateFrames(1, 0);
                detector->ActivateFrames(1, 0);
                m_trajCollisionDetector = detector;
        }
        else
        {
                assert(false);
        }
}

void PL_ATACE::DeleteTrajectoryCD()
{
        if (m_trajCollisionDetector)
                delete m_trajCollisionDetector;
        if (m_trajUniverse)
                delete m_trajUniverse;
}

//------------------------------------------------------------------
//IsInterfering
//
//This function is used to check collision between two end-effector poses.
//
//Input Parameter:
//      pose1, pose2: Two end-effector poses.
// 
//Ouput Parameter: 
//      N/A.
//
//Return Value:
//      TRUE,  colliding in between.
//      FALSE, collision free along the path.
//
//------------------------------------------------------------------
bool PL_ATACE::IsInterfering (const Pose& pose1, const Pose &pose2)
{
        double length = Distance(pose1, pose2);
        int sampleNum = (int)(length/LEN_RESOLUTION) + 1;
        Vector4 pos1 = pose1.GetTranslationVector();
        Vector4 pos2 = pose2.GetTranslationVector();
        for (int i=0; i<sampleNum; i++)
        {
                Pose tran;
                Vector4 pos;
                pos = pos1*(double(sampleNum-i)/double(sampleNum)) + pos2*(double(i)/double(sampleNum));
                tran.SetTranslationVector(pos);
                if (((CD_Vcollide *)m_trajCollisionDetector)->IsInterfering(tran))
                        return true;
        }
        
        return false;
}

//****************************************************************************
//             Function to Get Tool Frame: End-effector Frame.
//****************************************************************************
Frame PL_ATACE::GetToolFrame( const Configuration& config ) const
{
        Frame toolFrame = Matrix4x4();

        if ( collisionDetector != NULL )
        {
                FrameManager* frameManager = collisionDetector->GetFrameManager();
                collisionDetector->SetConfiguration( config );
                //toolFrame = frameManager->GetWorldFrame( m_toolFrame );
                if (m_toolFrame != -1)
                        toolFrame = *(frameManager->GetFrameRef(m_toolFrame));
                toolFrame = frameManager->GetTransformRelative(collisionDetector->DOF(), 0) * toolFrame;
        }
        return toolFrame;
}

//****************************************************************************
//             Functions to generate random node/configuration.
//****************************************************************************
void PL_ATACE::GenerateRandomNode(Node &randNode)
{
        randNode.conf = GenerateRandomConfig();
        randNode.pose = GetToolFrame(randNode.conf);
}

Configuration PL_ATACE::GenerateRandomConfig ()
{
        int dof = collisionDetector->DOF() ;

        Configuration returnme ;
        returnme.SetLength( dof ) ;

        for( int i = 0; i < dof; i++ )
        {
                double max = collisionDetector->JointMax( i ) ;
                double min = collisionDetector->JointMin( i ) ;
                double random = 2*( double( rand() ) / RAND_MAX ) - 1; //this is a random number between [-1..1]
                double jointvalue = ( max - min ) * random;

                // Create a uniform distribution by having the random number go over the limits have wrapping the C-space;
                if ( jointvalue < min )
                {
                        jointvalue += (max - min);
                }
                else if ( jointvalue > max )
                {
                        jointvalue -= (max - min);
                }

                returnme[ i ] = jointvalue;
        }

        return returnme ;
}

//****************************************************************************
//             Operation on Path in configuration space.
//****************************************************************************
void PL_ATACE::RetrievePath(Vertex &goalVert)
{
        Vertex currVert = goalVert;
        while (currVert != NULL)
        {
                InsertPath(path, GetPathFromTree(currVert));
                currVert = m_trajTree.parent(currVert);
        }//end of while
}

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

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

void PL_ATACE::CopyPath(Path &target, Path &source)
{
        target = source;
}

void PL_ATACE::CopyEEPath(EEPath &target, EEPath &source)
{
        int nLen = source.size();
        if ( target.size() != 0 )
        {
                target.clear();
        }
        for( int i = 0; i < nLen; i++ )
        {
                target.push_back( source[ i ] ) ;
        }
}

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

//------------------------------------------------------------------
//InterpolatePath
//
//This function is used to interpolate a path between two configuration.
//
//Input Parameter:
//      start, end: Two configurations.
// 
//Ouput Parameter: 
//      target: The interpolated path.
//
//Return Value:
//      N/A.
//
//Note:
//      The reason we need this function is to improve the visual effect.
//      By tracking the end-effector path which is normally sampled into a 
//      a sequence of poses, the joint path we get normally is a sequence of 
//      configurations and consecutive configurations are pretty close. However,
//      for the basic motion planning problem, we do the last step to goal configuration
//      by simply connecting to the goal with a linear connection. To make the 
//      path has smoothier replay in UI, we also sample this linear connection.
//      This is what we do in InterpolatePath().
//
//------------------------------------------------------------------
void PL_ATACE::InterpolatePath(Path &target, Configuration &start, Configuration &end)
{
        double dDist = Distance(start, end);
        dDist = Deg2Rad(dDist);
        int nSample = dDist/m_timeInterval + 1;
        for (int i = 0; i<=nSample; i++)
        {
                Configuration conf = end * ((double)i/nSample) + start * ((double)(nSample-i)/nSample);
                target.AppendPoint(conf);
        }
}

//****************************************************************************
//              Parameter Setting Functions
//****************************************************************************

void PL_ATACE::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
        PL_Boolean_Output::SetCollisionDetector(collisionDetector);
        m_localPlanner->SetCollisionDetector(collisionDetector);

        if( this->collisionDetector != NULL )
        {
                //Get the Robot information from CD.
                if( dynamic_cast<R_OpenChain*>(collisionDetector->GetRobot(0))
                  )
                {
                        m_robot = (R_OpenChain *)collisionDetector->GetRobot(0);
                        m_toolFrame = m_robot->GetToolFrame(0);
                }
                else
                {
                        m_robot = NULL;
                        m_toolFrame = 0;
                }

                //Create virtual CD for trajectory.
                DeleteTrajectoryCD();
                CreateTrajectoryCD(collisionDetector);
        }

        //Compute m_stepSize/m_timeInterval adaptively:
        m_stepSize = 0;
        for (int i=0; i<m_robot->DOF(); i++)
        {
                DH_Link *currLink = (DH_Link *)m_robot->GetLink(i);
                Vector4 endPos = currLink->GetFrame().GetTranslationVector();
                m_stepSize += endPos.Magnitude(); 
        }
        m_stepSize = (double)((int)(m_stepSize/10));
        m_timeInterval = m_stepSize/10;
}

void PL_ATACE::SetLocalPlanner(int localPlanner)
{
        if (m_nLocalPlanner != localPlanner)
        {
                if (m_localPlanner)
                        delete m_localPlanner;

                if (localPlanner == LOCAL_RRT)
                        m_localPlanner = new RRT_TrajPlanner;
                else
                        m_localPlanner = new Jacobian_TrajPlanner;

                m_nLocalPlanner = localPlanner;
                m_localPlanner->SetCollisionDetector(this->collisionDetector);
        }
}

void PL_ATACE::SetStartConfig( const Configuration& config )
{
        PL_Boolean_Output::SetStartConfig( config );
        if (this->m_useStartConf)
        {
                m_isStartConfSet = true;
        }
}

void PL_ATACE::SetGoalConfig ( const Configuration& config )
{
        PL_Boolean_Output::SetGoalConfig( config );
        if (this->m_useGoalConf)
        {
                m_isGoalConfSet = true;
                m_goalPose = GetToolFrame( config );
        }
}

void PL_ATACE::SetStartPose(const Pose &start)
{
        m_startPose = start;
        m_isStartPoseSet = true;
}

void PL_ATACE::SetGoalPose(const Pose &goal)
{
        m_goalPose = goal;
        m_isGoalPoseSet = true;
}

//------------------------------------------------------------------
//SetProblemType
//
//This function is used to set a planning task.
//
//Input Parameter:
//      type: problem type.
// 
//Ouput Parameter: 
//      N/A.
//
//Return Value:
//      true/false.
//
//Note:
//      It might be confusing setting a task. Setting a task includes:
//      - Set the type of problem: 
//          CONFIG-TO-CONFIG/CONFIG-TO-POSE/POSE-TO-CONFIG/POSE-TO-POSE;
//         Actually, current version[Nov-12-2004] don't support the last two types.
//      - Set the start: Set start configuration/pose
//      - Set the goal: Set goal configuration/pose
//
//------------------------------------------------------------------
void PL_ATACE::SetProblemType(unsigned int type)
{
        m_useGoalPose = ((type & TYPE_GOAL_POSE)!=0);
        m_useGoalConf = ((type & TYPE_GOAL_CONF)!=0);
        m_useStartPose = ((type & TYPE_START_POSE)!=0);
        m_useStartConf = ((type & TYPE_START_CONF)!=0);
        
        assert(m_useStartConf==true);
}

//------------------------------------------------------------------
//IsReady
//
//This function is used to check whether a planning task is properly set.
//
//Input Parameter:
//      N/A.
// 
//Ouput Parameter: 
//      N/A.
//
//Return Value:
//      true/false.
//
//Note:
//      Start/Goal should be set and match the type of the problem.
//
//------------------------------------------------------------------
bool PL_ATACE::IsReady()
{
        if (IsPoseSatified == NULL)
                return false;
        if (AdjustPoseToSatisfy == NULL)
                return false;
        if (GetVelocity == NULL)
                return false;
        if (GetDirectedVelocity == NULL)
                return false;   
        if (m_localPlanner == NULL)
                return false;
        if ((m_isStartConfSet == false) && (m_isStartPoseSet == false))
                return false;
        if ((m_isGoalConfSet == false) && (m_isGoalPoseSet == false))
                return false;

        if ((m_useStartConf == false) && (m_useStartPose == false))
                return false;
        if ((m_useGoalConf == false) && (m_useGoalPose == false))
                return false;

        if ((m_useStartConf == true) && (m_isStartConfSet == false))
                return false;
        if ((m_useGoalConf == true) && (m_isGoalConfSet == false))
                return false;
        if ((m_useStartPose == true) && (m_isStartPoseSet == false))
                return false;
        if ((m_useGoalPose == true) && (m_isGoalPoseSet == false))
                return false;

        return true;
}

//****************************************************************************
//              Draw the tree
//****************************************************************************
bool PL_ATACE::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;
                //Drawing NODEs
                glBegin(GL_POINTS);
                for(i = 0; i<m_vertices.size(); i++)
                {
                        Vertex vert;
                        VertexInfo *info;
                        vert = m_vertices[i];
                        info = (VertexInfo *)((dynamic_trees &)m_trajTree).vertex_inf(vert);
                        if (info->flag == FLAG_OBSOLETE)
                                continue;

                        if (!m_drawCSpace)
                        {
                                Vector4 pos = info->pose.GetTranslationVector();

                                Xcor = pos[0];
                                Ycor = pos[1];
                                Zcor = pos[2];
                        }
                        else
                        {
                                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();

                //Drawing EDGEs
                glBegin(GL_LINES);
                for(i = 1; i<m_vertices.size(); i++)
                {
                        Vertex vert, parentVert;
                        VertexInfo *info, *parentInfo;

                        vert = m_vertices[i];
                        parentVert = ((dynamic_trees &)m_trajTree).parent(vert);
                        if (parentVert==NULL)
                                continue;

                        info = (VertexInfo *)((dynamic_trees &)m_trajTree).vertex_inf(vert);
                        parentInfo = (VertexInfo *)((dynamic_trees &)m_trajTree).vertex_inf(parentVert);

                        if (!m_drawCSpace)
                        {

                                Vector4 pos = info->pose.GetTranslationVector();
                                Vector4 parentPos = parentInfo->pose.GetTranslationVector();

                                Xcor = pos[0];
                                Ycor = pos[1];
                                Zcor = pos[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 = parentPos[0];
                                Ycor = parentPos[1];
                                Zcor = parentPos[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

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

---