00001
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00049
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
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
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
00114
00115
00116 bool PL_MPEP::Plan()
00117 {
00118
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
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
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
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 }
00250 if ((retry == exploreNum))
00251 {
00252 return ERROR_GET_COLLISION_FREE_FAILURE;
00253 }
00254 }
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
00265
00266
00267 int logRetry=0;
00268 while (true)
00269 {
00270 Frame currentFrame = GetToolFrame(config);
00271 Vector4 currentPos = currentFrame.GetTranslationVector();
00272
00273
00274
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
00289
00290
00291 VectorN endEffOffset(offset[0], offset[1], offset[2]);
00292 Configuration jointOffset;
00293 jointOffset = mJEndInv * endEffOffset * Rad2Deg(1);
00294 config += jointOffset;
00295 }
00296
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
00325
00326
00327
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
00336
00337
00338 for (int i=0; i<randomRetry; i++)
00339 {
00340
00341 GetRandomConfiguration(randConf);
00342
00343
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
00372 return ERROR_GET_COLLISION_FREE_FAILURE;
00373 }
00374
00375
00376 nodeInGraph = g->AddConnection(nodeInGraph, newConf);
00377 (*nextConf) = g->GetConfiguration(nodeInGraph);
00378
00379
00380
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
00388
00389 return ERROR_OK;
00390 }
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
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
00424
00425
00426
00427 Configuration randConf, activeConf, newConf;
00428 Configuration *nearConf;
00429 bool bFind = false;
00430
00431
00432
00433 for (int i=0; i<randomRetry; i++)
00434 {
00435 GetRandomConfiguration(randConf);
00436
00437
00438 nodeInGraph = GetNearestNodeInGraph(g, &randConf);
00439 nearConf = g->GetConfiguration(nodeInGraph);
00440
00441 if (!GetConfigurationByDirection(activeConf, *(nearConf), randConf))
00442 continue;
00443
00444
00445
00446 if (!redundant.GetConfigurationByActive(newConf, activeConf, *nearConf,
00447 poses[node->poses], distTolerance))
00448
00449
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
00482
00483
00484
00485
00486
00487
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
00507
00508
00509
00510
00511
00512
00513
00514
00515 }
00516 else
00517 {
00518 result = ERROR_SMGRAPH_FULL;
00519 }
00520 return result;
00521 }
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
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 }
00592
00593 if (!bFind)
00594 return ERROR_GET_COLLISION_FREE_FAILURE;
00595 else
00596 return ERROR_OK;
00597 }
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
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
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
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
00660
00661
00662
00663
00664
00665
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
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688 walkinotherdir = true;
00689 }
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
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
00710
00711
00712
00713
00714
00715
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
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738 walkinotherdir = true;
00739 }
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 }
00750
00751 if (! connected)
00752 return ERROR_RRT_EXTEND_FAILURE;
00753
00754 RetrievePathFromTree();
00755 return ERROR_OK;
00756 }
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769
00770
00771
00772
00773
00774
00775
00776
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
00810
00811 walkinotherdir = false;
00812 while ((!walkinotherdir) && (!connected))
00813 {
00814
00815
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
00831
00832 result = Establish_Self_Motion_Graph(tree, extendedVert);
00833 if (result != ERROR_OK)
00834 walkinotherdir = true;
00835 }
00836 }
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 }
00850
00851
00852
00853 if (! connected)
00854 return ERROR_RRT_EXTEND_FAILURE;
00855
00856 RetrievePathFromTree();
00857 return ERROR_OK;
00858 }
00859
00860
00861
00862
00863
00864
00865
00866
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
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
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
00940
00941 if (result==ERROR_OK)
00942 {
00943 goalPoseVertex = extendedVert;
00944 RetrievePathFromTree();
00945 }
00946
00947 return result;
00948 }
00949
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959
00960
00961
00962
00963
00964
00965
00966
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
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031
01032
01033
01034
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
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084
01085
01086
01087
01088
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
01127
01128
01129
01130
01131
01132
01133
01134
01135
01136
01137
01138
01139
01140
01141
01142
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
01185
01186
01187
01188
01189
01190
01191
01192
01193
01194
01195
01196
01197
01198
01199
01200
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
01251
01252
01253
01254
01255
01256
01257
01258
01259
01260
01261
01262
01263
01264
01265
01266
01267
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
01328 Pose_Node newNode;
01329 Pose_Node *nearPose;
01330
01331 bool bFind = false;
01332
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
01343
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
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 }
01403 if (retry == exploreNum)
01404 {
01405
01406 return ERROR_GET_COLLISION_FREE_FAILURE;
01407 }
01408
01409 i += dir;
01410 }
01411
01412 return ERROR_OK;
01413 }
01414
01415
01416
01417
01418
01419
01420 vertex PL_MPEP::GetNearestNodeInTree(dynamic_trees &tr, std::vector<vertex> &vert, Configuration &conf)
01421 {
01422
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
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
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
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
01533
01534
01535 while (!redundant.GetRandomConfiguration(firstNode.conf, poses[_LAST_POSE]));
01536
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
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
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;
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
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
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
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
01939 }
01940
01941 bool PL_MPEP::DrawExplicit () const
01942 {
01943
01944 double Xcor, Ycor, Zcor;
01945
01946
01947 Semaphore semaphore( this->guid );
01948 semaphore.Lock();
01949
01950
01951
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
01958
01959
01960 glShadeModel( GL_SMOOTH );
01961
01962
01963
01964
01965
01966
01967 glPointSize(5.0f);
01968
01969 if (vertices.size() )
01970 {
01971 int i;
01972
01973
01974 glBegin(GL_LINES);
01975 for(i = 1; i<vertices.size(); i++)
01976 {
01977
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)
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);
02004 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02005
02006 Xcor = (*conf2)[0];
02007 Ycor = (*conf2)[1];
02008 Zcor = (*conf2)[2];
02009 glColor3f(0.0f,0.0f,1.0f);
02010 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02011
02012 }
02013 }
02014 glEnd();
02015
02016
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)
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);
02035 else
02036 glColor3f(1.0f,1.0f,0.0f);
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);
02060 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02061
02062 Xcor = parentConf[0];
02063 Ycor = parentConf[1];
02064 Zcor = parentConf[2];
02065 glColor3f(1.0f,0.0f,0.0f);
02066 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02067 }
02068 glEnd();
02069 }
02070
02071
02072 if (usingGoalTree)
02073 {
02074 if (goal_vertices.size() )
02075 {
02076 int i;
02077
02078
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);
02094 else
02095 glColor3f(1.0f,1.0f,0.0f);
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);
02117 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02118
02119 Xcor = parentConf[0];
02120 Ycor = parentConf[1];
02121 Zcor = parentConf[2];
02122 glColor3f(1.0f,0.0f,0.0f);
02123 glVertex3f( (float)Xcor, (float)Ycor, (float)Zcor );
02124 }
02125 glEnd();
02126 }
02127 }
02128
02129
02130 if( lightingState != FALSE )
02131 {
02132 glEnable( GL_LIGHTING );
02133 }
02134
02135
02136 semaphore.Unlock();
02137
02138 return true;
02139 }
02140