going places

mxgrey · May 30, 2014 · 86df1fd · 86df1fd
1 parent 5b261b1
commit 86df1fd
Show file tree

Hide file tree

Showing 5 changed files with 123 additions and 26 deletions.
diff --git a/ccrrt/Constraint.h b/ccrrt/Constraint.h
@@ -13,7 +13,8 @@ class Constraint
 
         INVALID = 0,
         AT_RISK,
-        VALID
+        VALID,
+        STUCK
 
     } validity_t;
 

diff --git a/src/Chomper.cpp b/src/Chomper.cpp
@@ -154,9 +154,10 @@ Constraint::validity_t Chomper::iterate(bool quit_if_valid)
     }
     else
     {
-        HAinvHt_inv.setZero();
-        Ainv_Ht_HAinvHt_inv.setZero();
-        std::cout << "singular" << std::endl;
+//        HAinvHt_inv.setZero();
+//        Ainv_Ht_HAinvHt_inv.setZero();
+//        std::cout << "stuck" << std::endl;
+        return Constraint::STUCK;
     }
 
 //    std::cout << "\nHAinvHt_inv:\n" << HAinvHt_inv << std::endl;

diff --git a/src/MultiChomper.cpp b/src/MultiChomper.cpp
@@ -6,7 +6,7 @@ using namespace ccrrt;
 
 MultiChomper::MultiChomper()
 {
-    max_attempts = 10;
+    max_attempts = 5;
 }
 
 bool MultiChomper::run(const Trajectory &trajectory,

diff --git a/test/chomprrt_test.cpp b/test/chomprrt_test.cpp
@@ -5,6 +5,8 @@
 #include "../ccrrt/CircleConstraint.h"
 #include "../ccrrt/Drawer.h"
 
+#include <time.h>
+
 using namespace ccrrt;
 using namespace Eigen;
 
@@ -15,6 +17,17 @@ int main(int argc, char* argv[])
     std::vector<CircleConstraint*> circles;
 
     // Infinitesimal Passage
+//    circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1,2), 2, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1,4.00), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.8,-0.3), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
+
+
+    // Super-Tiny Passage
     circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(1,2), 2, 0.1));
@@ -23,17 +36,41 @@ int main(int argc, char* argv[])
     circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
-
-    // High Narrow Passage
-//    circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(1,2), 2, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(1,3.8), 1, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(2.8,-0.3), 1, 0.1));
+
+    // Multiple Narrow Passages
+//    circles.push_back(new CircleConstraint(Vector2d(-1.85,-0.98), 0.45, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-2.2,-0.55), 0.4, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-2.2,0.2), 0.4, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.8,0.6), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.7,1.2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.5,2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.0,2.5), 0.5, 0.1));
+
+//    circles.push_back(new CircleConstraint(Vector2d(2.4,0.6), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.3,1.2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.05,1.55), 0.3, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.85,2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.1,2.5), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.07,2.89), 0.3, 0.1));
+
+//    circles.push_back(new CircleConstraint(Vector2d(2.80,-0.85), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.05,-0.35), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.50,-0.0), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.40,0.5), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.29,1.15), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.10,1.69), 0.3, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(0.685,1.585), 0.5, 0.1));
+
+//    circles.push_back(new CircleConstraint(Vector2d(0.0,3.2), 0.8, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1,4.2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(3.0,0.25), 0.6, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-0,-2.6), 0.6, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.1,-1.6), 0.6, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
-
+
+
     // Middle Narrow Passage
 //    circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
@@ -43,8 +80,8 @@ int main(int argc, char* argv[])
 //    circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
-    
-    
+
+
     // Simple
 //    circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
@@ -83,6 +120,10 @@ int main(int argc, char* argv[])
     std::cout << "Start" << std::endl;
     RRT_Result_t result = RRT_NOT_FINISHED;
     size_t counter=0;
+
+    clock_t start_time;
+    start_time = clock();
+
     while(RRT_NOT_FINISHED == result)
     {
         ++counter;
@@ -92,8 +133,14 @@ int main(int argc, char* argv[])
 //            break;
     }
 
+    clock_t end_time;
+    end_time = clock();
+
+    double total_time = (end_time-start_time)/(double)(CLOCKS_PER_SEC);
     if(result != RRT_SOLVED)
-        std::cout << "Failure: " << result << std::endl;
+        std::cout << "Failure: " << result << " (" << total_time << ")" <<  std::endl;
+    else
+        std::cout << "Time: " << total_time << std::endl;
 
     std::cout << "Steps: " << counter << std::endl;
 

diff --git a/test/constrained_rrt_test.cpp b/test/constrained_rrt_test.cpp
@@ -4,6 +4,8 @@
 #include "../ccrrt/CircleConstraint.h"
 #include "../ccrrt/Drawer.h"
 
+#include <time.h>
+
 using namespace ccrrt;
 using namespace Eigen;
 
@@ -14,24 +16,60 @@ int main(int argc, char* argv[])
     std::vector<CircleConstraint*> circles;
 
     // Infinitesimal Passage
+//    circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1,2), 2, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1,4.00), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.8,-0.3), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
+
+    // Super-Tiny Passage
     circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(1,2), 2, 0.1));
-    circles.push_back(new CircleConstraint(Vector2d(1,4.00), 1, 0.1));
+    circles.push_back(new CircleConstraint(Vector2d(1,3.99), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(2.8,-0.3), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
     circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
-
-    // Super-Tiny Passage
-//    circles.push_back(new CircleConstraint(Vector2d(0,0), 2, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(-1,2), 1, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(1,2), 2, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(1,3.99), 1, 0.1));
-//    circles.push_back(new CircleConstraint(Vector2d(2.8,-0.3), 1, 0.1));
+
+
+    // Multiple Narrow Passages
+//    circles.push_back(new CircleConstraint(Vector2d(-1.85,-0.98), 0.45, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-2.2,-0.55), 0.4, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-2.2,0.2), 0.4, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.8,0.6), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.7,1.2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.5,2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-1.0,2.5), 0.5, 0.1));
+
+//    circles.push_back(new CircleConstraint(Vector2d(2.4,0.6), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.3,1.2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.05,1.55), 0.3, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.85,2), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.1,2.5), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.07,2.89), 0.3, 0.1));
+
+//    circles.push_back(new CircleConstraint(Vector2d(2.80,-0.85), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.05,-0.35), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.50,-0.0), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.40,0.5), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.29,1.15), 0.5, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1.10,1.69), 0.3, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(0.685,1.585), 0.5, 0.1));
+
+//    circles.push_back(new CircleConstraint(Vector2d(0.0,3.2), 0.8, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(1,4.2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(3.0,0.25), 0.6, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(4.2,-0.3), 1, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(-1,-2), 1, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(-0,-2.6), 0.6, 0.1));
+//    circles.push_back(new CircleConstraint(Vector2d(2.1,-1.6), 0.6, 0.1));
 //    circles.push_back(new CircleConstraint(Vector2d(1.3,-2.3), 1, 0.1));
+
+
 
 
     // Middle Narrow Passage
@@ -72,15 +110,25 @@ int main(int argc, char* argv[])
     std::cout << "Start" << std::endl;
     RRT_Result_t result = RRT_NOT_FINISHED;
     size_t counter=0;
+
+    clock_t start_time;
+    start_time = clock();
+
     while(RRT_NOT_FINISHED == result)
     {
         ++counter;
         result = rrt.growTrees();
     }
 
+    clock_t end_time;
+    end_time = clock();
+
+    double total_time = (end_time-start_time)/(double)(CLOCKS_PER_SEC);
     if(result != RRT_SOLVED)
-        std::cout << "Failure: " << result << std::endl;
-
+        std::cout << "Failure: " << result << " (" << total_time << ")" <<  std::endl;
+    else
+        std::cout << "Time: " << total_time << std::endl;
+
     std::cout << "Steps: " << counter << std::endl;
 
     Drawer draw;
-Original file line number
+Diff line change
@@ Expand Up / @@ -13,7 +13,8 @@ class Constraint @@
             INVALID = 0,
             AT_RISK,
-            VALID
+            VALID,
+            STUCK
         } validity_t;
@@ Expand Down @@