planners/ik_mpep/PL_MPEP.cpp

Go to the documentation of this file.
00001 
00003 //
00004 //Contents: Class PL_MPEP
00005 // 
00006 //PL_MPEP: Planner for Motion Planning along given End-effector Path.
00007 //This algorithm is based on the following paper, For more information
00008 //about original consideration and algorithm, please refer to that.
00009 //
00010 //        M.Ottavi, G.Oriolo and M.Vendittelli. Probabilistic motion planning
00011 //        for redundant robots along given end-effector paths. 
00012 //        IEEE Intl. Conf. on Intelligent Robots and Systems, 2002.
00013 //
00014 //Extending from this paper, some improved algorithm is proposed as well.
00015 //
00016 //For original paper, the following algorithms are implemented:
00017 //       Greedy Planner
00018 //       RRT-Like Planner
00019 //       RRT-Connect-Like Planner
00020 //       RRT-Greedy-Like Planner
00021 //       RRT-Greedy-Connect Planner
00022 //      
00023 //For extension, the following algorithms are imlemented:
00024 //       Improved-Greedy Planner
00025 //       Self-motion-Greedy Planner
00026 //       Self-motion-RRT-Greedy-Like Planner
00027 //       Self-motion-RRT-Connect-Like Planner
00028 //
00029 //
00030 //Revision History:
00031 //       Date     Author    Description
00032 //      ---------------------------------------------------------
00033 //     Jan-2002:  Z.Yao     Original algorithms implemented
00034 //     Mar-2003:  Z.Yao     Improve algorithms implemented
00036 
00037 #include <stdio.h>
00038 
00039 #include "math/math2.h"
00040 #include "math/matrixmxn.h"
00041 #include "math/vector4.h"
00042 #include <synchronization/semaphore.h>
00043 #include "PL_Mpep.h"
00044 #include "opengl/glos.h"
00045 #include <gl/gl.h>
00046 #include "SMGraph.h"
00047 #include "debug/debug.h"
00048 //#define Log(a, b, c) ;
00049 //#define LogMessage(a, b) ;
00050 
00051 using namespace std;
00052 
00053 #define _FIRST_POSE   (0)
00054 #define _LAST_POSE    (poses.size()-1)
00055 #define DEF_SMG_STEPSIZE        10
00056 #define DEF_ERROR_TOLERANCE     0.01
00057 
00058 #define MAX_SMG_EXTEND_RETRY    10
00059 
00060 static int numSMGraph=0;
00061 static int numSMGNode=0;
00062 static int numFinalSMGraph=0;
00063 static int numFinalSMGNode=0;
00064 static int numSMGraphInPath=0;
00065 static int numSMGNodeInPath=0;
00066 static int numTotalNode=0;
00067 //------------------------------------------------------------------
00068 //
00069 //Construction and Deconstruction function
00070 //
00071 //------------------------------------------------------------------
00072 PL_MPEP::PL_MPEP()
00073 {
00074         distTolerance = DEFAULT_TOLERANCE;
00075         randomRetry = DEFAULT_RANDOM_RETRY;
00076         algorithm = ALG_GREEDY_PLANNER;
00077         passiveNum = 2;
00078         extendNum = DEFAULT_EXTEND_RETRY;
00079         exploreNum = DEFAULT_EXPLORE_RETRY;
00080         iterationNum = DEFAULT_ITERATION;
00081         smgNodeNum = DEFAULT_SMGNODE_NUM;
00082         usingStartConfig = true;
00083         usingGoalConfig = false;
00084         usingGoalTree = false;
00085         m_pJacobian = NULL;
00086         usingJacobianExt = false;
00087         ClearTree();
00088 }
00089 
00090 PL_MPEP::~PL_MPEP()
00091 {
00092         if (m_pJacobian != NULL)
00093                 delete m_pJacobian;
00094         ClearTree();
00095 }
00096 
00097 //------------------------------------------------------------------
00098 //
00099 //Get tool frame by configuration
00100 //
00101 //------------------------------------------------------------------
00102 Frame PL_MPEP::GetToolFrame( const Configuration& config ) const
00103 {
00104         Frame toolFrame = Matrix4x4();
00105 
00106         redundant.GetDesireFrameByConfiguration(config, toolFrame);
00107 
00108         return toolFrame;
00109 }
00110 
00111 //------------------------------------------------------------------
00112 //
00113 //Begin planning
00114 //
00115 //------------------------------------------------------------------
00116 bool PL_MPEP::Plan()
00117 {
00118         //Set seed of random, it's supposed be done by upper layer, actually
00119         if (m_pJacobian == NULL)
00120                 m_pJacobian = new CJacobian(*m_pRobot);
00121 
00122         srand( (unsigned)time( NULL ) );
00123 
00124         numTotalNode = 0;
00125 
00126         path.Clear();
00127 
00128         int result=ERROR_OK;
00129 
00130         Configuration startConfig = GetStartConfig();
00131         Configuration goalConfig = GetGoalConfig();
00132 
00133         if (usingStartConfig)
00134         {
00135                 if (collisionDetector->IsInterfering(startConfig))
00136                 {
00137                         result = ERROR_START_CONFIG_INVALID;
00138                 }
00139         }
00140 
00141         StartTimer();
00142 
00143         if (result == ERROR_OK)
00144         {
00145                 //AddTrajectory(startConfig, goalConfig);
00146                 switch (algorithm)
00147                 {
00148                 case ALG_GREEDY_PLANNER:
00149                         result = Planner_Greedy();
00150                         break;
00151                 case ALG_RRT_LIKE:
00152                         result = Planner_RRT_Like();
00153                         break;
00154                 case ALG_RRT_CONNECT_LIKE:
00155                         result = Planner_RRT_Connect_Like();
00156                         break;
00157                 case ALG_RRT_GREEDY_LIKE:
00158                         result = Planner_RRT_Greedy_Like();
00159                         break;
00160                 case ALG_RRT_GREED_CONNECT:
00161                         result = Planner_RRT_Greedy_Connect();
00162                         break;
00163                 case ALG_IMP_GREEDY_PLANNER:
00164                         result = Planner_IMP_Greedy();
00165                         break;
00166                 case ALG_SMG_GREEDY_PLANNER:
00167                         result = Planner_SMG_Greedy();
00168                         break;
00169                 case ALG_SMG_RRT_PLANNER:
00170                         result = Planner_SMG_RRT_Greedy_Like();
00171                         break;
00172                 case ALG_SMG_CONNECT_PLANNER:
00173                         result = Planner_SMG_RRT_Connect_Like();
00174                         break;
00175                 default:
00176                         result = ERROR_UNKNOW_PLANNER;
00177                         break;
00178                 }
00179         }
00180 
00181         poses.clear();
00182         if (result == ERROR_OK)
00183                 return true;
00184 
00185         Display_Error_Message(result);
00186         path.Clear();
00187         path.AppendPoint(startConfig);
00188         return false;
00189 }
00190 
00191 //------------------------------------------------------------------
00192 //This function is use to extend from pose "begin" to pose "end",
00193 //      it just for the GREEDY-series planners.
00194 //
00195 //Input Parameter:
00196 //      begin: The index of pose we going to explore from
00197 //      end  : The index of target pose
00198 //      base : The pointer to vertex corresponding to "begin"
00199 //
00200 //Ouput Parameter:
00201 //      begin: The index of pose where we fail to go further
00202 //      base : The pointer to the failure vertex
00203 //
00204 //Return Value:
00205 //      ERROR_OK, if success
00206 //      ERROR_GET_RANDOM_CONF_FAILURE,
00207 //      ERROR_GET_COLLISION_FREE_FAILURE,
00208 //
00209 //Notes:
00210 //      The exploration will fail if:
00211 //       - fail to get a closed configuration, or
00212 //       - fail to get a collision free configuration
00213 //------------------------------------------------------------------
00214 int PL_MPEP::Greedy_Step(int &begin, int end, vertex &base)
00215 {
00216         int i;
00217         int retry;
00218         int last=begin;
00219         vertex parent=base;
00220         Pose_Node *node=(Pose_Node *)tree.vertex_inf(base);
00221         Pose_Node newNode;
00222         Configuration conf;
00223         Configuration prevConf=node->conf;
00224 
00225         for(i=begin; i<end; i++)
00226         {
00227                 retry = 0;
00228                 while (retry < exploreNum)
00229                 {
00230                         if (GetRandomConfiguration(conf, poses[i+1], prevConf))
00231                         {
00232                                 if (!collisionDetector->IsInterfering(conf))
00233                                 {
00234                                         if (!collisionDetector->IsInterferingLinear(prevConf, conf))
00235                                         {
00236                                                 newNode.conf = conf;
00237                                                 newNode.poses = i+1;
00238                                                 newNode.graph = NULL;
00239                                                 parent = ConnectNode(tree, vertices, parent, newNode);
00240                                                 prevConf = conf;
00241                                                 begin = i+1;
00242                                                 base = parent;
00243                                                 break;
00244                                         }
00245                                 }
00246                         }
00247 
00248                         retry ++;
00249                 }//End while
00250                 if ((retry == exploreNum))
00251                 {
00252                         return ERROR_GET_COLLISION_FREE_FAILURE;
00253                 }
00254         }//End for
00255 
00256         return ERROR_OK;
00257 }
00258 
00259 bool PL_MPEP::AdjustConfiguration(Configuration &config, int nPose)
00260 {
00261         Frame desiredFrame = poses[nPose];
00262         Vector4 desiredPos = desiredFrame.GetTranslationVector();
00263 
00264         //LogMessage("reconfig.log", "Start From Here................");
00265         //LogMatrix("reconfig.log", desiredFrame, "Desired Frame");
00266 
00267         int logRetry=0;
00268         while (true)
00269         {
00270                 Frame currentFrame = GetToolFrame(config);
00271                 Vector4 currentPos = currentFrame.GetTranslationVector();
00272 
00273                 //LogConfiguration("reconfig.log", config);
00274                 //LogMatrix("reconfig.log", currentFrame, "Current Frame");
00275                 
00276                 Vector4 offset = desiredPos - currentPos;
00277                 double dDistance = offset.Magnitude();
00278                 if (dDistance < DEF_ERROR_TOLERANCE)
00279                         break;
00280 
00281                 logRetry++;
00282 
00283                 Matrixmxn mJEnd, mJEndInv;
00284                 m_pJacobian->SetConfiguration(config);
00285                 m_pJacobian->SetInterestPoint(m_nDof-1, m_frEndEffector, true, false);
00286                 mJEnd = *(m_pJacobian->GetMatrix());
00287                 mJEnd.Inverse(mJEndInv);
00288                 //LogMatrix("reconfig.log", mJEnd, "Jacobian");
00289                 //LogMatrix("reconfig.log", mJEndInv, "Jacobian Inverse");
00290 
00291                 VectorN endEffOffset(offset[0], offset[1], offset[2]);
00292                 Configuration jointOffset;
00293                 jointOffset = mJEndInv * endEffOffset *  Rad2Deg(1);
00294                 config += jointOffset;
00295         }
00296         //Log("reconfig.log", "It take: %d  iterations to do adjustment", logRetry);
00297         return true;
00298 }
00299 
00300 bool PL_MPEP::IsJointWithinLimits(Configuration& config)
00301 {
00302         bool bResult = true;
00303         for (int i = 0; i<m_nDof; i++)
00304         {
00305                 if ( config[i] < collisionDetector->JointMin(i) )
00306                 {
00307                         bResult = false;
00308                 }
00309                 else if ( config[i] > collisionDetector->JointMax(i) )
00310                 {
00311                         bResult = false;
00312                 }
00313         }
00314         return bResult;
00315 }
00316 
00317 int PL_MPEP::Extend_Node_Self_Motion_Graph_Jacobian(vertex g_vertex, Configuration **nextConf)
00318 {
00319         void* nodeInGraph=NULL;
00320         Pose_Node *node = (Pose_Node*) tree.vertex_inf(g_vertex);
00321         SMGraph *g = node->graph;
00322         (*nextConf)=NULL;
00323 
00324         //The method use to explore the S-M-G is RRT like exploration method
00325         //  1. Put a random configuration, as a random direction
00326         //  2. Get the closest configuration "q" in this graph
00327         //  3. Make one step further from "q" in this direction, with fixed length
00328         Configuration randConf, activeConf, newConf;
00329         Configuration *nearConf;
00330         bool bFind = false;
00331 
00332         Matrixmxn mJEnd, mJEndInv;
00333         Matrixmxn mTemp(m_nDof, m_nDof); 
00334         Matrixmxn matrixIdentity(m_nDof, m_nDof);
00335         //int logK = 0;
00336         //for (int k=0; k<MAX_SMG_EXTEND_RETRY; k++)
00337         //{
00338         for (int i=0; i<randomRetry; i++)
00339         {
00340                 //logK++;
00341                 GetRandomConfiguration(randConf);
00342 
00343                 //The v here, is actually the node in self-motion graph
00344                 nodeInGraph = GetNearestNodeInGraph(g, &randConf);
00345                 nearConf = g->GetConfiguration(nodeInGraph);
00346                 Configuration dirVel = randConf - (*nearConf);
00347 
00348                 mJEnd = *(m_pJacobian->GetMatrix());
00349                 mJEnd.Inverse(mJEndInv);
00350                 mTemp = matrixIdentity;
00351                 mTemp -= mJEndInv * mJEnd;
00352                 dirVel = mTemp * dirVel;
00353                 dirVel *= (DEF_SMG_STEPSIZE/ dirVel.Magnitude());
00354                 newConf = *nearConf + dirVel;
00355                 if (!IsJointWithinLimits(newConf))
00356                         continue;
00357                 if (collisionDetector->IsInterfering(newConf))
00358                         continue;
00359 
00360                 AdjustConfiguration(newConf, node->poses);
00361                 if (collisionDetector->IsInterferingLinear(newConf, *nearConf))
00362                         continue;
00363 
00364                 bFind = true;
00365                 break;
00366         }
00367         
00368         //}
00369         if (!bFind)
00370         {
00371                 //Log("PlannerForTest.log", "Failed to extend SMG after : %d  retries", logK);
00372                 return ERROR_GET_COLLISION_FREE_FAILURE;
00373         }
00374 
00375         //Log("PlannerForTest.log", "Succeeded to extend SMG after : %d  retries", logK);
00376         nodeInGraph = g->AddConnection(nodeInGraph, newConf);
00377         (*nextConf) = g->GetConfiguration(nodeInGraph);
00378 
00379 
00380         //Testing for validity, need to be removed later
00381         Frame currentFrame = GetToolFrame(**nextConf);
00382         Frame desiredFrame = poses[node->poses];
00383         Vector4 vec1 = currentFrame.GetTranslationVector();
00384         Vector4 vec2 = desiredFrame.GetTranslationVector();
00385         vec2 -= vec1;
00386         double dDistance = vec2.Magnitude();
00387         //Log("PlannerForTest.log", "The distance to pose: %d, is %f", node->poses, dDistance);
00388 
00389         return ERROR_OK;
00390 }
00391 
00392 //------------------------------------------------------------------
00393 //This function is use to explore self-motion graph for a given pose,
00394 //      it is for the those planners enabled self-motion.
00395 //
00396 //Input Parameter:
00397 //      g_vertex: The vertex in the tree for the given pose
00398 //
00399 //Ouput Parameter:
00400 //      nextConf: The valid configuration generated in this graph
00401 //
00402 //Return Value:
00403 //      ERROR_OK, if success
00404 //      ERROR_GET_RANDOM_CONF_FAILURE,
00405 //
00406 //Notes I:
00407 //      Here, we just consider those node which is connecteable to current graph.
00408 //          those not connectable will be discarded
00409 //
00410 //Notes II:
00411 //      The exploration will fail if:
00412 //       - fail to get a closed configuration, or
00413 //       - fail to get a collision free configuration
00414 //       - the number of node achieve the limit
00415 //------------------------------------------------------------------
00416 int PL_MPEP::Extend_Node_Self_Motion_Graph(vertex g_vertex, Configuration **nextConf)
00417 {
00418         void* nodeInGraph=NULL;
00419         Pose_Node *node = (Pose_Node*) tree.vertex_inf(g_vertex);
00420         SMGraph *g = node->graph;
00421         (*nextConf)=NULL;
00422 
00423         //The method use to explore the S-M-G is RRT like exploration method
00424         //  1. Put a random configuration, as a random direction
00425         //  2. Get the closest configuration "q" in this graph
00426         //  3. Make one step further from "q" in this direction, with fixed length
00427         Configuration randConf, activeConf, newConf;
00428         Configuration *nearConf;
00429         bool bFind = false;
00430 
00431         //for (int k=0; k<MAX_SMG_EXTEND_RETRY; k++)
00432         //{
00433         for (int i=0; i<randomRetry; i++)
00434         {
00435                 GetRandomConfiguration(randConf);
00436 
00437                 //The v here, is actually the node in self-motion graph
00438                 nodeInGraph = GetNearestNodeInGraph(g, &randConf);
00439                 nearConf = g->GetConfiguration(nodeInGraph);
00440 
00441                 if (!GetConfigurationByDirection(activeConf, *(nearConf), randConf))
00442                         continue;
00443 
00444                 //I am thinking whether we should change the tolerance to make generating
00445                 //   feasible configurations easier.
00446                 if (!redundant.GetConfigurationByActive(newConf, activeConf, *nearConf,
00447                                                                                                 poses[node->poses], distTolerance))
00448                 //if (!redundant.GetConfigurationByActive(newConf, activeConf, *nearConf,
00449                 //                                        poses[node->poses], activeConf.DOF()*distTolerance))
00450                         continue;
00451 
00452                 if (collisionDetector->IsInterfering(newConf))
00453                         continue;
00454 
00455                 if (collisionDetector->IsInterferingLinear(newConf, *nearConf))
00456                         continue;
00457 
00458                 bFind = true;
00459                 break;
00460         }
00461         
00462         if (!bFind)
00463                 return ERROR_GET_COLLISION_FREE_FAILURE;
00464 
00465         nodeInGraph = g->AddConnection(nodeInGraph, newConf);
00466         (*nextConf) = g->GetConfiguration(nodeInGraph);
00467         return ERROR_OK;
00468 }
00469 
00470 int PL_MPEP::Establish_Self_Motion_Graph(dynamic_trees &tr, vertex extended)
00471 {
00472         int result;
00473         Pose_Node *node = (Pose_Node *)tr.vertex_inf(extended);
00474         if (node->graph==NULL)
00475         {
00476                 numSMGraph++;
00477                 numFinalSMGraph++;
00478 
00479                 node->graph = new SMGraph(node->conf);
00480 
00481                 //for (int i=0; i<5; i++)
00482                 //{
00483                 //      Configuration *nextConf;
00484                 //      result = Extend_Node_Self_Motion_Graph(extended, &nextConf);
00485                 //      if (result != ERROR_OK)
00486                 //      {
00487                 //              return result;
00488                 //      }
00489                 //}
00490         }
00491         if (node->graph->Node_Num()<smgNodeNum)
00492         {
00493                 Configuration *nextConf;
00494 
00495                 if (usingJacobianExt)
00496                         result = Extend_Node_Self_Motion_Graph_Jacobian(extended, &nextConf);
00497                 else
00498                         result = Extend_Node_Self_Motion_Graph(extended, &nextConf);
00499                 if (result==ERROR_OK)
00500                 {
00501                         numSMGNode++;
00502                         numFinalSMGNode++;
00503                         node->conf = *nextConf;
00504                 }
00505                 /*
00506                 for (int retry=0; retry < exploreNum; retry++)
00507                 {
00508                         result = Extend_Node_Self_Motion_Graph(extended, &nextConf);
00509                         if (result==ERROR_OK)
00510                         {
00511                                 node->conf = *nextConf;
00512                                 break;
00513                         }
00514                 }*/
00515         }
00516         else
00517         {
00518                 result = ERROR_SMGRAPH_FULL;
00519         }
00520         return result;
00521 }
00522 
00523 //------------------------------------------------------------------
00524 //This function is use to check the connectivity from a new node
00525 //      to the random tree growing up from the other direction;
00526 //
00527 //Input Parameter:
00528 //      extendedVert: The new node ;
00529 //                tr: The random tree;
00530 //              vert: The arry storing the vertices in the tree;
00531 //               dir: The growing relative direction between the node and the tree
00532 //                     1: node is in the tree from START, tree grows from GOAL;
00533 //                    -1: node is in the tree from GOAL, tree grows from START;
00534 //
00535 //Ouput Parameter:
00536 //     connectedVert: The node connectable to extendedVert in the given random tree.
00537 //
00538 //Return Value:
00539 //      ERROR_OK, if success
00540 //      ERROR_GET_COLLISION_FREE_FAILURE,
00541 //
00542 //Notes I:
00543 //      Here, we just consider those node which is connecteable to current graph.
00544 //          those not connectable will be discarded
00545 //
00546 //Notes II:
00547 //      The exploration will fail if:
00548 //       - fail to get a closed configuration, or
00549 //       - fail to get a collision free configuration
00550 //       - the number of node achieve the limit
00551 //------------------------------------------------------------------
00552 int PL_MPEP::CheckConnectivity(dynamic_trees &tr, std::vector<vertex> &vert, vertex extendedVert, vertex &connectedVert, int dir)
00553 {
00554         int i;
00555         Pose_Node *node;
00556         Pose_Node *extendedNode;
00557         extendedNode = (Pose_Node *)tree.vertex_inf(extendedVert);
00558 
00559         int index = extendedNode->poses + dir;
00560         bool bFind = false;
00561 
00562         for (i=0; i<vert.size(); i++)
00563         {
00564                 node = (Pose_Node *)tr.vertex_inf(vert[i]);
00565                 if (node->poses == index)
00566                 {
00567                         if (!collisionDetector->IsInterferingLinear(node->conf, extendedNode->conf))
00568                         {
00569                                 bFind = true;
00570                                 connectedVert = vert[i];
00571                         }
00572                         else if (node->graph != NULL)
00573                         {
00574                                 void *nodeInGraph = node->graph->FirstNode();
00575                                 for (; nodeInGraph != NULL ; )
00576                                 {
00577                                         Configuration *confInGraph = node->graph->GetConfiguration(nodeInGraph);
00578                                         if (!collisionDetector->IsInterferingLinear(node->conf, extendedNode->conf))
00579                                         {
00580                                                 bFind = true;
00581                                                 node->conf = *(confInGraph);
00582                                                 connectedVert = vert[i];
00583                                                 break;
00584                                         }
00585                                         nodeInGraph = node->graph->NextNode();
00586                                 }
00587                         }
00588                         if (bFind)
00589                                 break;
00590                 }
00591         } //end for
00592 
00593         if (!bFind)
00594                 return ERROR_GET_COLLISION_FREE_FAILURE;
00595         else
00596                 return ERROR_OK;
00597 }
00598 
00599 //------------------------------------------------------------------
00600 //This function is RRT-Connect Like Planner, with Self-Motion Graph enabled;
00601 //
00602 //Input Parameter:
00603 //      N/A;
00604 //
00605 //Ouput Parameter:
00606 //      N/A.
00607 //
00608 //Return Value:
00609 //      ERROR_OK, if success;
00610 //      ERROR_SMGRAPH_FULL, if fail due to self motion graph for a node is full;
00611 //      ERROR_RRT_EXTEND_FAILURE, if fail after a certain number of retry;
00612 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
00613 //
00614 //Description:
00615 //      Two tree growing up from START and GOAL respectively, and make extension from two tree in turn.
00616 //      After every EXTENTION, we greedily step to the tree growing up from the other direction.
00617 //            in where fail to get further, propagate a Self-Motion-Graph.
00618 //            in where succeed to get further, try to connect to the random tree growing up from the other direction.
00619 //------------------------------------------------------------------
00620 int PL_MPEP::Planner_SMG_RRT_Connect_Like()
00621 {
00622         int poseIndexFromStart = _FIRST_POSE;
00623         int poseIndexFromGoal = _LAST_POSE;
00624         int retry = 0;
00625         int dir, result;
00626         bool walkinotherdir = false;
00627         bool connected = false;
00628         vertex extendedVert;
00629         vertex connectedVert = NULL;
00630 
00631         usingGoalTree = true;
00632         while ((!connected) && (retry<iterationNum))
00633         {
00634                 if ((result=InitializeTree())!=ERROR_OK)
00635                         //return result;
00636                 {
00637                         retry ++;
00638                         continue;
00639                 }
00640 
00641                 int extend_retry = 0;
00642 
00643                 do
00644                 {
00645                         dir = 1;
00646                         if (RRT_Extend_Like(tree, vertices, poseIndexFromStart, extendedVert, dir)==ERROR_OK)
00647                         {
00648                                 //----------------------------------------------------------------------
00649                                 //  Walk from Randomtree growing up from START;
00650                                 //----------------------------------------------------------------------
00651                                 walkinotherdir = false;
00652                                 while ((!walkinotherdir) && (!connected))
00653                                 {
00654                                         result = Step(tree, vertices, poseIndexFromStart, poseIndexFromStart+1, extendedVert);
00655                                         if (result == ERROR_OK)
00656                                         {
00657                                                 poseIndexFromStart = poseIndexFromStart + 1;
00658                                                 /*
00659                                                 if (poseIndexFromStart == _LAST_POSE)
00660                                                 {
00661                                                         connected = true;
00662                                                         goalPoseVertex = extendedVert;
00663                                                         goalPoseVertex2 = NULL;
00664                                                 }
00665                                                 else
00666                                                 */
00667                                                 if(extend_retry > (extendNum*0.6))
00668                                                 {
00669                                                         if (CheckConnectivity(goal_tree, goal_vertices, extendedVert, connectedVert, dir)==ERROR_OK)
00670                                                         {
00671                                                                 connected = true;
00672                                                                 goalPoseVertex = extendedVert;
00673                                                                 goalPoseVertex2 = connectedVert;
00674                                                         }
00675                                                 }
00676                                         }
00677                                         /*
00678                                         else
00679                                         {
00680                                                 //----------------------------------------------------------------------
00681                                                 //  Create or Explore the self-motion-graph for the leaf;
00682                                                 //----------------------------------------------------------------------
00683                                                 result = Establish_Self_Motion_Graph(tree, extendedVert);
00684                                                 if (result != ERROR_OK)
00685                                                         walkinotherdir = true;
00686                                         }
00687                                         */
00688                                         walkinotherdir = true;
00689                                 } //while ((!walkinother) && (!connected))
00690                         }
00691 
00692                         if (connected)
00693                                 break;
00694 
00695                         dir = -1;
00696                         if (RRT_Extend_Like(goal_tree, goal_vertices, poseIndexFromGoal, extendedVert, dir)==ERROR_OK)
00697                         {
00698                                 //----------------------------------------------------------------------
00699                                 //  Walk from Randomtree growing up from GOAL;
00700                                 //----------------------------------------------------------------------
00701                                 walkinotherdir = false;
00702                                 while ((!walkinotherdir) && (!connected))
00703                                 {
00704                                         result = Step(goal_tree, goal_vertices, poseIndexFromGoal, poseIndexFromGoal-1, extendedVert, dir);
00705                                         if (result == ERROR_OK)
00706                                         {
00707                                                 poseIndexFromGoal = poseIndexFromGoal - 1;
00708                                                 /*
00709                                                 if (poseIndexFromGoal == _FIRST_POSE)
00710                                                 {
00711                                                         connected = true;
00712                                                         goalPoseVertex2 = extendedVert;
00713                                                         goalPoseVertex = NULL;
00714                                                 }
00715                                                 else
00716                                                 */
00717                                                 if(extend_retry > (extendNum*0.6))
00718                                                 {
00719                                                         if (CheckConnectivity(tree, vertices, extendedVert, connectedVert, dir)==ERROR_OK)
00720                                                         {
00721                                                                 connected = true;
00722                                                                 goalPoseVertex = connectedVert;
00723                                                                 goalPoseVertex2 = extendedVert;
00724                                                         }
00725                                                 }
00726                                         }
00727                                         /*
00728                                         else
00729                                         {
00730                                                 //----------------------------------------------------------------------
00731                                                 //  Create or Explore the self-motion-graph for the leaf;
00732                                                 //----------------------------------------------------------------------
00733                                                 result = Establish_Self_Motion_Graph(goal_tree, extendedVert);
00734                                                 if (result != ERROR_OK)
00735                                                         walkinotherdir = true;
00736                                         }
00737                                         */
00738                                         walkinotherdir = true;
00739                                 } //while ((!walkinother) && (!connected))
00740                         }
00741 
00742                         extend_retry +=2;
00743 
00744                         if ( HasTimeLimitExpired() )
00745                                 return ERROR_PLANNING_TIMEOUT;
00746                 } while ((!connected) && (extend_retry<extendNum));
00747 
00748                 retry ++;
00749         } //while ((!connected) && (retry<iterationNum))
00750 
00751         if (! connected)
00752                 return ERROR_RRT_EXTEND_FAILURE;
00753 
00754         RetrievePathFromTree();
00755         return ERROR_OK;
00756 }
00757 
00758 //------------------------------------------------------------------
00759 //This function is RRT-Greedy Like Planner, with Self-Motion Graph enabled;
00760 //
00761 //Input Parameter:
00762 //      N/A;
00763 //
00764 //Ouput Parameter:
00765 //      N/A.
00766 //
00767 //Return Value:
00768 //      ERROR_OK, if success;
00769 //      ERROR_SMGRAPH_FULL, if fail due to self motion graph for a node is full;
00770 //      ERROR_RRT_EXTEND_FAILURE, if fail after a certain number of retry;
00771 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
00772 //
00773 //Description:
00774 //      Only one tree growing up from START.
00775 //      After every EXTENTION, we greedily step to the goal.
00776 //            in where fail to get further, propagate a Self-Motion-Graph.
00777 //------------------------------------------------------------------
00778 int PL_MPEP::Planner_SMG_RRT_Greedy_Like()
00779 {
00780         int poseIndexFromStart = _FIRST_POSE;
00781         int poseIndexFromGoal = _LAST_POSE;
00782         int retry = 0;
00783         int dir, result;
00784         bool walkinotherdir = false;
00785         bool connected = false;
00786         vertex extendedVert;
00787         vertex connectedVert = NULL;
00788 
00789         LogMessage("SMGCount.Log", "Start SMG-RRT");
00790         
00791         numSMGraph = numSMGNode = 0;
00792 
00793         usingGoalTree = false;
00794         while ((!connected) && (retry<iterationNum))
00795         {
00796                 numFinalSMGraph = numFinalSMGNode = 0;
00797 
00798                 if ((result=InitializeTree())!=ERROR_OK)
00799                         return result;
00800 
00801                 int extend_retry = 0;
00802 
00803                 do
00804                 {
00805                         dir = 1;
00806                         if (RRT_Extend_Like(tree, vertices, poseIndexFromStart, extendedVert, dir)==ERROR_OK)
00807                         {
00808                                 //----------------------------------------------------------------------
00809                                 //  Walk from Randomtree growing up from START;
00810                                 //----------------------------------------------------------------------
00811                                 walkinotherdir = false;
00812                                 while ((!walkinotherdir) && (!connected))
00813                                 {
00814                                         //Here is a loop; It tries to go one step further each iteration;
00815                                         //    once it fail, it propogate an SMG.
00816                                         result = Step(tree, vertices, poseIndexFromStart, poseIndexFromStart+1, extendedVert);
00817                                         if (result == ERROR_OK)
00818                                         {
00819                                                 poseIndexFromStart = poseIndexFromStart + 1;
00820                                                 if (poseIndexFromStart == _LAST_POSE)
00821                                                 {
00822                                                         connected = true;
00823                                                         goalPoseVertex = extendedVert;
00824                                                         goalPoseVertex2 = NULL;
00825                                                 }
00826                                         }
00827                                         else
00828                                         {
00829                                                 //----------------------------------------------------------------------
00830                                                 //  Create or Explore the self-motion-graph for the leaf;
00831                                                 //----------------------------------------------------------------------
00832                                                 result = Establish_Self_Motion_Graph(tree, extendedVert);
00833                                                 if (result != ERROR_OK)
00834                                                         walkinotherdir = true;
00835                                         }
00836                                 } //while ((!walkinother) && (!connected))
00837                         }
00838 
00839                         if (connected)
00840                                 break;
00841 
00842                         extend_retry ++;
00843 
00844                         if ( HasTimeLimitExpired() )
00845                                 return ERROR_PLANNING_TIMEOUT;
00846                 } while ((!connected) && (extend_retry<extendNum));
00847 
00848                 retry ++;
00849         } //while ((!connected) && (retry<iterationNum))
00850 
00851         //Log("SMGCount.Log", "%d\t%d\t%d\t%d", numSMGraph, numSMGNode, numFinalSMGraph, numFinalSMGNode);
00852 
00853         if (! connected)
00854                 return ERROR_RRT_EXTEND_FAILURE;
00855 
00856         RetrievePathFromTree();
00857         return ERROR_OK;
00858 }
00859 
00860 //------------------------------------------------------------------
00861 //This function is Greedy Planner, with Self-Motion Graph enabled;
00862 //
00863 //Input Parameter:
00864 //      N/A;
00865 //
00866 //Ouput Parameter:
00867 //      N/A.
00868 //
00869 //Return Value:
00870 //      ERROR_OK, if success;
00871 //      ERROR_SMGRAPH_FULL, if fail due to self motion graph for a node is full;
00872 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
00873 //      ERROR_GET_COLLISION_FREE_FAILURE
00874 //
00875 //Description:
00876 //      Pretty much same as Greedy Planner, the only difference is:
00877 //            in where fail to get further, propagate a Self-Motion-Graph,
00878 //            and then try to connect from the nodes in S-M-G to GOAL.
00879 //------------------------------------------------------------------
00880 int PL_MPEP::Planner_SMG_Greedy()
00881 {
00882         double timeForSMG=0;
00883         double timePost=0;
00884         double timeForGreedy=0;
00885 
00886         int retry = 0;
00887         int result;
00888         int poseIndex = _FIRST_POSE;
00889         vertex extendedVert;
00890         Configuration conf;
00891 
00892         usingGoalTree = false;
00893 
00894         LogMessage("SMGCount.Log", "Start SMG-Greedy");
00895         numSMGraph = numSMGNode = 0;
00896 
00897         while (retry < iterationNum)
00898         {
00899                 numFinalSMGraph = numFinalSMGNode = 0;
00900                 if ((result=InitializeTree())!=ERROR_OK)
00901                         break;
00902 
00903                 extendedVert = vertices[0];
00904 
00905                 poseIndex = _FIRST_POSE;
00906                 do
00907                 {
00908                         timePost = GetTimeElapsedInSeconds();
00909                         result = Greedy_Step(poseIndex, _LAST_POSE, extendedVert);
00910                         timeForGreedy += (GetTimeElapsedInSeconds()-timePost);
00911 
00912                         //After return, poseIndex is where it fail and extendedVert is corresponding Vertex in tree.
00913 
00914                         if (result != ERROR_OK)
00915                         {
00916                                 timePost = GetTimeElapsedInSeconds();
00917                                 result = Establish_Self_Motion_Graph(tree, extendedVert);
00918                                 timeForSMG += (GetTimeElapsedInSeconds()-timePost);
00919                                 if (result != ERROR_OK)
00920                                         break;
00921                         }
00922                         else
00923                         {
00924                                 break;
00925                         }
00926 
00927                         if ( HasTimeLimitExpired() )
00928                         {
00929                                 result = ERROR_PLANNING_TIMEOUT;
00930                                 break;
00931                         }
00932                 } while (true);
00933 
00934                 retry ++;
00935                 if ((result == ERROR_PLANNING_TIMEOUT) || (result == ERROR_OK) )
00936                         break;
00937         }
00938 
00939         //Log("SMGCount.Log", "%d\t%d\t%d\t%d", numSMGraph, numSMGNode, numFinalSMGraph, numFinalSMGNode);
00940         //Log("SMGCount.Log", "Time used in SMG: %.4f \t Time used in Greedy: %.4f", timeForSMG, timeForGreedy);
00941         if (result==ERROR_OK)
00942         {
00943                 goalPoseVertex = extendedVert;
00944                 RetrievePathFromTree();
00945         }
00946 
00947         return result;
00948 }
00949 
00950 //------------------------------------------------------------------
00951 //This function is Greedy Planner, with mechanism of back-tracking;
00952 //
00953 //Input Parameter:
00954 //      N/A;
00955 //
00956 //Ouput Parameter:
00957 //      N/A.
00958 //
00959 //Return Value:
00960 //      ERROR_OK, if success;
00961 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
00962 //
00963 //Description:
00964 //      Pretty much same as Greedy Planner, the only difference is:
00965 //            in where fail to get further, propagate a Self-Motion-Graph,
00966 //            and then try to connect from the nodes in S-M-G to GOAL.
00967 //------------------------------------------------------------------
00968 int PL_MPEP::Planner_IMP_Greedy()
00969 {
00970         int retry = 0;
00971         int result;
00972         int poseIndex = _FIRST_POSE;
00973         vertex extendedVert;
00974         Configuration conf;
00975 
00976         usingGoalTree = false;
00977 
00978         while (retry < iterationNum)
00979         {
00980                 if ((result=InitializeTree())!=ERROR_OK)
00981                         break;
00982 
00983                 extendedVert = vertices[0];
00984 
00985                 poseIndex = _FIRST_POSE;
00986                 do
00987                 {
00988                         result = Greedy_Step(poseIndex, _LAST_POSE, extendedVert);
00989                         if (result != ERROR_OK)
00990                         {
00991                                 poseIndex --;
00992                                 extendedVert = tree.parent(extendedVert);
00993                         }
00994                         else
00995                         {
00996                                 break;
00997                         }
00998                         if ( HasTimeLimitExpired() )
00999                         {
01000                                 result = ERROR_PLANNING_TIMEOUT;
01001                                 break;
01002                         }
01003                 } while (poseIndex >= 0);
01004 
01005                 if (result != ERROR_OK)
01006                         retry ++;
01007                 else
01008                         break;
01009         }
01010 
01011         if (result==ERROR_OK)
01012         {
01013                 goalPoseVertex = extendedVert;
01014                 RetrievePathFromTree();
01015         }
01016 
01017         return result;
01018 }
01019 
01020 //------------------------------------------------------------------
01021 //This function is Greedy Planner;
01022 //
01023 //Input Parameter:
01024 //      N/A;
01025 //
01026 //Ouput Parameter:
01027 //      N/A.
01028 //
01029 //Return Value:
01030 //      ERROR_OK, if success;
01031 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
01032 //
01033 //Description:
01034 //      Please refer to Oriolo's paper.
01035 //------------------------------------------------------------------
01036 int PL_MPEP::Planner_Greedy()
01037 {
01038         int retry = 0;
01039         int result;
01040         vertex extendedVert;
01041         Configuration conf;
01042 
01043         usingGoalTree = false;
01044 
01045         while (retry < iterationNum)
01046         {
01047                 if ((result=InitializeTree())!=ERROR_OK)
01048                         break;
01049 
01050                 extendedVert = vertices[0];
01051                 result = Step(tree, vertices, _FIRST_POSE, _LAST_POSE, extendedVert);
01052                 if (result != ERROR_OK)
01053                         retry ++;
01054                 else
01055                         break;
01056                 if ( HasTimeLimitExpired() )
01057                 {
01058                         result = ERROR_PLANNING_TIMEOUT;
01059                         break;
01060                 }
01061         }
01062 
01063         if (result==ERROR_OK)
01064         {
01065                 goalPoseVertex = extendedVert;
01066                 RetrievePathFromTree();
01067         }
01068 
01069         return result;
01070 }
01071 
01072 //------------------------------------------------------------------
01073 //This function is RRT-Like Planner;
01074 //
01075 //Input Parameter:
01076 //      N/A;
01077 //
01078 //Ouput Parameter:
01079 //      N/A.
01080 //
01081 //Return Value:
01082 //      ERROR_OK, if success;
01083 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
01084 //
01085 //Description:
01086 //      Please refer to Oriolo's paper.
01087 //      Try to make one step further in every extension,
01088 //            making the tree randomly grow up evenly in every direction;
01089 //------------------------------------------------------------------
01090 int PL_MPEP::Planner_RRT_Like()
01091 {
01092         int poseIndex = _FIRST_POSE;
01093         int retry = 0;
01094         int result;
01095         vertex extendedVert;
01096 
01097         usingGoalTree = false;
01098 
01099         while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
01100         {
01101                 if ((result=InitializeTree())!=ERROR_OK)
01102                         return result;
01103 
01104                 int extend_retry = 0;
01105                 do
01106                 {
01107                         RRT_Extend_Like(tree, vertices, poseIndex, extendedVert);
01108                         extend_retry ++;
01109 
01110                         if ( HasTimeLimitExpired() )
01111                                 return ERROR_PLANNING_TIMEOUT;
01112                 } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));
01113 
01114                 retry ++;
01115         }
01116 
01117         if (poseIndex != _LAST_POSE)
01118                 return ERROR_RRT_EXTEND_FAILURE;
01119 
01120         goalPoseVertex = extendedVert;
01121         RetrievePathFromTree();
01122         return ERROR_OK;
01123 }
01124 
01125 //------------------------------------------------------------------
01126 //This function is RRT-Connect-Like Planner;
01127 //
01128 //Input Parameter:
01129 //      N/A;
01130 //
01131 //Ouput Parameter:
01132 //      N/A.
01133 //
01134 //Return Value:
01135 //      ERROR_OK, if success;
01136 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
01137 //
01138 //Description:
01139 //      Please refer to Oriolo's paper.
01140 //      Try to make one step further in every extension,
01141 //            making the tree randomly grow up evenly in every direction;
01142 //      After that, greedily step to the GOAL
01143 //------------------------------------------------------------------
01144 int PL_MPEP::Planner_RRT_Connect_Like()
01145 {
01146         int poseIndex = 0;
01147         int retry = 0;
01148         int result;
01149         vertex extendedVert;
01150 
01151         usingGoalTree = false;
01152 
01153         while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
01154         {
01155                 if ((result=InitializeTree())!=ERROR_OK)
01156                         return result;
01157 
01158                 int extend_retry = 0;
01159                 do
01160                 {
01161                         if (RRT_Extend_Like(tree, vertices, poseIndex, extendedVert)==ERROR_OK)
01162                         {
01163                                 if (Step(tree, vertices, poseIndex, _LAST_POSE, extendedVert)==ERROR_OK)
01164                                         poseIndex = _LAST_POSE;
01165                         }
01166                         extend_retry ++;
01167 
01168                         if ( HasTimeLimitExpired() )
01169                                 return ERROR_PLANNING_TIMEOUT;
01170                 } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));
01171 
01172                 retry ++;
01173         }
01174 
01175         if (poseIndex != _LAST_POSE)
01176                 return ERROR_RRT_EXTEND_FAILURE;
01177 
01178         goalPoseVertex = extendedVert;
01179         RetrievePathFromTree();
01180         return ERROR_OK;
01181 }
01182 
01183 //------------------------------------------------------------------
01184 //This function is RRT-Connect-Like Planner;
01185 //
01186 //Input Parameter:
01187 //      N/A;
01188 //
01189 //Ouput Parameter:
01190 //      N/A.
01191 //
01192 //Return Value:
01193 //      ERROR_OK, if success;
01194 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
01195 //
01196 //Description:
01197 //      Please refer to Oriolo's paper.
01198 //      Try to make one step further in every extension,
01199 //            making the tree randomly grow up evenly in every direction;
01200 //      After that, greedily try once more step.
01201 //------------------------------------------------------------------
01202 int PL_MPEP::Planner_RRT_Greedy_Like()
01203 {
01204         int poseIndex = 0;
01205         int retry = 0;
01206         int result;
01207         vertex extendedVert;
01208 
01209         usingGoalTree = false;
01210 
01211         while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
01212         {
01213                 if ((result=InitializeTree())!=ERROR_OK)
01214                         return result;
01215 
01216                 int extend_retry = 0;
01217                 do
01218                 {
01219                         if (RRT_Extend_Like(tree, vertices, poseIndex, extendedVert)==ERROR_OK)
01220                         {
01221                                 if (poseIndex<_LAST_POSE)
01222                                 {
01223                                         vertex closestVert = GetClosestPoseVert(tree, vertices);
01224                                         if (closestVert!=NULL)
01225                                         {
01226                                                 Pose_Node *closestNode=(Pose_Node *)tree.vertex_inf(closestVert);
01227                                                 if (Step(tree, vertices, closestNode->poses, closestNode->poses+1, extendedVert)==ERROR_OK)
01228                                                         poseIndex = closestNode->poses + 1;
01229                                         }
01230                                 }
01231                         }
01232                         extend_retry ++;
01233 
01234                         if ( HasTimeLimitExpired() )
01235                                 return ERROR_PLANNING_TIMEOUT;
01236                 } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));
01237 
01238                 retry ++;
01239         }
01240 
01241         if (poseIndex != _LAST_POSE)
01242                 return ERROR_RRT_EXTEND_FAILURE;
01243 
01244         goalPoseVertex = extendedVert;
01245         RetrievePathFromTree();
01246         return ERROR_OK;
01247 }
01248 
01249 //------------------------------------------------------------------
01250 //This function is RRT-Greedy-Connect-Like Planner;
01251 //
01252 //Input Parameter:
01253 //      N/A;
01254 //
01255 //Ouput Parameter:
01256 //      N/A.
01257 //
01258 //Return Value:
01259 //      ERROR_OK, if success;
01260 //      ERROR_GET_RANDOM_CONF_FAILURE, if fail after a certain number of retry to get configuration.
01261 //
01262 //Description:
01263 //      Please refer to Oriolo's paper.
01264 //      Try to make one step further in every extension,
01265 //            making the tree randomly grow up evenly in every direction;
01266 //      After that, greedily step toward the GOAL
01267 //            if fail, then try once more step from the closest node to the GOAL.
01268 //------------------------------------------------------------------
01269 int PL_MPEP::Planner_RRT_Greedy_Connect()
01270 {
01271         int poseIndex = 0;
01272         int retry = 0;
01273         int result;
01274         vertex extendedVert;
01275 
01276         usingGoalTree = false;
01277 
01278         while ((poseIndex != _LAST_POSE) && (retry<iterationNum))
01279         {
01280                 if ((result=InitializeTree())!=ERROR_OK)
01281                         return result;
01282 
01283                 int extend_retry = 0;
01284                 do
01285                 {
01286                         if (RRT_Extend_Like(tree, vertices, poseIndex, extendedVert)==ERROR_OK)
01287                         {
01288                                 if (Step(tree, vertices, poseIndex, _LAST_POSE, extendedVert)==ERROR_OK)
01289                                         poseIndex = _LAST_POSE;
01290                                 if (poseIndex<_LAST_POSE)
01291                                 {
01292                                         vertex closestVert = GetClosestPoseVert(tree, vertices);
01293                                         if (closestVert!=NULL)
01294                                         {
01295                                                 Pose_Node *closestNode=(Pose_Node *)tree.vertex_inf(closestVert);
01296                                                 if (Step(tree, vertices, closestNode->poses, closestNode->poses+1, closestVert)==ERROR_OK)
01297                                                 {
01298                                                         extendedVert = closestVert;
01299                                                         poseIndex = closestNode->poses + 1;
01300                                                 }
01301                                         }
01302                                 }
01303                         }
01304                         extend_retry ++;
01305 
01306                         if ( HasTimeLimitExpired() )
01307                                 return ERROR_PLANNING_TIMEOUT;
01308                 } while ((poseIndex!=_LAST_POSE) && (extend_retry<extendNum));
01309 
01310                 retry ++;
01311         }
01312 
01313         if (poseIndex != _LAST_POSE)
01314                 return ERROR_RRT_EXTEND_FAILURE;
01315 
01316         goalPoseVertex = extendedVert;
01317         RetrievePathFromTree();
01318         return ERROR_OK;
01319 }
01320 
01321 
01322 //--------------------------------------------------------------
01323 int PL_MPEP::RRT_Extend_Like(dynamic_trees &tr, std::vector<vertex> &vert, int &extPose, vertex &extended, int dir)
01324 {
01325         vertex nearVert, newVert;
01326         Configuration randConf, activeConf;
01327         //Pose_Node *newNode;
01328         Pose_Node newNode;
01329         Pose_Node *nearPose;
01330 
01331         bool bFind = false;
01332         //for (int i=0; i<randomRetry; i++)
01333         //{
01334         GetRandomConfiguration(randConf);
01335         nearVert = GetNearestNodeInTree(tr, vert, randConf);
01336         nearPose = (Pose_Node *)tr.vertex_inf(nearVert);
01337 
01338         bFind = GetConfigurationByDirection(activeConf, nearPose->conf, randConf);
01339         if (bFind)
01340                 bFind = redundant.GetConfigurationByActive(newNode.conf, activeConf, nearPose->conf,
01341                                                     poses[nearPose->poses+dir], distTolerance);
01342                 //if (!redundant.GetConfigurationByActive(newNode.conf, activeConf, nearPose->conf,
01343                 //                                        poses[nearPose->poses+dir], activeConf.DOF()*distTolerance))
01344         if (bFind)
01345         {
01346                 bFind = false;
01347                 if (!collisionDetector->IsInterfering(newNode.conf))
01348                 {
01349                         if (!collisionDetector->IsInterferingLinear(newNode.conf, nearPose->conf))
01350                                 bFind = true;
01351                 }
01352         }
01353 
01354         if (!bFind)
01355         {
01356                 return ERROR_GET_COLLISION_FREE_FAILURE;
01357         }
01358         else
01359         {
01360                 extPose = newNode.poses = nearPose->poses + dir;
01361                 newVert = ConnectNode(tr, vert, nearVert, newNode);
01362                 extended = newVert;
01363                 return ERROR_OK;
01364         }
01365 }
01366 
01367 int PL_MPEP::Step(dynamic_trees &tr, std::vector<vertex> &vert, int begin, int end, vertex &base, int dir)
01368 {
01369         int i;
01370         int retry;
01371         vertex parent=base;
01372         Pose_Node *node=(Pose_Node *)tr.vertex_inf(base);
01373         Pose_Node newNode;
01374         Configuration conf;
01375         Configuration prevConf=node->conf;
01376 
01377         i = begin;
01378         while (i!=end)
01379         {
01380                 retry = 0;
01381                 while (retry < exploreNum)
01382                 {
01383                         if (GetRandomConfiguration(conf, poses[i+dir], prevConf))
01384                         {
01385                                 //==================*************** key
01386                                 if (!collisionDetector->IsInterfering(conf))
01387                                 {
01388                                         if (!collisionDetector->IsInterferingLinear(prevConf, conf))
01389                                         {
01390                                                 newNode.conf = conf;
01391                                                 newNode.poses = i+dir;
01392                                                 newNode.graph = NULL;
01393                                                 parent = ConnectNode(tr, vert, parent, newNode);
01394                                                 prevConf = conf;
01395                                                 base = parent;
01396                                                 break;
01397                                         }
01398                                 }
01399                         }
01400 
01401                         retry ++;
01402                 }//End while
01403                 if (retry == exploreNum)
01404                 {
01405                         //LogMessage("mpep.log", "ERROR_GET_COLLISION_FREE_FAILURE");
01406                         return ERROR_GET_COLLISION_FREE_FAILURE;
01407                 }
01408 
01409                 i += dir;
01410         }//End while (i!=end)
01411 
01412         return ERROR_OK;
01413 }
01414 
01415 //--------------------------------------------------------------
01416 //Get the node in the tree which is closest to given configuration.
01417 //According Oriolo's paper, just the active joints is computed.
01418 //While here, we acutally compute the whole configuration,
01419 //  thinking there will be no much difference
01420 vertex PL_MPEP::GetNearestNodeInTree(dynamic_trees &tr, std::vector<vertex> &vert, Configuration &conf)
01421 {
01422         //tree.
01423         if (vert.size() == 0)
01424                 return NULL;
01425 
01426         Pose_Node *node;
01427         double minDist, dist;
01428         int minPose=0;
01429         node = (Pose_Node *)tr.vertex_inf(vert[0]);
01430         minDist = DistanceInActiveJoints(node->conf, conf);
01431 
01432         for (int i=1; i<vert.size(); i++)
01433         {
01434                 node = (Pose_Node *)tr.vertex_inf(vert[i]);
01435                 dist = DistanceInActiveJoints(node->conf, conf);
01436                 if (dist<minDist)
01437                 {
01438                         minDist = dist;
01439                         minPose = i;
01440                 }
01441         }
01442 
01443         return vert[minPose];
01444 }
01445 
01446 void *PL_MPEP::GetNearestNodeInGraph(SMGraph *graph, Configuration *conf)
01447 {
01448         void *nodeInGraph=NULL;
01449         void *minNode=NULL;
01450         Configuration *confInGraph;
01451 
01452         double dist;
01453         double minDist = 360.0*conf->DOF();
01454 
01455         nodeInGraph = graph->FirstNode();
01456         confInGraph = graph->GetConfiguration(nodeInGraph);
01457         while(nodeInGraph != NULL)
01458         {
01459                 dist = DistanceInActiveJoints(*confInGraph, *conf);
01460                 if (dist<minDist)
01461                 {
01462                         minDist = dist;
01463                         minNode = nodeInGraph;
01464                 }
01465                 nodeInGraph = graph->NextNode();
01466                 if (nodeInGraph)
01467                         confInGraph = graph->GetConfiguration(nodeInGraph);
01468         }
01469 
01470         return minNode;
01471 }
01472 
01473 int PL_MPEP::InitializeTree()
01474 {
01475         int i;
01476         bool bFind;
01477         Pose_Node firstNode;
01478         Pose_Node *newNode;
01479         vertex newVert;
01480         Semaphore semaphore( guid );
01481 
01482         ClearTree();
01483 
01484         //Create the Random tree growing up from the Start Pose.
01485         firstNode.poses = _FIRST_POSE;
01486         if (usingStartConfig)
01487         {
01488                 firstNode.conf = GetStartConfig();
01489         }
01490         else
01491         {
01492                 bFind = false;
01493                 for (i=0; i<randomRetry; i++)
01494                 {
01495                         if (!redundant.GetRandomConfiguration(firstNode.conf, poses[_FIRST_POSE]))
01496                                 continue;
01497                         if (collisionDetector->IsInterfering(firstNode.conf))
01498                                 continue;
01499                         bFind = true;
01500                         break;
01501                 }
01502                 if (!bFind)
01503                         return ERROR_START_CONFIG_INVALID;
01504         }
01505 
01506         //Create the root of dynamic tree
01507         newNode = new Pose_Node;
01508         newNode->conf = firstNode.conf;
01509         newNode->poses = firstNode.poses;
01510         newNode->graph = NULL;
01511 
01512         semaphore.Lock();
01513         newVert = tree.make(newNode);
01514         vertices.push_back(newVert);
01515         semaphore.Unlock();
01516 
01517         if (!usingGoalTree)
01518                 return ERROR_OK;
01519 
01520         //Create the Random tree growing up from the Goal Pose.
01521         firstNode.poses = _LAST_POSE;
01522 
01523         if (usingGoalConfig)
01524         {
01525                 firstNode.conf = GetGoalConfig();
01526         }
01527         else
01528         {
01529                 bFind = false;
01530                 for (i=0; i<randomRetry; i++)
01531                 {
01532                         //if (!redundant.GetRandomConfiguration(firstNode.conf, poses[_LAST_POSE]))
01533                         //  continue;
01534 
01535                         while (!redundant.GetRandomConfiguration(firstNode.conf, poses[_LAST_POSE]));
01536                         //continue;
01537                         if (collisionDetector->IsInterfering(firstNode.conf))
01538                                 continue;
01539                         bFind = true;
01540                         break;
01541                 }
01542                 if (!bFind)
01543                         return ERROR_GOAL_CONFIG_INVALID;
01544         }
01545 
01546         //Create the root of dynamic tree
01547         newNode = new Pose_Node;
01548         newNode->conf = firstNode.conf;
01549         newNode->poses = firstNode.poses;
01550         newNode->graph = NULL;
01551 
01552         semaphore.Lock();
01553         newVert = goal_tree.make(newNode);
01554         goal_vertices.push_back(newVert);
01555         semaphore.Unlock();
01556 
01557         return ERROR_OK;
01558 }
01559 
01560 void PL_MPEP::ClearTree()
01561 {
01562         numTotalNode += vertices.size();
01563 
01564         Semaphore semaphore( guid );
01565         semaphore.Lock();
01566 
01567         int i;
01568 
01569         for (i=0; i<vertices.size(); i++)
01570         {
01571                 Pose_Node *node = (Pose_Node*)tree.vertex_inf(vertices[i]);
01572                 if (node->graph)
01573                         delete node->graph;
01574                 delete node;
01575         }
01576         tree.clear();
01577         vertices.clear();
01578 
01579         for (i=0; i<goal_vertices.size(); i++)
01580         {
01581                 Pose_Node *node = (Pose_Node*)goal_tree.vertex_inf(goal_vertices[i]);
01582                 if (node->graph)
01583                         delete node->graph;
01584                 delete node;
01585         }
01586         goal_tree.clear();
01587         goal_vertices.clear();
01588 
01589         goalPoseVertex = NULL;
01590 
01591         semaphore.Unlock();
01592 }
01593 
01594 vertex PL_MPEP::ConnectNode(dynamic_trees &tr, std::vector<vertex> &vert, vertex &parent, Pose_Node &child)
01595 {
01596         Semaphore semaphore( guid );
01597         semaphore.Lock();
01598 
01599         Pose_Node *newNode = new Pose_Node;
01600         newNode->conf = child.conf;
01601         newNode->poses = child.poses;
01602         newNode->graph = NULL;
01603 
01604         vertex newVert = tr.make(newNode);
01605         vert.push_back(newVert);
01606         tr.link(newVert, parent, 1);
01607 
01608         semaphore.Unlock();
01609 
01610         return newVert;
01611 }
01612 
01613 void PL_MPEP::RetrievePathFromTree()
01614 {
01615         vertex current;
01616 
01617         //ASSERT(goalPoseVertex != NULL);
01618 
01619         itinerary.clear();
01620 
01621         if (goalPoseVertex != NULL)
01622         {
01623                 current = goalPoseVertex;
01624 
01625                 itinerary.push_back(current);
01626 
01627                 while ((current = tree.parent(current)) != NULL)
01628                 {
01629                         itinerary.insert(itinerary.begin(), current);
01630                 }
01631         }
01632 
01633         if ((usingGoalTree) && (goalPoseVertex2 != NULL))
01634         {
01635                 current = goalPoseVertex2;
01636                 while((current = goal_tree.parent(current)) != NULL)
01637                 {
01638                         itinerary.push_back(current);
01639                 }
01640         }
01641 
01642         numSMGraphInPath=0;
01643         numSMGNodeInPath=0;
01644         numTotalNode += vertices.size();
01645 
01646         for (int i=0; i<itinerary.size(); i++)
01647         {
01648                 Pose_Node *node;
01649                 node = (Pose_Node*)(tree.vertex_inf(itinerary[i]));
01650                 if (node->graph == NULL)
01651                 {
01652                         path.AppendPoint(node->conf);
01653                 }
01654                 else
01655                 {
01656                         numSMGraphInPath ++;
01657                         node->graph->GetPath();
01658                         Configuration *conf = node->graph->FirstConfigInPath();
01659                         while (conf != NULL)
01660                         {
01661                                 numSMGNodeInPath ++;
01662                                 path.AppendPoint(*(conf));
01663                                 conf = node->graph->NextConfigInPath();
01664                         }
01665                         numSMGNodeInPath = numSMGNodeInPath - 1; //Get rid of the root
01666                 }
01667         }
01668         Log("SMGCount.Log", "%d\t%d\t%d\t%d\t%d\t%d\tTotalNodes:\t%d\t%d", 
01669                 numSMGraph, numSMGNode, numFinalSMGraph, numFinalSMGNode, numSMGraphInPath, numSMGNodeInPath,
01670                 numTotalNode, vertices.size());
01671 }
01672 
01673 vertex PL_MPEP::GetClosestPoseVert(dynamic_trees &tr, std::vector<vertex> &vert)
01674 {
01675         if (vert.size() == 0)
01676                 return NULL;
01677 
01678         Pose_Node *node;
01679         double maxPose, poseIndex;
01680         int maxIndex=0;
01681 
01682         node = (Pose_Node *)tr.vertex_inf(vert[0]);
01683         maxPose = node->poses;
01684 
01685         for (int i=1; i<vert.size(); i++)
01686         {
01687                 node = (Pose_Node *)tr.vertex_inf(vert[i]);
01688                 poseIndex = node->poses;
01689                 if (poseIndex>maxPose)
01690                 {
01691                         maxPose = poseIndex;
01692                         maxIndex = i;
01693                 }
01694         }
01695 
01696         return vert[maxIndex];
01697 }
01698 
01699 //This function computes the distance according the two configurations
01700 double PL_MPEP::Distance(Configuration &conf1, Configuration &conf2)
01701 {
01702         VectorN jointDiff;
01703         jointDiff = collisionDetector->JointDisplacement(conf1, conf2);
01704 
01705         double distance = 0;
01706         for( int i = 0; i < jointDiff.Length(); i++ )
01707         {
01708                 distance += Sqr(jointDiff[i] );
01709         }
01710 
01711         return sqrt(distance);
01712 }
01713 //This function computes the distance according the active joints in these two configurations
01714 double PL_MPEP::DistanceInActiveJoints(Configuration &conf1, Configuration &conf2)
01715 {
01716         VectorN jointDiff;
01717         jointDiff = collisionDetector->JointDisplacement(conf1, conf2);
01718 
01719         double distance = 0;
01720         for( int i = 0; i < jointDiff.Length()-passiveNum; i++ )
01721         {
01722                 distance += Sqr(jointDiff[i] );
01723         }
01724 
01725         return sqrt(distance);
01726 }
01727 
01728 //--------------------------------------------------------------
01729 bool PL_MPEP::GetConfigurationByDirection(Configuration &conf, Configuration &base, Configuration &direct)
01730 {
01731         int i;
01732         Configuration activeConf, activeDirect;
01733         int dof = collisionDetector->DOF();
01734         int activeJointNum = dof-passiveNum;
01735         activeConf.SetLength(dof);
01736         activeDirect.SetLength(dof);
01737         conf.SetLength(activeJointNum);
01738         for (i=0; i<activeJointNum; i++)
01739         {
01740                 activeConf[i] = base[i];
01741                 activeDirect[i] = direct[i];
01742         }
01743         double dist = DistanceInActiveJoints(activeConf, activeDirect);
01744         double landa;
01745         if (dist==0)
01746                 return false;
01747         else
01748                 landa = (Rad2Deg(distTolerance)*sqrt(activeJointNum))/dist;
01749 
01750         for (i=0; i<activeJointNum; i++)
01751         {
01752                 conf[i] = (1-landa)*activeConf[i]+landa*activeDirect[i];
01753         }
01754 
01755         return true;
01756 }
01757 
01758 bool PL_MPEP::GetRandomConfiguration(Configuration &conf)
01759 {
01760         Configuration randConf;
01761         if (redundant.GetRandomConfiguration(randConf))
01762         {
01763                 conf = randConf;
01764                 return true;
01765         }
01766         return false;
01767 }
01768 
01769 bool PL_MPEP::GetRandomConfiguration(Configuration &conf, Frame &frame)
01770 {
01771         Configuration randConf;
01772         if (redundant.GetRandomConfiguration(randConf, frame))
01773         {
01774                 conf = randConf;
01775                 return true;
01776         }
01777 
01778         return false;
01779 }
01780 
01781 bool PL_MPEP::GetRandomConfiguration(Configuration &conf, Frame &frame, Configuration &bias)
01782 {
01783         if (distTolerance <= 0)
01784                 return GetRandomConfiguration(conf, frame);
01785 
01786         Configuration randConf;
01787         if (redundant.GetRandomConfiguration(randConf, bias, frame, distTolerance))
01788         {
01789                 conf = randConf;
01790                 return true;
01791         }
01792 
01793         return false;
01794 }
01795 
01796 //--------------------------------------------------------------
01797 void PL_MPEP::SetCollisionDetector (CD_BasicStyle* collisionDetector)
01798 {
01799         PL_Boolean_Output::SetCollisionDetector(collisionDetector);
01800 
01801         //null is a valid collision detector to set this to
01802         if( this->collisionDetector != NULL )
01803         {
01804                 FrameManager *frameManager=this->collisionDetector->GetFrameManager();
01805                 redundant.SetFrameManager(frameManager);
01806                 redundant.SetCollisionDetector(this->collisionDetector);
01807 
01808                 int nDOF = collisionDetector->DOF();
01809 
01810                 redundant.SetActiveBaseFrame(0);
01811                 redundant.SetActiveFirstJoint(0);
01812                 redundant.SetActiveLastJoint(nDOF-3);
01813 
01814                 redundant.SetPassiveBaseFrame(0);
01815                 redundant.SetPassiveFirstJoint(nDOF-2);
01816                 redundant.SetPassiveLastJoint(nDOF-1);
01817 
01818                 m_nDof = collisionDetector->DOF();
01819                 m_pRobot = dynamic_cast<R_OpenChain*>(collisionDetector->GetRobot(0));
01820                 m_nToolFrame = m_pRobot->GetToolFrame(0);
01821                 m_frEndEffector = Matrix4x4::Identity();
01822                 if (m_nToolFrame != -1)
01823                 {
01824                         FrameManager* frameManager = collisionDetector->GetFrameManager();
01825                         m_frEndEffector = *(frameManager->GetFrameRef(m_nToolFrame));
01826                 }
01827                 if (m_pJacobian != NULL)
01828                 {
01829                         delete m_pJacobian;
01830                         m_pJacobian = new CJacobian(*m_pRobot);
01831                 }
01832         }
01833 }
01834 
01835 void PL_MPEP::SetIterationNum(unsigned int num)
01836 {
01837         iterationNum = num;
01838 }
01839 
01840 void PL_MPEP::SetExploreNum(unsigned int num)
01841 {
01842         exploreNum = num;
01843 }
01844 
01845 void PL_MPEP::SetExtendNum(unsigned int num)
01846 {
01847         extendNum = num;
01848 }
01849 
01850 void PL_MPEP::SetPassiveNum(unsigned int num)
01851 {
01852         passiveNum = num;
01853         int nDOF = collisionDetector->DOF();
01854 
01855         redundant.SetActiveBaseFrame(0);
01856         redundant.SetActiveFirstJoint(0);
01857         redundant.SetActiveLastJoint(nDOF-num-1);
01858 
01859         redundant.SetPassiveBaseFrame(0);
01860         redundant.SetPassiveFirstJoint(nDOF-num);
01861         redundant.SetPassiveLastJoint(nDOF-1);
01862 }
01863 
01864 void PL_MPEP::SetAlgorithm(unsigned int alg)
01865 {
01866         algorithm = alg;
01867 }
01868 
01869 void PL_MPEP::SetTrajectory(std::vector<Frame> &traj)
01870 {
01871         poses.clear();
01872         for (int i=0; i<traj.size(); i++)
01873         {
01874                 poses.push_back(traj[i]);
01875         }
01876 }
01877 
01878 void PL_MPEP::SetUsingJacobianExt(bool use)
01879 {
01880         usingJacobianExt = use;
01881 }
01882 
01883 void PL_MPEP::SetTolerance(double dist)
01884 {
01885         distTolerance = dist;
01886 }
01887 
01888 void PL_MPEP::SetUsingStartConfig(bool use)
01889 {
01890         usingStartConfig = use;
01891 }
01892 
01893 void PL_MPEP::SetUsingGoalConfig(bool use)
01894 {
01895         usingGoalConfig = use;
01896 }
01897 
01898 void PL_MPEP::SetRandomRetry(unsigned int retry)
01899 {
01900         randomRetry = retry;
01901 }
01902 
01903 void PL_MPEP::SetSMGNodeNum(unsigned num)
01904 {
01905         smgNodeNum = num;
01906 }
01907 
01908 void PL_MPEP::Display_Error_Message(int result)
01909 {
01910         char *szErrorMsg;
01911         switch(result)
01912         {
01913         case ERROR_GET_RANDOM_CONF_FAILURE:
01914                 szErrorMsg = "Can not get random configuration!";
01915                 break;
01916         case ERROR_GET_COLLISION_FREE_FAILURE:
01917                 szErrorMsg = "Can not get collisiion free configuration!";
01918                 break;
01919         case ERROR_RRT_EXTEND_FAILURE:
01920                 szErrorMsg = "Can not get good extension!";
01921                 break;
01922         case ERROR_START_CONFIG_INVALID:
01923                 szErrorMsg = "The start configuration is not valid!";
01924                 break;
01925         case ERROR_GOAL_CONFIG_INVALID:
01926                 szErrorMsg = "The goal configuration is not valid!";
01927                 break;
01928         case ERROR_SMGRAPH_FULL:
01929                 szErrorMsg = "The Self Motion Graph is FULL!";
01930                 break;
01931         case ERROR_PLANNING_TIMEOUT:
01932                 szErrorMsg = "Time out!";
01933                 break;
01934         default:
01935                 szErrorMsg = "Unknown Message";
01936                 break;
01937         }
01938         //MessageBox(NULL, szErrorMsg, "Motion Planning along End-effect Path", MB_OK);
01939 }
01940 
01941 bool PL_MPEP::DrawExplicit () const
01942 {
01943         // IMPROVE:  Add Open GL code to inser the graph here.
01944         double Xcor, Ycor, Zcor;        // temp coordinate variables
01945 
01946         // Lock Graph for Drawing
01947         Semaphore semaphore( this->guid );
01948         semaphore.Lock();
01949 
01950         // Setup GL Environment
01951         //glClearColor( 0.0f, 0.0f, 0.2f, 1.0f );
01952         glClearColor(1.0f, 1.0f, 1.0f, 0.0f);
01953         glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
01954         BOOL lightingState = glIsEnabled( GL_LIGHTING );
01955         glDisable( GL_LIGHTING );
01956 
01957         // Create Shading Effects that will allow edges to have two end colours.
01958         //  IMPROVE:  Does this change the shading for others.  If so, is there a way
01959         //                    to check for current shading mode and save it.
01960         glShadeModel( GL_SMOOTH );
01961 
01962         // The depth function is set to its default of GL_LESS.  This means that any object closer to the viewport
01963         // will have its colour displayed for that pixel.  This means closer objects will be drawn on top of further
01964         // objects.  Any objects that are in the same plane, will have the colour of the object drawn first.
01965 
01966         // Draw all NODES first, as these are to be seen on top of any edges.
01967         glPointSize(5.0f);      // Set size of points associated with the nodes.
01968 
01969         if (vertices.size() )
01970         {
01971                 int i;
01972 
01973                 //Draw Self-motion graph
01974                 glBegin(GL_LINES);
01975                 for(i = 1; i<vertices.size(); i++)
01976                 {
01977                         //here node is data type for graph.
01978                         vertex vert;
01979                         Pose_Node *smgNode;
01980                         edge e;
01981 
01982                         vert = vertices[i];
01983                         vertex parentVert = ((dynamic_trees &)tree).parent(vert);
01984                         if (parentVert==NULL) //Don't draw standalone vertices;
01985                                 continue;
01986 
01987                         smgNode = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(vert);
01988                         SMGraph *graph = smgNode->graph;
01989                         if (graph==NULL)
01990                                 continue;
01991                         GRAPH<Configuration *, double> *smgGraph = graph->GetSMGraph();
01992                         forall_edges(e, *smgGraph)
01993                         {
01994                                 Configuration *conf1, *conf2;
01995                                 conf1 = conf2 = NULL;
01996                                 graph->GetConfig(e, &conf1, &conf2);
01997 
01998                                 ASSERT((conf1!=NULL) && (conf2!=NULL));
01999 
02000                                 Xcor = (*conf1)[0];
02001                                 Ycor = (*conf1)[1];
02002                                 Zcor = (*conf1)[2];
02003                                 glColor3f(0.0f,0.0f,1.0f);      // set colour to green
02004                                 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line
02005 
02006                                 Xcor = (*conf2)[0];
02007                                 Ycor = (*conf2)[1];
02008                                 Zcor = (*conf2)[2];
02009                                 glColor3f(0.0f,0.0f,1.0f);      // set colour to green
02010                                 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line
02011 
02012                         }
02013                 }
02014                 glEnd();
02015 
02016                 //const dynamic_trees *ptrTree = &tree;
02017                 glBegin(GL_POINTS);
02018                 for(i = 0; i<vertices.size(); i++)
02019                 {
02020                         vertex vert;
02021                         Pose_Node *node;
02022                         vert = vertices[i];
02023                         vertex parentVert = ((dynamic_trees &)tree).parent(vert);
02024                         if (parentVert==NULL) //Don't draw standalone vertices;
02025                                 continue;
02026                         node = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(vert);
02027                         Configuration conf = node->conf;
02028 
02029                         Xcor = conf[0];
02030                         Ycor = conf[1];
02031                         Zcor = conf[2];
02032 
02033                         if ((i==0) || (i==(vertices.size()-1)))
02034                                 glColor3f(0.0f,0.0f,1.0f);    //Set the start and end to be blue
02035                         else
02036                                 glColor3f(1.0f,1.0f,0.0f);              // set colour to yellow
02037                         glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02038                 }
02039                 glEnd();
02040 
02041                 glBegin(GL_LINES);
02042                 for(i = 1; i<vertices.size(); i++)
02043                 {
02044                         vertex vert, parentVert;
02045                         Pose_Node *node, *parentNode;
02046 
02047                         vert = vertices[i];
02048                         parentVert = ((dynamic_trees &)tree).parent(vert);
02049                         if (parentVert==NULL) 
02050                                 continue;
02051                         node = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(vert);
02052                         parentNode = (Pose_Node *)((dynamic_trees &)tree).vertex_inf(parentVert);
02053                         Configuration conf = node->conf;
02054                         Configuration parentConf = parentNode->conf;
02055 
02056                         Xcor = conf[0];
02057                         Ycor = conf[1];
02058                         Zcor = conf[2];
02059                         glColor3f(1.0f,0.5f,0.0f);      // set colour to orange
02060                         glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line
02061 
02062                         Xcor = parentConf[0];
02063                         Ycor = parentConf[1];
02064                         Zcor = parentConf[2];
02065                         glColor3f(1.0f,0.0f,0.0f);      // set colour to red
02066                         glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line
02067                 }
02068                 glEnd();
02069         }
02070 
02071         //draw goal tree if there is
02072         if (usingGoalTree)
02073         {
02074                 if (goal_vertices.size() )
02075                 {
02076                         int i;
02077 
02078                         //const dynamic_trees *ptrTree = &tree;
02079                         glBegin(GL_POINTS);
02080                         for(i = 0; i<goal_vertices.size(); i++)
02081                         {
02082                                 vertex vert;
02083                                 Pose_Node *node;
02084                                 vert = goal_vertices[i];
02085                                 node = (Pose_Node *)((dynamic_trees &)goal_tree).vertex_inf(vert);
02086                                 Configuration conf = node->conf;
02087 
02088                                 Xcor = conf[0];
02089                                 Ycor = conf[1];
02090                                 Zcor = conf[2];
02091 
02092                                 if ((i==0) || (i==(goal_vertices.size()-1)))
02093                                         glColor3f(0.0f,0.0f,1.0f);    //Set the start and end to be blue
02094                                 else
02095                                         glColor3f(1.0f,1.0f,0.0f);              // set colour to yellow
02096                                 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02097                         }
02098                         glEnd();
02099 
02100                         glBegin(GL_LINES);
02101                         for(i = 1; i<goal_vertices.size(); i++)
02102                         {
02103                                 vertex vert, parentVert;
02104                                 Pose_Node *node, *parentNode;
02105 
02106                                 vert = goal_vertices[i];
02107                                 parentVert = ((dynamic_trees &)goal_tree).parent(vert);
02108                                 node = (Pose_Node *)((dynamic_trees &)goal_tree).vertex_inf(vert);
02109                                 parentNode = (Pose_Node *)((dynamic_trees &)goal_tree).vertex_inf(parentVert);
02110                                 Configuration conf = node->conf;
02111                                 Configuration parentConf = parentNode->conf;
02112 
02113                                 Xcor = conf[0];
02114                                 Ycor = conf[1];
02115                                 Zcor = conf[2];
02116                                 glColor3f(1.0f,0.5f,0.0f);      // set colour to orange
02117                                 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add source vertex for edge line
02118 
02119                                 Xcor = parentConf[0];
02120                                 Ycor = parentConf[1];
02121                                 Zcor = parentConf[2];
02122                                 glColor3f(1.0f,0.0f,0.0f);      // set colour to red
02123                                 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );    // add target vertex for edge line
02124                         }
02125                         glEnd();
02126                 }
02127         }//end if (usingGoalTree)
02128 
02129         // Clean up Environment
02130         if( lightingState != FALSE )
02131         {
02132                 glEnable( GL_LIGHTING );
02133         }
02134 
02135         // Unlock Semaphore
02136         semaphore.Unlock();
02137 
02138         return true;
02139 }
02140 

Generated on Sat Apr 1 21:30:43 2006 for Motion Planning Kernel by  doxygen 1.4.6-NO