planners/fortest/PL_ForTest.cpp

Go to the documentation of this file.

#include "synchronization/semaphore.h"
#include "Planners/fortest/PL_ForTest.h"
#include "planners/ik_mpep/SMGraph.h"
#include "math/matrixmxn.h"
#include "math/vector4.h"
#include <assert.h>
#include <gl\gl.h>

#define DEF_STEP_AHEAD          2
#define MAX_SMG_EXTEND_RETRY    10
#define DEF_SMG_STEPSIZE        5
#define DEF_ERROR_TOLERANCE     0.01

PL_ForTest::PL_ForTest()
{
        m_pJacobian = NULL;
        m_nStepAhead = DEF_STEP_AHEAD;
        m_bUseJacobianSMGExploration = false;
        m_bUseBacktracking = true;
        m_pLocalPlanner = new Jacobian_TrajPlanner;
        m_nSMGStepSize = DEF_SMG_STEPSIZE;
        m_bUseObstacleAvoidLP = true;
        m_nSMGNodes = m_nSMGraphs = 0;
        smgNodeNum = 50;
}

PL_ForTest::~PL_ForTest()
{
        if (m_pJacobian != NULL)
                delete m_pJacobian;
        if (m_pLocalPlanner != NULL)
        {
                delete m_pLocalPlanner;
        }
}

bool PL_ForTest::DrawExplicit () const
{
        return PL_MPEP::DrawExplicit();
}

void PL_ForTest::SetCollisionDetector(CD_BasicStyle* collisionDetector)
{
        PL_MPEP::SetCollisionDetector(collisionDetector);
        m_pLocalPlanner->SetCollisionDetector(collisionDetector);

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

//does all the planning
bool PL_ForTest::Plan ()
{
        //Set up the trajectory, for test purpose
        //SetTrajectory();
        
        //MessageBox(NULL, "This planner is for test purpose!", "Test Planner", MB_OK);
        if (m_pJacobian == NULL)
                m_pJacobian = new CJacobian(*m_pRobot);

        Configuration startConfig = GetStartConfig();
        m_pJacobian->SetConfiguration(startConfig);
        m_pJacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);

        //Set seed of random, it's supposed be done by upper layer, actually
        srand( (unsigned)time( NULL ) );

        path.Clear();

        int result=ERROR_OK;

        //Always use Start Configuration
        usingStartConfig = true;
        usingGoalConfig = false;
        if (collisionDetector->IsInterfering(startConfig))
        {
                return false;
        }

        StartTimer();

        if ((result=InitializeTree())!=ERROR_OK)
        {
                return false;
        }

        int nStepAhead = m_nStepAhead;
        if (m_nStepAhead >= poses.size())
                nStepAhead = poses.size()-1;

        PA_Points foundPath;
        PA_Points bufferPath;
        Configuration start = startConfig;
        int nStartIndex = 0;
        int nEndIndex = nStartIndex + nStepAhead;
        int nCurrentIndex = 0;
        vertex extendedVert = vertices[0];

        while (nEndIndex < poses.size())
        {
                bool bFound;
                if (m_bUseObstacleAvoidLP)
                        bFound = FindLocalPath2(start, nStartIndex, nEndIndex, foundPath);
                else
                        bFound = FindLocalPath(start, nStartIndex, nEndIndex, foundPath);
                if (bFound)
                {
                        for (int i=0; i<foundPath.Size(); i++)
                                bufferPath.AppendPoint(foundPath[i]);

                        Pose_Node newNode;
                        newNode.conf = bufferPath[0];
                        bufferPath.DeleteFirstPoint();
                        newNode.poses = nCurrentIndex + 1;
                        newNode.graph = NULL;
                        extendedVert = ConnectNode(tree, vertices, extendedVert, newNode); //extendedVert is the parent
                        nStartIndex = nEndIndex;
                        nEndIndex++;
                        nCurrentIndex++;
                        start = foundPath[foundPath.Size()-1];
                        foundPath.Clear();
                        //Log("PlannerForTest.log", "Append configuration for pose: %d", nCurrentIndex);
                }
                else
                {
                        nStartIndex = nCurrentIndex;
                        nEndIndex = nStartIndex + nStepAhead;
                        bufferPath.Clear();

                        //Log("PlannerForTest.log", "Exploring SMG for pose: %d", nCurrentIndex);
                        //Propagate an SMG here.
                        do {
                                result = Establish_Self_Motion_Graph(tree, extendedVert);
                        if (HasTimeLimitExpired())
                                        break;
                        } while ((result!=ERROR_OK) && (result!=ERROR_SMGRAPH_FULL));

                        if (result==ERROR_OK)
                        {
                                Pose_Node *node=(Pose_Node *)tree.vertex_inf(extendedVert);
                                start = node->conf;
                        }
                }

                while (result==ERROR_SMGRAPH_FULL)
                {
                        vertex backtrackVertex;
                        backtrackVertex = tree.parent(extendedVert);
                        
                        //Remove All node vertices current vertex to the backtracking vertex
                        do
                        {
                                vertex prevVertex = tree.parent(extendedVert);
                                RemoveNode(extendedVert);
                                extendedVert = prevVertex;
                        } while(extendedVert != backtrackVertex );
                        
                        Pose_Node *node=(Pose_Node *)tree.vertex_inf(extendedVert);
                        nCurrentIndex = node->poses;
                        nStartIndex = nCurrentIndex;
                        nEndIndex = nStartIndex + nStepAhead;

                        //Log("PlannerForTest.log", "Back track to pose: %d", nCurrentIndex);
                        do {
                                result = Establish_Self_Motion_Graph(tree, extendedVert);
                                if (HasTimeLimitExpired())
                                        break;
                        } while ((result!=ERROR_OK) && (result!=ERROR_SMGRAPH_FULL));

                        if (result==ERROR_OK)
                        {
                                Pose_Node *node=(Pose_Node *)tree.vertex_inf(extendedVert);
                                start = node->conf;
                        }
                }

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

        if (result == ERROR_OK)
        {
                for (int i=0; i<bufferPath.Size(); i++)
                {
                        Pose_Node newNode;
                        newNode.conf = bufferPath[i];
                        newNode.poses = nCurrentIndex + 1;
                        nCurrentIndex ++;
                        newNode.graph = NULL;
                        extendedVert = ConnectNode(tree, vertices, extendedVert, newNode); //extendedVert is the parent         
                }
        }
        bufferPath.Clear();

        //Return the path anyway.
        goalPoseVertex = extendedVert;
        RetrievePathFromTree();

        LogStatistics();
        if (result == ERROR_OK)
                return true;
        else
                return false;
}


int PL_ForTest::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);
                m_nSMGraphs ++;
        }

        //The condition (node->poses > 0) is used such that not backtracking is allowed
        //    at the starting pose.
        if ((m_bUseBacktracking) && (node->graph->Node_Num()>smgNodeNum) && (node->poses > 0))
        {
                result = ERROR_SMGRAPH_FULL;
        }
        else//if (node->graph->Node_Num()<smgNodeNum)
        {
                Configuration *nextConf;
                if (m_bUseJacobianSMGExploration)
                        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;
                        m_nSMGNodes ++;
                }
        }

        return result;
}


int PL_ForTest::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;
}

bool PL_ForTest::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_ForTest::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 *= (m_nSMGStepSize/ 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;
}

bool PL_ForTest::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;
}

VectorN PL_ForTest::ComputeDeltaEndEff(Frame &fromPose, Frame &toPose)
{
        VectorN returnMe;

        //=======Here, no orientation is considered!!!!!
        returnMe.SetLength(3);

        int index = 0;
        returnMe[index++] = toPose(0, 3) - fromPose(0, 3);
        returnMe[index++] = toPose(1, 3) - fromPose(1, 3);
        returnMe[index++] = toPose(2, 3) - fromPose(2, 3);

        return returnMe;        
}


//Note:
//  1. CALL m_pJacobian->SetInterestPoint(), before calling this function.
//  2. The start configuration is not store in the localpath.
//  3. This function is made to be called frequently.
bool PL_ForTest::FindLocalPath(Configuration &start, int nStart, int nEnd, PA_Points &localPath)
{
        Matrixmxn mJEnd, mJEndInv;
        bool bValid = true;
        VectorN deltaEndEff;
        Configuration deltaConf;
        Configuration nextConf = start;

        for(int i=nStart; i<nEnd; i++)
        {
                m_pJacobian->SetConfiguration(nextConf);
                m_pJacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
                mJEnd = *(m_pJacobian->GetMatrix());
                mJEnd.Inverse(mJEndInv);
                //To avoid drifting...
                Frame currentPose = GetToolFrame(nextConf);
                //deltaEndEff = ComputeDeltaEndEff(poses[i], poses[i+1]);
                deltaEndEff = ComputeDeltaEndEff(currentPose, poses[i+1]);
                deltaConf = mJEndInv * deltaEndEff;
                deltaConf *= Rad2Deg(1);

                //LogVector("PlannerForTest.log", deltaEndEff, "End-effector increment:");
                //LogMessage("PlannerForTest.log", "Joint increment:");
                //LogConfiguration("PlannerForTest.log", deltaConf);

                Configuration newConf;
                newConf = nextConf + deltaConf;
                if (!IsJointWithinLimits(newConf))
                {
                        localPath.Clear();
                        bValid = false;
                        break;
                }
                AdjustConfiguration(newConf, i+1);
                if (collisionDetector->IsInterferingLinear(newConf, nextConf))
                {
                        localPath.Clear();
                        bValid = false;
                        break;
                }
                else
                {
                        nextConf = newConf;
                        localPath.AppendPoint(nextConf);
                }
        }

        return bValid;
}


bool PL_ForTest::FindLocalPath2(Configuration &start, int nStart, int nEnd, PA_Points &localPath)
{
        std::vector<Frame> piece;
        for (int i=nStart; i<=nEnd; i++)
                piece.push_back(poses[i]);

        m_pLocalPlanner->SetStartConfig(start);
        m_pLocalPlanner->SetTrajectory(piece);
        m_pLocalPlanner->SetUsingOrientation(false);
        m_pLocalPlanner->SetUseGoalConfig(false);
        if (m_pLocalPlanner->Plan())
        {
                PA_Points *foundPath = (PA_Points *)m_pLocalPlanner->GetPath();
                for (int k=1; k<foundPath->Size(); k++)
                        localPath.AppendPoint((*foundPath)[k]);
                return true;
        }
        return false;
}

void PL_ForTest::RemoveNode(vertex failVert)
{
        //Here, assume failVertex is a leaf node in the tree.
        assert(failVert != NULL);

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

        tree.cut(failVert);
        Pose_Node *node=(Pose_Node *)tree.vertex_inf(failVert);
        if (node->graph != NULL)
        {
                node->graph->Clear();
                delete node->graph;
                node->graph = NULL;
        }

        semaphore.Unlock();
}

void PL_ForTest::ResetStatistics()
{
        m_nSMGNodes = 0;
        m_nSMGraphs = 0;
}

void PL_ForTest::LogStatistics()
{
        int nNodes = vertices.size();
        int nPathLen = path.Size();
        Log("ForTest.log", "SMGNodes: %d, SMGraphs: %d, TreeNodes: %d, PathLen: %d", 
                        m_nSMGNodes, m_nSMGraphs, nNodes, nPathLen);
}

---