diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e44c1f..b298fb7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,9 +12,13 @@ list(APPEND CATKIN_SOURCES
 )
 
 find_package(catkin REQUIRED COMPONENTS ${CATKIN_SOURCES})
+
 #Octomap is not a a catkin package, a.k.a. it has to be found like this
 find_package(octomap)
 
+#We need boost to build succesfully
+find_package(Boost REQUIRED filesystem serialization)
+
 # needed to convert octomaps
 SET(OpenGL_GL_PREFERENCE "GLVND")
 find_package(gl_depth_sim)
@@ -51,6 +55,7 @@ list(APPEND LIBRARIES
   gl_depth_sim::glad
   gl_depth_sim::gl_depth_sim_interfaces
   yaml-cpp
+  Boost::serialization 
   )
 
 
diff --git a/configs/ompl/ompl_planning_baxter.yaml b/configs/ompl/ompl_planning_baxter.yaml
index ed53087..0d0d785 100644
--- a/configs/ompl/ompl_planning_baxter.yaml
+++ b/configs/ompl/ompl_planning_baxter.yaml
@@ -52,272 +52,13 @@ planner_configs:
     max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
   PRMstarkConfigDefault:
     type: geometric::PRMstar
-  ThunderConfig:
-    type: geometric::RRTConnect
-    range: 0.5
-    experience: "thunder"    
-  BiEST_0_05:
-    type: geometric::BiEST
-    range: 0.05 
-  BiEST_0_25:
-    type: geometric::BiEST
-    range: 0.25 
-  BiEST_0_50:
-    type: geometric::BiEST
-    range: 0.50 
-  BiEST_0_75:
-    type: geometric::BiEST
-    range: 0.75 
-  BiEST_1_00:
-    type: geometric::BiEST
-    range: 1.00 
-  BiEST_1_25:
-    type: geometric::BiEST
-    range: 1.25 
-  BiEST_1_50:
-    type: geometric::BiEST
-    range: 1.50 
-  BiEST_1_75:
-    type: geometric::BiEST
-    range: 1.75 
-  BiEST_2_00:
-    type: geometric::BiEST
-    range: 2.00 
-  BiEST_2_25:
-    type: geometric::BiEST
-    range: 2.25 
-  BiEST_2_50:
-    type: geometric::BiEST
-    range: 2.50 
-  BiEST_2_75:
-    type: geometric::BiEST
-    range: 2.75 
-  BiEST_3_00:
-    type: geometric::BiEST
-    range: 3.00 
-  BiEST_3_25:
-    type: geometric::BiEST
-    range: 3.25 
-  BiEST_3_50:
-    type: geometric::BiEST
-    range: 3.50 
-  BiEST_3_75:
-    type: geometric::BiEST
-    range: 3.75 
-  BiEST_4_00:
-    type: geometric::BiEST
-    range: 4.00 
-  BiEST_4_25:
-    type: geometric::BiEST
-    range: 4.25 
-  BiEST_4_50:
-    type: geometric::BiEST
-    range: 4.50 
-  BiEST_4_75:
-    type: geometric::BiEST
-    range: 4.75 
-  BiEST_5_00:
-    type: geometric::BiEST
-    range: 5.00 
-  BiEST_5_25:
+  BiESTkConfigDefault:
     type: geometric::BiEST
-    range: 5.25 
-  BiEST_5_50:
-    type: geometric::BiEST
-    range: 5.50 
-  BiEST_5_75:
-    type: geometric::BiEST
-    range: 5.75 
-  BiEST_6_00:
-    type: geometric::BiEST
-    range: 6.00 
-  BiEST_6_25:
-    type: geometric::BiEST
-    range: 6.25 
-  BiEST_6_50:
-    type: geometric::BiEST
-    range: 6.50 
-  BiEST_6_75:
-    type: geometric::BiEST
-    range: 6.75 
-  BiEST_7_00:
-    type: geometric::BiEST
-    range: 7.00 
-  BKPIECE_0_05:
-    type: geometric::BKPIECE
-    range: 0.05 
-  BKPIECE_0_25:
-    type: geometric::BKPIECE
-    range: 0.25 
-  BKPIECE_0_50:
-    type: geometric::BKPIECE
-    range: 0.50 
-  BKPIECE_0_75:
-    type: geometric::BKPIECE
-    range: 0.75 
-  BKPIECE_1_00:
-    type: geometric::BKPIECE
-    range: 1.00 
-  BKPIECE_1_25:
-    type: geometric::BKPIECE
-    range: 1.25 
-  BKPIECE_1_50:
-    type: geometric::BKPIECE
-    range: 1.50 
-  BKPIECE_1_75:
-    type: geometric::BKPIECE
-    range: 1.75 
-  BKPIECE_2_00:
-    type: geometric::BKPIECE
-    range: 2.00 
-  BKPIECE_2_25:
-    type: geometric::BKPIECE
-    range: 2.25 
-  BKPIECE_2_50:
-    type: geometric::BKPIECE
-    range: 2.50 
-  BKPIECE_2_75:
-    type: geometric::BKPIECE
-    range: 2.75 
-  BKPIECE_3_00:
-    type: geometric::BKPIECE
-    range: 3.00 
-  BKPIECE_3_25:
-    type: geometric::BKPIECE
-    range: 3.25 
-  BKPIECE_3_50:
-    type: geometric::BKPIECE
-    range: 3.50 
-  BKPIECE_3_75:
-    type: geometric::BKPIECE
-    range: 3.75 
-  BKPIECE_4_00:
-    type: geometric::BKPIECE
-    range: 4.00 
-  BKPIECE_4_25:
-    type: geometric::BKPIECE
-    range: 4.25 
-  BKPIECE_4_50:
-    type: geometric::BKPIECE
-    range: 4.50 
-  BKPIECE_4_75:
-    type: geometric::BKPIECE
-    range: 4.75 
-  BKPIECE_5_00:
-    type: geometric::BKPIECE
-    range: 5.00 
-  BKPIECE_5_25:
-    type: geometric::BKPIECE
-    range: 5.25 
-  BKPIECE_5_50:
-    type: geometric::BKPIECE
-    range: 5.50 
-  BKPIECE_5_75:
-    type: geometric::BKPIECE
-    range: 5.75 
-  BKPIECE_6_00:
-    type: geometric::BKPIECE
-    range: 6.00 
-  BKPIECE_6_25:
-    type: geometric::BKPIECE
-    range: 6.25 
-  BKPIECE_6_50:
-    type: geometric::BKPIECE
-    range: 6.50 
-  BKPIECE_6_75:
-    type: geometric::BKPIECE
-    range: 6.75 
-  BKPIECE_7_00:
-    type: geometric::BKPIECE
-    range: 7.00 
-  RRTConnect_0_05:
-    type: geometric::RRTConnect
-    range: 0.05 
-  RRTConnect_0_25:
-    type: geometric::RRTConnect
-    range: 0.25 
-  RRTConnect_0_50:
-    type: geometric::RRTConnect
-    range: 0.50 
-  RRTConnect_0_75:
-    type: geometric::RRTConnect
-    range: 0.75 
-  RRTConnect_1_00:
-    type: geometric::RRTConnect
-    range: 1.00 
-  RRTConnect_1_25:
-    type: geometric::RRTConnect
-    range: 1.25 
-  RRTConnect_1_50:
-    type: geometric::RRTConnect
-    range: 1.50 
-  RRTConnect_1_75:
-    type: geometric::RRTConnect
-    range: 1.75 
-  RRTConnect_2_00:
-    type: geometric::RRTConnect
-    range: 2.00 
-  RRTConnect_2_25:
-    type: geometric::RRTConnect
-    range: 2.25 
-  RRTConnect_2_50:
-    type: geometric::RRTConnect
-    range: 2.50 
-  RRTConnect_2_75:
-    type: geometric::RRTConnect
-    range: 2.75 
-  RRTConnect_3_00:
-    type: geometric::RRTConnect
-    range: 3.00 
-  RRTConnect_3_25:
-    type: geometric::RRTConnect
-    range: 3.25 
-  RRTConnect_3_50:
-    type: geometric::RRTConnect
-    range: 3.50 
-  RRTConnect_3_75:
-    type: geometric::RRTConnect
-    range: 3.75 
-  RRTConnect_4_00:
-    type: geometric::RRTConnect
-    range: 4.00 
-  RRTConnect_4_25:
-    type: geometric::RRTConnect
-    range: 4.25 
-  RRTConnect_4_50:
-    type: geometric::RRTConnect
-    range: 4.50 
-  RRTConnect_4_75:
-    type: geometric::RRTConnect
-    range: 4.75 
-  RRTConnect_5_00:
-    type: geometric::RRTConnect
-    range: 5.00 
-  RRTConnect_5_25:
-    type: geometric::RRTConnect
-    range: 5.25 
-  RRTConnect_5_50:
-    type: geometric::RRTConnect
-    range: 5.50 
-  RRTConnect_5_75:
-    type: geometric::RRTConnect
-    range: 5.75 
-  RRTConnect_6_00:
-    type: geometric::RRTConnect
-    range: 6.00 
-  RRTConnect_6_25:
-    type: geometric::RRTConnect
-    range: 6.25 
-  RRTConnect_6_50:
-    type: geometric::RRTConnect
-    range: 6.50 
-  RRTConnect_6_75:
-    type: geometric::RRTConnect
-    range: 6.75 
-  RRTConnect_7_00:
-    type: geometric::RRTConnect
-    range: 7.00 
-
+    range: 0.0   # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  BITstarkConfigDefault:
+    type: geometric::BITstar
+  AITstarkConfigDefault:
+    type: geometric::AITstar
 left_arm:
   planner_configs:
     - SBLkConfigDefault
@@ -331,94 +72,9 @@ left_arm:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
-    - ThunderConfig 
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(left_s0,left_s1)
   longest_valid_segment_fraction: 0.005
 right_arm:
@@ -434,94 +90,9 @@ right_arm:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
-    - ThunderConfig 
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(right_s0,right_s1)
   longest_valid_segment_fraction: 0.005
 right_3dof:
@@ -537,6 +108,9 @@ right_3dof:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(right_w0,right_w1)
   longest_valid_segment_fraction: 0.005
 right_4dof:
@@ -552,6 +126,9 @@ right_4dof:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(right_e1,right_w0)
   longest_valid_segment_fraction: 0.005
 right_5dof:
@@ -567,6 +144,9 @@ right_5dof:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(right_e0,right_e1)
   longest_valid_segment_fraction: 0.005
 both_arms:
@@ -582,94 +162,9 @@ both_arms:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
-    - ThunderConfig 
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(left_s0,left_s1)
   longest_valid_segment_fraction: 0.005
 left_hand:
@@ -685,6 +180,9 @@ left_hand:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
 right_hand:
   planner_configs:
     - SBLkConfigDefault
@@ -698,3 +196,6 @@ right_hand:
     - TRRTkConfigDefault
     - PRMkConfigDefault
     - PRMstarkConfigDefault
+    - BiESTkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
diff --git a/configs/ompl/ompl_planning_fetch.yaml b/configs/ompl/ompl_planning_fetch.yaml
index fd5c344..1fabfd5 100644
--- a/configs/ompl/ompl_planning_fetch.yaml
+++ b/configs/ompl/ompl_planning_fetch.yaml
@@ -1,446 +1,47 @@
 planner_configs:
-  BKPIECE_BOOKSHELF_SMALL:
-    type: geometric::BKPIECE
-    range: 0.4209
-  BKPIECE_BOOKSHELF_TALL:
-    type: geometric::BKPIECE
-    range: 0.4727
-  BKPIECE_BOOKSHELF_THIN:
-    type: geometric::BKPIECE
-    range: 0.4635
-  BKPIECE_BOX:
-    type: geometric::BKPIECE
-    range: 0.9723
-  BKPIECE_CAGE:
-    type: geometric::BKPIECE
-    range: 0.3644
-  BKPIECE_KITCHEN:
-    type: geometric::BKPIECE
-    range: 0.5914
-  BKPIECE_TABLE_PICK:
-    type: geometric::BKPIECE
-    range: 0.9257
-  BKPIECE_DEFAULT:
-    type: geometric::BKPIECE
-  RRTConnect_BOOKSHELF_SMALL:
-    type: geometric::RRTConnect
-    range: 0.2142
-  RRTConnect_BOOKSHELF_TALL:
-    type: geometric::RRTConnect
-    range: 0.7611
-  RRTConnect_BOOKSHELF_THIN:
-    type: geometric::RRTConnect
-    range: 0.5080
-  RRTConnect_BOX:
-    type: geometric::RRTConnect
-    range: 4.2123
-  RRTConnect_CAGE:
-    type: geometric::RRTConnect
-    range: 0.2167
-  RRTConnect_KITCHEN:
-    type: geometric::RRTConnect
-    range: 4.2357
-  RRTConnect_TABLE_PICK:
-    type: geometric::RRTConnect
-    range: 2.7199
-  RRTConnect_DEFAULT:
-    type: geometric::RRTConnect
-  BiEST_BOOKSHELF_SMALL:
-    type: geometric::BiEST
-    range: 1.149
-  BiEST_BOOKSHELF_TALL:
-    type: geometric::BiEST
-    range: 1.499 
-  BiEST_BOOKSHELF_THIN:
-    type: geometric::BiEST
-    range: 1.205 
-  BiEST_BOX:
-    type: geometric::BiEST
-    range: 14.157 
-  BiEST_CAGE:
-    type: geometric::BiEST
-    range: 1.4018 
-  BiEST_KITCHEN:
-    type: geometric::BiEST
-    range: 14.248
-  BiEST_TABLE_PICK:
-    type: geometric::BiEST
-    range: 14.116
-  BiEST_DEFAULT:
-    type: geometric::BiEST
-  BiEST_0_05:
-    type: geometric::BiEST
-    range: 0.05 
-  BiEST_0_25:
-    type: geometric::BiEST
-    range: 0.25 
-  BiEST_0_50:
-    type: geometric::BiEST
-    range: 0.50 
-  BiEST_0_75:
-    type: geometric::BiEST
-    range: 0.75 
-  BiEST_1_00:
-    type: geometric::BiEST
-    range: 1.00 
-  BiEST_1_25:
-    type: geometric::BiEST
-    range: 1.25 
-  BiEST_1_50:
-    type: geometric::BiEST
-    range: 1.50 
-  BiEST_1_75:
-    type: geometric::BiEST
-    range: 1.75 
-  BiEST_2_00:
-    type: geometric::BiEST
-    range: 2.00 
-  BiEST_2_25:
-    type: geometric::BiEST
-    range: 2.25 
-  BiEST_2_50:
-    type: geometric::BiEST
-    range: 2.50 
-  BiEST_2_75:
-    type: geometric::BiEST
-    range: 2.75 
-  BiEST_3_00:
-    type: geometric::BiEST
-    range: 3.00 
-  BiEST_3_25:
-    type: geometric::BiEST
-    range: 3.25 
-  BiEST_3_50:
-    type: geometric::BiEST
-    range: 3.50 
-  BiEST_3_75:
-    type: geometric::BiEST
-    range: 3.75 
-  BiEST_4_00:
-    type: geometric::BiEST
-    range: 4.00 
-  BiEST_4_25:
-    type: geometric::BiEST
-    range: 4.25 
-  BiEST_4_50:
-    type: geometric::BiEST
-    range: 4.50 
-  BiEST_4_75:
-    type: geometric::BiEST
-    range: 4.75 
-  BiEST_5_00:
-    type: geometric::BiEST
-    range: 5.00 
-  BiEST_5_25:
-    type: geometric::BiEST
-    range: 5.25 
-  BiEST_5_50:
-    type: geometric::BiEST
-    range: 5.50 
-  BiEST_5_75:
-    type: geometric::BiEST
-    range: 5.75 
-  BiEST_6_00:
-    type: geometric::BiEST
-    range: 6.00 
-  BiEST_6_25:
-    type: geometric::BiEST
-    range: 6.25 
-  BiEST_6_50:
-    type: geometric::BiEST
-    range: 6.50 
-  BiEST_6_75:
-    type: geometric::BiEST
-    range: 6.75 
-  BiEST_7_00:
-    type: geometric::BiEST
-    range: 7.00 
-  BKPIECE_0_05:
-    type: geometric::BKPIECE
-    range: 0.05 
-  BKPIECE_0_25:
-    type: geometric::BKPIECE
-    range: 0.25 
-  BKPIECE_0_50:
-    type: geometric::BKPIECE
-    range: 0.50 
-  BKPIECE_0_75:
-    type: geometric::BKPIECE
-    range: 0.75 
-  BKPIECE_1_00:
-    type: geometric::BKPIECE
-    range: 1.00 
-  BKPIECE_1_25:
-    type: geometric::BKPIECE
-    range: 1.25 
-  BKPIECE_1_50:
-    type: geometric::BKPIECE
-    range: 1.50 
-  BKPIECE_1_75:
-    type: geometric::BKPIECE
-    range: 1.75 
-  BKPIECE_2_00:
-    type: geometric::BKPIECE
-    range: 2.00 
-  BKPIECE_2_25:
-    type: geometric::BKPIECE
-    range: 2.25 
-  BKPIECE_2_50:
-    type: geometric::BKPIECE
-    range: 2.50 
-  BKPIECE_2_75:
-    type: geometric::BKPIECE
-    range: 2.75 
-  BKPIECE_3_00:
-    type: geometric::BKPIECE
-    range: 3.00 
-  BKPIECE_3_25:
-    type: geometric::BKPIECE
-    range: 3.25 
-  BKPIECE_3_50:
-    type: geometric::BKPIECE
-    range: 3.50 
-  BKPIECE_3_75:
-    type: geometric::BKPIECE
-    range: 3.75 
-  BKPIECE_4_00:
-    type: geometric::BKPIECE
-    range: 4.00 
-  BKPIECE_4_25:
-    type: geometric::BKPIECE
-    range: 4.25 
-  BKPIECE_4_50:
-    type: geometric::BKPIECE
-    range: 4.50 
-  BKPIECE_4_75:
-    type: geometric::BKPIECE
-    range: 4.75 
-  BKPIECE_5_00:
-    type: geometric::BKPIECE
-    range: 5.00 
-  BKPIECE_5_25:
-    type: geometric::BKPIECE
-    range: 5.25 
-  BKPIECE_5_50:
-    type: geometric::BKPIECE
-    range: 5.50 
-  BKPIECE_5_75:
-    type: geometric::BKPIECE
-    range: 5.75 
-  BKPIECE_6_00:
-    type: geometric::BKPIECE
-    range: 6.00 
-  BKPIECE_6_25:
-    type: geometric::BKPIECE
-    range: 6.25 
-  BKPIECE_6_50:
-    type: geometric::BKPIECE
-    range: 6.50 
-  BKPIECE_6_75:
-    type: geometric::BKPIECE
-    range: 6.75 
-  BKPIECE_7_00:
-    type: geometric::BKPIECE
-    range: 7.00 
-  RRTConnect_0_05:
-    type: geometric::RRTConnect
-    range: 0.05 
-  RRTConnect_0_25:
-    type: geometric::RRTConnect
-    range: 0.25 
-  RRTConnect_0_50:
-    type: geometric::RRTConnect
-    range: 0.50 
-  RRTConnect_0_75:
-    type: geometric::RRTConnect
-    range: 0.75 
-  RRTConnect_1_00:
-    type: geometric::RRTConnect
-    range: 1.00 
-  RRTConnect_1_25:
-    type: geometric::RRTConnect
-    range: 1.25 
-  RRTConnect_1_50:
-    type: geometric::RRTConnect
-    range: 1.50 
-  RRTConnect_1_75:
-    type: geometric::RRTConnect
-    range: 1.75 
-  RRTConnect_2_00:
-    type: geometric::RRTConnect
-    range: 2.00 
-  RRTConnect_2_25:
-    type: geometric::RRTConnect
-    range: 2.25 
-  RRTConnect_2_50:
-    type: geometric::RRTConnect
-    range: 2.50 
-  RRTConnect_2_75:
-    type: geometric::RRTConnect
-    range: 2.75 
-  RRTConnect_3_00:
-    type: geometric::RRTConnect
-    range: 3.00 
-  RRTConnect_3_25:
-    type: geometric::RRTConnect
-    range: 3.25 
-  RRTConnect_3_50:
-    type: geometric::RRTConnect
-    range: 3.50 
-  RRTConnect_3_75:
-    type: geometric::RRTConnect
-    range: 3.75 
-  RRTConnect_4_00:
-    type: geometric::RRTConnect
-    range: 4.00 
-  RRTConnect_4_25:
-    type: geometric::RRTConnect
-    range: 4.25 
-  RRTConnect_4_50:
-    type: geometric::RRTConnect
-    range: 4.50 
-  RRTConnect_4_75:
-    type: geometric::RRTConnect
-    range: 4.75 
-  RRTConnect_5_00:
-    type: geometric::RRTConnect
-    range: 5.00 
-  RRTConnect_5_25:
-    type: geometric::RRTConnect
-    range: 5.25 
-  RRTConnect_5_50:
-    type: geometric::RRTConnect
-    range: 5.50 
-  RRTConnect_5_75:
-    type: geometric::RRTConnect
-    range: 5.75 
-  RRTConnect_6_00:
-    type: geometric::RRTConnect
-    range: 6.00 
-  RRTConnect_6_25:
-    type: geometric::RRTConnect
-    range: 6.25 
-  RRTConnect_6_50:
-    type: geometric::RRTConnect
-    range: 6.50 
-  RRTConnect_6_75:
-    type: geometric::RRTConnect
-    range: 6.75 
-  RRTConnect_7_00:
-    type: geometric::RRTConnect
-    range: 7.00 
+  SBLkConfigDefault:
+    type: geometric::SBL
+  ESTkConfigDefault:
+    type: geometric::EST
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+  RRTkConfigDefault:
+    type: geometric::RRT
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+  BiESTkConfigDefault:
+    type: geometric::BiEST
+  PRMkConfigDefault:
+    type: geometric::PRM
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+  BITstarkConfigDefault:
+    type: geometric::BITstar
+  AITstarkConfigDefault:
+    type: geometric::AITstar
 arm_with_torso:
   planner_configs:
-    - BKPIECE_BOOKSHELF_SMALL
-    - BKPIECE_BOOKSHELF_TALL
-    - BKPIECE_BOOKSHELF_THIN
-    - BKPIECE_BOX
-    - BKPIECE_CAGE
-    - BKPIECE_KITCHEN
-    - BKPIECE_TABLE_PICK
-    - BKPIECE_DEFAULT
-    - RRTConnect_BOOKSHELF_SMALL
-    - RRTConnect_BOOKSHELF_TALL
-    - RRTConnect_BOOKSHELF_THIN
-    - RRTConnect_BOX
-    - RRTConnect_CAGE
-    - RRTConnect_KITCHEN
-    - RRTConnect_TABLE_PICK
-    - RRTConnect_DEFAULT
-    - BiEST_BOOKSHELF_SMALL
-    - BiEST_BOOKSHELF_TALL
-    - BiEST_BOOKSHELF_THIN
-    - BiEST_BOX
-    - BiEST_CAGE
-    - BiEST_KITCHEN
-    - BiEST_TABLE_PICK
-    - BiEST_DEFAULT
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - BiESTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(torso_lift_joint,shoulder_pan_joint)
-  longest_valid_segment_fraction: 0.005
+  longest_valid_segment_fraction: 0.005 
diff --git a/configs/ompl/ompl_planning_fetch_old.yaml b/configs/ompl/ompl_planning_fetch_old.yaml
new file mode 100644
index 0000000..fd5c344
--- /dev/null
+++ b/configs/ompl/ompl_planning_fetch_old.yaml
@@ -0,0 +1,446 @@
+planner_configs:
+  BKPIECE_BOOKSHELF_SMALL:
+    type: geometric::BKPIECE
+    range: 0.4209
+  BKPIECE_BOOKSHELF_TALL:
+    type: geometric::BKPIECE
+    range: 0.4727
+  BKPIECE_BOOKSHELF_THIN:
+    type: geometric::BKPIECE
+    range: 0.4635
+  BKPIECE_BOX:
+    type: geometric::BKPIECE
+    range: 0.9723
+  BKPIECE_CAGE:
+    type: geometric::BKPIECE
+    range: 0.3644
+  BKPIECE_KITCHEN:
+    type: geometric::BKPIECE
+    range: 0.5914
+  BKPIECE_TABLE_PICK:
+    type: geometric::BKPIECE
+    range: 0.9257
+  BKPIECE_DEFAULT:
+    type: geometric::BKPIECE
+  RRTConnect_BOOKSHELF_SMALL:
+    type: geometric::RRTConnect
+    range: 0.2142
+  RRTConnect_BOOKSHELF_TALL:
+    type: geometric::RRTConnect
+    range: 0.7611
+  RRTConnect_BOOKSHELF_THIN:
+    type: geometric::RRTConnect
+    range: 0.5080
+  RRTConnect_BOX:
+    type: geometric::RRTConnect
+    range: 4.2123
+  RRTConnect_CAGE:
+    type: geometric::RRTConnect
+    range: 0.2167
+  RRTConnect_KITCHEN:
+    type: geometric::RRTConnect
+    range: 4.2357
+  RRTConnect_TABLE_PICK:
+    type: geometric::RRTConnect
+    range: 2.7199
+  RRTConnect_DEFAULT:
+    type: geometric::RRTConnect
+  BiEST_BOOKSHELF_SMALL:
+    type: geometric::BiEST
+    range: 1.149
+  BiEST_BOOKSHELF_TALL:
+    type: geometric::BiEST
+    range: 1.499 
+  BiEST_BOOKSHELF_THIN:
+    type: geometric::BiEST
+    range: 1.205 
+  BiEST_BOX:
+    type: geometric::BiEST
+    range: 14.157 
+  BiEST_CAGE:
+    type: geometric::BiEST
+    range: 1.4018 
+  BiEST_KITCHEN:
+    type: geometric::BiEST
+    range: 14.248
+  BiEST_TABLE_PICK:
+    type: geometric::BiEST
+    range: 14.116
+  BiEST_DEFAULT:
+    type: geometric::BiEST
+  BiEST_0_05:
+    type: geometric::BiEST
+    range: 0.05 
+  BiEST_0_25:
+    type: geometric::BiEST
+    range: 0.25 
+  BiEST_0_50:
+    type: geometric::BiEST
+    range: 0.50 
+  BiEST_0_75:
+    type: geometric::BiEST
+    range: 0.75 
+  BiEST_1_00:
+    type: geometric::BiEST
+    range: 1.00 
+  BiEST_1_25:
+    type: geometric::BiEST
+    range: 1.25 
+  BiEST_1_50:
+    type: geometric::BiEST
+    range: 1.50 
+  BiEST_1_75:
+    type: geometric::BiEST
+    range: 1.75 
+  BiEST_2_00:
+    type: geometric::BiEST
+    range: 2.00 
+  BiEST_2_25:
+    type: geometric::BiEST
+    range: 2.25 
+  BiEST_2_50:
+    type: geometric::BiEST
+    range: 2.50 
+  BiEST_2_75:
+    type: geometric::BiEST
+    range: 2.75 
+  BiEST_3_00:
+    type: geometric::BiEST
+    range: 3.00 
+  BiEST_3_25:
+    type: geometric::BiEST
+    range: 3.25 
+  BiEST_3_50:
+    type: geometric::BiEST
+    range: 3.50 
+  BiEST_3_75:
+    type: geometric::BiEST
+    range: 3.75 
+  BiEST_4_00:
+    type: geometric::BiEST
+    range: 4.00 
+  BiEST_4_25:
+    type: geometric::BiEST
+    range: 4.25 
+  BiEST_4_50:
+    type: geometric::BiEST
+    range: 4.50 
+  BiEST_4_75:
+    type: geometric::BiEST
+    range: 4.75 
+  BiEST_5_00:
+    type: geometric::BiEST
+    range: 5.00 
+  BiEST_5_25:
+    type: geometric::BiEST
+    range: 5.25 
+  BiEST_5_50:
+    type: geometric::BiEST
+    range: 5.50 
+  BiEST_5_75:
+    type: geometric::BiEST
+    range: 5.75 
+  BiEST_6_00:
+    type: geometric::BiEST
+    range: 6.00 
+  BiEST_6_25:
+    type: geometric::BiEST
+    range: 6.25 
+  BiEST_6_50:
+    type: geometric::BiEST
+    range: 6.50 
+  BiEST_6_75:
+    type: geometric::BiEST
+    range: 6.75 
+  BiEST_7_00:
+    type: geometric::BiEST
+    range: 7.00 
+  BKPIECE_0_05:
+    type: geometric::BKPIECE
+    range: 0.05 
+  BKPIECE_0_25:
+    type: geometric::BKPIECE
+    range: 0.25 
+  BKPIECE_0_50:
+    type: geometric::BKPIECE
+    range: 0.50 
+  BKPIECE_0_75:
+    type: geometric::BKPIECE
+    range: 0.75 
+  BKPIECE_1_00:
+    type: geometric::BKPIECE
+    range: 1.00 
+  BKPIECE_1_25:
+    type: geometric::BKPIECE
+    range: 1.25 
+  BKPIECE_1_50:
+    type: geometric::BKPIECE
+    range: 1.50 
+  BKPIECE_1_75:
+    type: geometric::BKPIECE
+    range: 1.75 
+  BKPIECE_2_00:
+    type: geometric::BKPIECE
+    range: 2.00 
+  BKPIECE_2_25:
+    type: geometric::BKPIECE
+    range: 2.25 
+  BKPIECE_2_50:
+    type: geometric::BKPIECE
+    range: 2.50 
+  BKPIECE_2_75:
+    type: geometric::BKPIECE
+    range: 2.75 
+  BKPIECE_3_00:
+    type: geometric::BKPIECE
+    range: 3.00 
+  BKPIECE_3_25:
+    type: geometric::BKPIECE
+    range: 3.25 
+  BKPIECE_3_50:
+    type: geometric::BKPIECE
+    range: 3.50 
+  BKPIECE_3_75:
+    type: geometric::BKPIECE
+    range: 3.75 
+  BKPIECE_4_00:
+    type: geometric::BKPIECE
+    range: 4.00 
+  BKPIECE_4_25:
+    type: geometric::BKPIECE
+    range: 4.25 
+  BKPIECE_4_50:
+    type: geometric::BKPIECE
+    range: 4.50 
+  BKPIECE_4_75:
+    type: geometric::BKPIECE
+    range: 4.75 
+  BKPIECE_5_00:
+    type: geometric::BKPIECE
+    range: 5.00 
+  BKPIECE_5_25:
+    type: geometric::BKPIECE
+    range: 5.25 
+  BKPIECE_5_50:
+    type: geometric::BKPIECE
+    range: 5.50 
+  BKPIECE_5_75:
+    type: geometric::BKPIECE
+    range: 5.75 
+  BKPIECE_6_00:
+    type: geometric::BKPIECE
+    range: 6.00 
+  BKPIECE_6_25:
+    type: geometric::BKPIECE
+    range: 6.25 
+  BKPIECE_6_50:
+    type: geometric::BKPIECE
+    range: 6.50 
+  BKPIECE_6_75:
+    type: geometric::BKPIECE
+    range: 6.75 
+  BKPIECE_7_00:
+    type: geometric::BKPIECE
+    range: 7.00 
+  RRTConnect_0_05:
+    type: geometric::RRTConnect
+    range: 0.05 
+  RRTConnect_0_25:
+    type: geometric::RRTConnect
+    range: 0.25 
+  RRTConnect_0_50:
+    type: geometric::RRTConnect
+    range: 0.50 
+  RRTConnect_0_75:
+    type: geometric::RRTConnect
+    range: 0.75 
+  RRTConnect_1_00:
+    type: geometric::RRTConnect
+    range: 1.00 
+  RRTConnect_1_25:
+    type: geometric::RRTConnect
+    range: 1.25 
+  RRTConnect_1_50:
+    type: geometric::RRTConnect
+    range: 1.50 
+  RRTConnect_1_75:
+    type: geometric::RRTConnect
+    range: 1.75 
+  RRTConnect_2_00:
+    type: geometric::RRTConnect
+    range: 2.00 
+  RRTConnect_2_25:
+    type: geometric::RRTConnect
+    range: 2.25 
+  RRTConnect_2_50:
+    type: geometric::RRTConnect
+    range: 2.50 
+  RRTConnect_2_75:
+    type: geometric::RRTConnect
+    range: 2.75 
+  RRTConnect_3_00:
+    type: geometric::RRTConnect
+    range: 3.00 
+  RRTConnect_3_25:
+    type: geometric::RRTConnect
+    range: 3.25 
+  RRTConnect_3_50:
+    type: geometric::RRTConnect
+    range: 3.50 
+  RRTConnect_3_75:
+    type: geometric::RRTConnect
+    range: 3.75 
+  RRTConnect_4_00:
+    type: geometric::RRTConnect
+    range: 4.00 
+  RRTConnect_4_25:
+    type: geometric::RRTConnect
+    range: 4.25 
+  RRTConnect_4_50:
+    type: geometric::RRTConnect
+    range: 4.50 
+  RRTConnect_4_75:
+    type: geometric::RRTConnect
+    range: 4.75 
+  RRTConnect_5_00:
+    type: geometric::RRTConnect
+    range: 5.00 
+  RRTConnect_5_25:
+    type: geometric::RRTConnect
+    range: 5.25 
+  RRTConnect_5_50:
+    type: geometric::RRTConnect
+    range: 5.50 
+  RRTConnect_5_75:
+    type: geometric::RRTConnect
+    range: 5.75 
+  RRTConnect_6_00:
+    type: geometric::RRTConnect
+    range: 6.00 
+  RRTConnect_6_25:
+    type: geometric::RRTConnect
+    range: 6.25 
+  RRTConnect_6_50:
+    type: geometric::RRTConnect
+    range: 6.50 
+  RRTConnect_6_75:
+    type: geometric::RRTConnect
+    range: 6.75 
+  RRTConnect_7_00:
+    type: geometric::RRTConnect
+    range: 7.00 
+arm_with_torso:
+  planner_configs:
+    - BKPIECE_BOOKSHELF_SMALL
+    - BKPIECE_BOOKSHELF_TALL
+    - BKPIECE_BOOKSHELF_THIN
+    - BKPIECE_BOX
+    - BKPIECE_CAGE
+    - BKPIECE_KITCHEN
+    - BKPIECE_TABLE_PICK
+    - BKPIECE_DEFAULT
+    - RRTConnect_BOOKSHELF_SMALL
+    - RRTConnect_BOOKSHELF_TALL
+    - RRTConnect_BOOKSHELF_THIN
+    - RRTConnect_BOX
+    - RRTConnect_CAGE
+    - RRTConnect_KITCHEN
+    - RRTConnect_TABLE_PICK
+    - RRTConnect_DEFAULT
+    - BiEST_BOOKSHELF_SMALL
+    - BiEST_BOOKSHELF_TALL
+    - BiEST_BOOKSHELF_THIN
+    - BiEST_BOX
+    - BiEST_CAGE
+    - BiEST_KITCHEN
+    - BiEST_TABLE_PICK
+    - BiEST_DEFAULT
+    - RRTConnect_0_05
+    - RRTConnect_0_25
+    - RRTConnect_0_50
+    - RRTConnect_0_75
+    - RRTConnect_1_00
+    - RRTConnect_1_25
+    - RRTConnect_1_50
+    - RRTConnect_1_75
+    - RRTConnect_2_00
+    - RRTConnect_2_25
+    - RRTConnect_2_50
+    - RRTConnect_2_75
+    - RRTConnect_3_00
+    - RRTConnect_3_25
+    - RRTConnect_3_50
+    - RRTConnect_3_75
+    - RRTConnect_4_00
+    - RRTConnect_4_25
+    - RRTConnect_4_50
+    - RRTConnect_4_75
+    - RRTConnect_5_00
+    - RRTConnect_5_25
+    - RRTConnect_5_50
+    - RRTConnect_5_75
+    - RRTConnect_6_00
+    - RRTConnect_6_25
+    - RRTConnect_6_50
+    - RRTConnect_6_75
+    - RRTConnect_7_00
+    - BiEST_0_05
+    - BiEST_0_25
+    - BiEST_0_50
+    - BiEST_0_75
+    - BiEST_1_00
+    - BiEST_1_25
+    - BiEST_1_50
+    - BiEST_1_75
+    - BiEST_2_00
+    - BiEST_2_25
+    - BiEST_2_50
+    - BiEST_2_75
+    - BiEST_3_00
+    - BiEST_3_25
+    - BiEST_3_50
+    - BiEST_3_75
+    - BiEST_4_00
+    - BiEST_4_25
+    - BiEST_4_50
+    - BiEST_4_75
+    - BiEST_5_00
+    - BiEST_5_25
+    - BiEST_5_50
+    - BiEST_5_75
+    - BiEST_6_00
+    - BiEST_6_25
+    - BiEST_6_50
+    - BiEST_6_75
+    - BiEST_7_00
+    - BKPIECE_0_05
+    - BKPIECE_0_25
+    - BKPIECE_0_50
+    - BKPIECE_0_75
+    - BKPIECE_1_00
+    - BKPIECE_1_25
+    - BKPIECE_1_50
+    - BKPIECE_1_75
+    - BKPIECE_2_00
+    - BKPIECE_2_25
+    - BKPIECE_2_50
+    - BKPIECE_2_75
+    - BKPIECE_3_00
+    - BKPIECE_3_25
+    - BKPIECE_3_50
+    - BKPIECE_3_75
+    - BKPIECE_4_00
+    - BKPIECE_4_25
+    - BKPIECE_4_50
+    - BKPIECE_4_75
+    - BKPIECE_5_00
+    - BKPIECE_5_25
+    - BKPIECE_5_50
+    - BKPIECE_5_75
+    - BKPIECE_6_00
+    - BKPIECE_6_25
+    - BKPIECE_6_50
+    - BKPIECE_6_75
+    - BKPIECE_7_00
+  projection_evaluator: joints(torso_lift_joint,shoulder_pan_joint)
+  longest_valid_segment_fraction: 0.005
diff --git a/configs/ompl/ompl_planning_panda.yaml b/configs/ompl/ompl_planning_panda.yaml
index 20df9af..522d3cc 100644
--- a/configs/ompl/ompl_planning_panda.yaml
+++ b/configs/ompl/ompl_planning_panda.yaml
@@ -124,271 +124,6 @@ planner_configs:
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
-  TrajOptDefault:
-    type: geometric::TrajOpt
-  BiEST_0_05:
-    type: geometric::BiEST
-    range: 0.05 
-  BiEST_0_25:
-    type: geometric::BiEST
-    range: 0.25 
-  BiEST_0_50:
-    type: geometric::BiEST
-    range: 0.50 
-  BiEST_0_75:
-    type: geometric::BiEST
-    range: 0.75 
-  BiEST_1_00:
-    type: geometric::BiEST
-    range: 1.00 
-  BiEST_1_25:
-    type: geometric::BiEST
-    range: 1.25 
-  BiEST_1_50:
-    type: geometric::BiEST
-    range: 1.50 
-  BiEST_1_75:
-    type: geometric::BiEST
-    range: 1.75 
-  BiEST_2_00:
-    type: geometric::BiEST
-    range: 2.00 
-  BiEST_2_25:
-    type: geometric::BiEST
-    range: 2.25 
-  BiEST_2_50:
-    type: geometric::BiEST
-    range: 2.50 
-  BiEST_2_75:
-    type: geometric::BiEST
-    range: 2.75 
-  BiEST_3_00:
-    type: geometric::BiEST
-    range: 3.00 
-  BiEST_3_25:
-    type: geometric::BiEST
-    range: 3.25 
-  BiEST_3_50:
-    type: geometric::BiEST
-    range: 3.50 
-  BiEST_3_75:
-    type: geometric::BiEST
-    range: 3.75 
-  BiEST_4_00:
-    type: geometric::BiEST
-    range: 4.00 
-  BiEST_4_25:
-    type: geometric::BiEST
-    range: 4.25 
-  BiEST_4_50:
-    type: geometric::BiEST
-    range: 4.50 
-  BiEST_4_75:
-    type: geometric::BiEST
-    range: 4.75 
-  BiEST_5_00:
-    type: geometric::BiEST
-    range: 5.00 
-  BiEST_5_25:
-    type: geometric::BiEST
-    range: 5.25 
-  BiEST_5_50:
-    type: geometric::BiEST
-    range: 5.50 
-  BiEST_5_75:
-    type: geometric::BiEST
-    range: 5.75 
-  BiEST_6_00:
-    type: geometric::BiEST
-    range: 6.00 
-  BiEST_6_25:
-    type: geometric::BiEST
-    range: 6.25 
-  BiEST_6_50:
-    type: geometric::BiEST
-    range: 6.50 
-  BiEST_6_75:
-    type: geometric::BiEST
-    range: 6.75 
-  BiEST_7_00:
-    type: geometric::BiEST
-    range: 7.00 
-  BKPIECE_0_05:
-    type: geometric::BKPIECE
-    range: 0.05 
-  BKPIECE_0_25:
-    type: geometric::BKPIECE
-    range: 0.25 
-  BKPIECE_0_50:
-    type: geometric::BKPIECE
-    range: 0.50 
-  BKPIECE_0_75:
-    type: geometric::BKPIECE
-    range: 0.75 
-  BKPIECE_1_00:
-    type: geometric::BKPIECE
-    range: 1.00 
-  BKPIECE_1_25:
-    type: geometric::BKPIECE
-    range: 1.25 
-  BKPIECE_1_50:
-    type: geometric::BKPIECE
-    range: 1.50 
-  BKPIECE_1_75:
-    type: geometric::BKPIECE
-    range: 1.75 
-  BKPIECE_2_00:
-    type: geometric::BKPIECE
-    range: 2.00 
-  BKPIECE_2_25:
-    type: geometric::BKPIECE
-    range: 2.25 
-  BKPIECE_2_50:
-    type: geometric::BKPIECE
-    range: 2.50 
-  BKPIECE_2_75:
-    type: geometric::BKPIECE
-    range: 2.75 
-  BKPIECE_3_00:
-    type: geometric::BKPIECE
-    range: 3.00 
-  BKPIECE_3_25:
-    type: geometric::BKPIECE
-    range: 3.25 
-  BKPIECE_3_50:
-    type: geometric::BKPIECE
-    range: 3.50 
-  BKPIECE_3_75:
-    type: geometric::BKPIECE
-    range: 3.75 
-  BKPIECE_4_00:
-    type: geometric::BKPIECE
-    range: 4.00 
-  BKPIECE_4_25:
-    type: geometric::BKPIECE
-    range: 4.25 
-  BKPIECE_4_50:
-    type: geometric::BKPIECE
-    range: 4.50 
-  BKPIECE_4_75:
-    type: geometric::BKPIECE
-    range: 4.75 
-  BKPIECE_5_00:
-    type: geometric::BKPIECE
-    range: 5.00 
-  BKPIECE_5_25:
-    type: geometric::BKPIECE
-    range: 5.25 
-  BKPIECE_5_50:
-    type: geometric::BKPIECE
-    range: 5.50 
-  BKPIECE_5_75:
-    type: geometric::BKPIECE
-    range: 5.75 
-  BKPIECE_6_00:
-    type: geometric::BKPIECE
-    range: 6.00 
-  BKPIECE_6_25:
-    type: geometric::BKPIECE
-    range: 6.25 
-  BKPIECE_6_50:
-    type: geometric::BKPIECE
-    range: 6.50 
-  BKPIECE_6_75:
-    type: geometric::BKPIECE
-    range: 6.75 
-  BKPIECE_7_00:
-    type: geometric::BKPIECE
-    range: 7.00 
-  RRTConnect_0_05:
-    type: geometric::RRTConnect
-    range: 0.05 
-  RRTConnect_0_25:
-    type: geometric::RRTConnect
-    range: 0.25 
-  RRTConnect_0_50:
-    type: geometric::RRTConnect
-    range: 0.50 
-  RRTConnect_0_75:
-    type: geometric::RRTConnect
-    range: 0.75 
-  RRTConnect_1_00:
-    type: geometric::RRTConnect
-    range: 1.00 
-  RRTConnect_1_25:
-    type: geometric::RRTConnect
-    range: 1.25 
-  RRTConnect_1_50:
-    type: geometric::RRTConnect
-    range: 1.50 
-  RRTConnect_1_75:
-    type: geometric::RRTConnect
-    range: 1.75 
-  RRTConnect_2_00:
-    type: geometric::RRTConnect
-    range: 2.00 
-  RRTConnect_2_25:
-    type: geometric::RRTConnect
-    range: 2.25 
-  RRTConnect_2_50:
-    type: geometric::RRTConnect
-    range: 2.50 
-  RRTConnect_2_75:
-    type: geometric::RRTConnect
-    range: 2.75 
-  RRTConnect_3_00:
-    type: geometric::RRTConnect
-    range: 3.00 
-  RRTConnect_3_25:
-    type: geometric::RRTConnect
-    range: 3.25 
-  RRTConnect_3_50:
-    type: geometric::RRTConnect
-    range: 3.50 
-  RRTConnect_3_75:
-    type: geometric::RRTConnect
-    range: 3.75 
-  RRTConnect_4_00:
-    type: geometric::RRTConnect
-    range: 4.00 
-  RRTConnect_4_25:
-    type: geometric::RRTConnect
-    range: 4.25 
-  RRTConnect_4_50:
-    type: geometric::RRTConnect
-    range: 4.50 
-  RRTConnect_4_75:
-    type: geometric::RRTConnect
-    range: 4.75 
-  RRTConnect_5_00:
-    type: geometric::RRTConnect
-    range: 5.00 
-  RRTConnect_5_25:
-    type: geometric::RRTConnect
-    range: 5.25 
-  RRTConnect_5_50:
-    type: geometric::RRTConnect
-    range: 5.50 
-  RRTConnect_5_75:
-    type: geometric::RRTConnect
-    range: 5.75 
-  RRTConnect_6_00:
-    type: geometric::RRTConnect
-    range: 6.00 
-  RRTConnect_6_25:
-    type: geometric::RRTConnect
-    range: 6.25 
-  RRTConnect_6_50:
-    type: geometric::RRTConnect
-    range: 6.50 
-  RRTConnect_6_75:
-    type: geometric::RRTConnect
-    range: 6.75 
-  RRTConnect_7_00:
-    type: geometric::RRTConnect
-    range: 7.00 
-
-
 panda_arm:
   planner_configs:
     - SBLkConfigDefault
@@ -417,93 +152,6 @@ panda_arm:
     - TrajOptDefault
     - BITstarkConfigDefault
     - AITstarkConfigDefault
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
   projection_evaluator: link(panda_link8)
 panda_arm_hand:
   planner_configs:
@@ -530,96 +178,8 @@ panda_arm_hand:
     - LazyPRMstarkConfigDefault
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
-    - TrajOptDefault
     - BITstarkConfigDefault
     - AITstarkConfigDefault
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
   projection_evaluator: link(panda_link8)
 hand:
   planner_configs:
diff --git a/configs/ompl/ompl_planning_shadow.yaml b/configs/ompl/ompl_planning_shadow.yaml
index 44813f4..a6e7ebb 100644
--- a/configs/ompl/ompl_planning_shadow.yaml
+++ b/configs/ompl/ompl_planning_shadow.yaml
@@ -120,8 +120,10 @@ planner_configs:
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
-  projection_evaluator: joints(FFJ1)
-  longest_valid_segment_fraction: 0.005
+  BITstarkConfigDefault:
+    type: geometric::BITstar
+  AITstarkConfigDefault:
+    type: geometric::AITstar
 all_fingers:
   planner_configs:
     - SBLkConfigDefault
@@ -147,9 +149,11 @@ all_fingers:
     - LazyPRMstarkConfigDefault
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(FFJ1)
   longest_valid_segment_fraction: 0.005
-ff_finger:
+finger_ff:
   planner_configs:
     - SBLkConfigDefault
     - ESTkConfigDefault
@@ -174,7 +178,7 @@ ff_finger:
     - LazyPRMstarkConfigDefault
     - SPARSkConfigDefault
     - SPARStwokConfigDefault
+    - BITstarkConfigDefault
+    - AITstarkConfigDefault
   projection_evaluator: joints(FFJ1)
   longest_valid_segment_fraction: 0.005
-
-
diff --git a/configs/ompl/ompl_planning_ur5.yaml b/configs/ompl/ompl_planning_ur5.yaml
index 9f7f07b..4b2a33e 100644
--- a/configs/ompl/ompl_planning_ur5.yaml
+++ b/configs/ompl/ompl_planning_ur5.yaml
@@ -46,12 +46,6 @@ planner_configs:
     type: geometric::BITstar
   AITstarkConfigDefault:
     type: geometric::AITstar
-  ThunderConfig:
-    type: geometric::RRTConnect
-    experience: "thunder"    
-  LightningConfig:
-    type: geometric::RRTConnect
-    experience: "lightning"    
   TRRTkConfigDefault:
     type: geometric::TRRT
     range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
@@ -136,267 +130,6 @@ planner_configs:
     sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
     dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
     max_failures: 5000  # maximum consecutive failure limit. default: 5000
-  BiEST_0_05:
-    type: geometric::BiEST
-    range: 0.05 
-  BiEST_0_25:
-    type: geometric::BiEST
-    range: 0.25 
-  BiEST_0_50:
-    type: geometric::BiEST
-    range: 0.50 
-  BiEST_0_75:
-    type: geometric::BiEST
-    range: 0.75 
-  BiEST_1_00:
-    type: geometric::BiEST
-    range: 1.00 
-  BiEST_1_25:
-    type: geometric::BiEST
-    range: 1.25 
-  BiEST_1_50:
-    type: geometric::BiEST
-    range: 1.50 
-  BiEST_1_75:
-    type: geometric::BiEST
-    range: 1.75 
-  BiEST_2_00:
-    type: geometric::BiEST
-    range: 2.00 
-  BiEST_2_25:
-    type: geometric::BiEST
-    range: 2.25 
-  BiEST_2_50:
-    type: geometric::BiEST
-    range: 2.50 
-  BiEST_2_75:
-    type: geometric::BiEST
-    range: 2.75 
-  BiEST_3_00:
-    type: geometric::BiEST
-    range: 3.00 
-  BiEST_3_25:
-    type: geometric::BiEST
-    range: 3.25 
-  BiEST_3_50:
-    type: geometric::BiEST
-    range: 3.50 
-  BiEST_3_75:
-    type: geometric::BiEST
-    range: 3.75 
-  BiEST_4_00:
-    type: geometric::BiEST
-    range: 4.00 
-  BiEST_4_25:
-    type: geometric::BiEST
-    range: 4.25 
-  BiEST_4_50:
-    type: geometric::BiEST
-    range: 4.50 
-  BiEST_4_75:
-    type: geometric::BiEST
-    range: 4.75 
-  BiEST_5_00:
-    type: geometric::BiEST
-    range: 5.00 
-  BiEST_5_25:
-    type: geometric::BiEST
-    range: 5.25 
-  BiEST_5_50:
-    type: geometric::BiEST
-    range: 5.50 
-  BiEST_5_75:
-    type: geometric::BiEST
-    range: 5.75 
-  BiEST_6_00:
-    type: geometric::BiEST
-    range: 6.00 
-  BiEST_6_25:
-    type: geometric::BiEST
-    range: 6.25 
-  BiEST_6_50:
-    type: geometric::BiEST
-    range: 6.50 
-  BiEST_6_75:
-    type: geometric::BiEST
-    range: 6.75 
-  BiEST_7_00:
-    type: geometric::BiEST
-    range: 7.00 
-  BKPIECE_0_05:
-    type: geometric::BKPIECE
-    range: 0.05 
-  BKPIECE_0_25:
-    type: geometric::BKPIECE
-    range: 0.25 
-  BKPIECE_0_50:
-    type: geometric::BKPIECE
-    range: 0.50 
-  BKPIECE_0_75:
-    type: geometric::BKPIECE
-    range: 0.75 
-  BKPIECE_1_00:
-    type: geometric::BKPIECE
-    range: 1.00 
-  BKPIECE_1_25:
-    type: geometric::BKPIECE
-    range: 1.25 
-  BKPIECE_1_50:
-    type: geometric::BKPIECE
-    range: 1.50 
-  BKPIECE_1_75:
-    type: geometric::BKPIECE
-    range: 1.75 
-  BKPIECE_2_00:
-    type: geometric::BKPIECE
-    range: 2.00 
-  BKPIECE_2_25:
-    type: geometric::BKPIECE
-    range: 2.25 
-  BKPIECE_2_50:
-    type: geometric::BKPIECE
-    range: 2.50 
-  BKPIECE_2_75:
-    type: geometric::BKPIECE
-    range: 2.75 
-  BKPIECE_3_00:
-    type: geometric::BKPIECE
-    range: 3.00 
-  BKPIECE_3_25:
-    type: geometric::BKPIECE
-    range: 3.25 
-  BKPIECE_3_50:
-    type: geometric::BKPIECE
-    range: 3.50 
-  BKPIECE_3_75:
-    type: geometric::BKPIECE
-    range: 3.75 
-  BKPIECE_4_00:
-    type: geometric::BKPIECE
-    range: 4.00 
-  BKPIECE_4_25:
-    type: geometric::BKPIECE
-    range: 4.25 
-  BKPIECE_4_50:
-    type: geometric::BKPIECE
-    range: 4.50 
-  BKPIECE_4_75:
-    type: geometric::BKPIECE
-    range: 4.75 
-  BKPIECE_5_00:
-    type: geometric::BKPIECE
-    range: 5.00 
-  BKPIECE_5_25:
-    type: geometric::BKPIECE
-    range: 5.25 
-  BKPIECE_5_50:
-    type: geometric::BKPIECE
-    range: 5.50 
-  BKPIECE_5_75:
-    type: geometric::BKPIECE
-    range: 5.75 
-  BKPIECE_6_00:
-    type: geometric::BKPIECE
-    range: 6.00 
-  BKPIECE_6_25:
-    type: geometric::BKPIECE
-    range: 6.25 
-  BKPIECE_6_50:
-    type: geometric::BKPIECE
-    range: 6.50 
-  BKPIECE_6_75:
-    type: geometric::BKPIECE
-    range: 6.75 
-  BKPIECE_7_00:
-    type: geometric::BKPIECE
-    range: 7.00 
-  RRTConnect_0_05:
-    type: geometric::RRTConnect
-    range: 0.05 
-  RRTConnect_0_25:
-    type: geometric::RRTConnect
-    range: 0.25 
-  RRTConnect_0_50:
-    type: geometric::RRTConnect
-    range: 0.50 
-  RRTConnect_0_75:
-    type: geometric::RRTConnect
-    range: 0.75 
-  RRTConnect_1_00:
-    type: geometric::RRTConnect
-    range: 1.00 
-  RRTConnect_1_25:
-    type: geometric::RRTConnect
-    range: 1.25 
-  RRTConnect_1_50:
-    type: geometric::RRTConnect
-    range: 1.50 
-  RRTConnect_1_75:
-    type: geometric::RRTConnect
-    range: 1.75 
-  RRTConnect_2_00:
-    type: geometric::RRTConnect
-    range: 2.00 
-  RRTConnect_2_25:
-    type: geometric::RRTConnect
-    range: 2.25 
-  RRTConnect_2_50:
-    type: geometric::RRTConnect
-    range: 2.50 
-  RRTConnect_2_75:
-    type: geometric::RRTConnect
-    range: 2.75 
-  RRTConnect_3_00:
-    type: geometric::RRTConnect
-    range: 3.00 
-  RRTConnect_3_25:
-    type: geometric::RRTConnect
-    range: 3.25 
-  RRTConnect_3_50:
-    type: geometric::RRTConnect
-    range: 3.50 
-  RRTConnect_3_75:
-    type: geometric::RRTConnect
-    range: 3.75 
-  RRTConnect_4_00:
-    type: geometric::RRTConnect
-    range: 4.00 
-  RRTConnect_4_25:
-    type: geometric::RRTConnect
-    range: 4.25 
-  RRTConnect_4_50:
-    type: geometric::RRTConnect
-    range: 4.50 
-  RRTConnect_4_75:
-    type: geometric::RRTConnect
-    range: 4.75 
-  RRTConnect_5_00:
-    type: geometric::RRTConnect
-    range: 5.00 
-  RRTConnect_5_25:
-    type: geometric::RRTConnect
-    range: 5.25 
-  RRTConnect_5_50:
-    type: geometric::RRTConnect
-    range: 5.50 
-  RRTConnect_5_75:
-    type: geometric::RRTConnect
-    range: 5.75 
-  RRTConnect_6_00:
-    type: geometric::RRTConnect
-    range: 6.00 
-  RRTConnect_6_25:
-    type: geometric::RRTConnect
-    range: 6.25 
-  RRTConnect_6_50:
-    type: geometric::RRTConnect
-    range: 6.50 
-  RRTConnect_6_75:
-    type: geometric::RRTConnect
-    range: 6.75 
-  RRTConnect_7_00:
-    type: geometric::RRTConnect
-    range: 7.00 
 manipulator:
   planner_configs:
     - SBLkConfigDefault
@@ -427,94 +160,5 @@ manipulator:
     - SPARStwokConfigDefault
     - BITstarkConfigDefault
     - AITstarkConfigDefault
-    - ThunderConfig 
-    - LightningConfig 
-    - RRTConnect_0_05
-    - RRTConnect_0_25
-    - RRTConnect_0_50
-    - RRTConnect_0_75
-    - RRTConnect_1_00
-    - RRTConnect_1_25
-    - RRTConnect_1_50
-    - RRTConnect_1_75
-    - RRTConnect_2_00
-    - RRTConnect_2_25
-    - RRTConnect_2_50
-    - RRTConnect_2_75
-    - RRTConnect_3_00
-    - RRTConnect_3_25
-    - RRTConnect_3_50
-    - RRTConnect_3_75
-    - RRTConnect_4_00
-    - RRTConnect_4_25
-    - RRTConnect_4_50
-    - RRTConnect_4_75
-    - RRTConnect_5_00
-    - RRTConnect_5_25
-    - RRTConnect_5_50
-    - RRTConnect_5_75
-    - RRTConnect_6_00
-    - RRTConnect_6_25
-    - RRTConnect_6_50
-    - RRTConnect_6_75
-    - RRTConnect_7_00
-    - BiEST_0_05
-    - BiEST_0_25
-    - BiEST_0_50
-    - BiEST_0_75
-    - BiEST_1_00
-    - BiEST_1_25
-    - BiEST_1_50
-    - BiEST_1_75
-    - BiEST_2_00
-    - BiEST_2_25
-    - BiEST_2_50
-    - BiEST_2_75
-    - BiEST_3_00
-    - BiEST_3_25
-    - BiEST_3_50
-    - BiEST_3_75
-    - BiEST_4_00
-    - BiEST_4_25
-    - BiEST_4_50
-    - BiEST_4_75
-    - BiEST_5_00
-    - BiEST_5_25
-    - BiEST_5_50
-    - BiEST_5_75
-    - BiEST_6_00
-    - BiEST_6_25
-    - BiEST_6_50
-    - BiEST_6_75
-    - BiEST_7_00
-    - BKPIECE_0_05
-    - BKPIECE_0_25
-    - BKPIECE_0_50
-    - BKPIECE_0_75
-    - BKPIECE_1_00
-    - BKPIECE_1_25
-    - BKPIECE_1_50
-    - BKPIECE_1_75
-    - BKPIECE_2_00
-    - BKPIECE_2_25
-    - BKPIECE_2_50
-    - BKPIECE_2_75
-    - BKPIECE_3_00
-    - BKPIECE_3_25
-    - BKPIECE_3_50
-    - BKPIECE_3_75
-    - BKPIECE_4_00
-    - BKPIECE_4_25
-    - BKPIECE_4_50
-    - BKPIECE_4_75
-    - BKPIECE_5_00
-    - BKPIECE_5_25
-    - BKPIECE_5_50
-    - BKPIECE_5_75
-    - BKPIECE_6_00
-    - BKPIECE_6_25
-    - BKPIECE_6_50
-    - BKPIECE_6_75
-    - BKPIECE_7_00
   projection_evaluator: link(ee_link)
   longest_valid_segment_fraction: 0.001
diff --git a/docker/Dockerfile b/docker/Dockerfile
index b6b7707..9092edd 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -17,16 +17,20 @@ RUN apt-get update && \
 ENV NVIDIA_VISIBLE_DEVICES all
 ENV NVIDIA_DRIVER_CAPABILITIES all
 
-# Build dependencies
+# Create catkin workspace 
 WORKDIR $ROS_UNDERLAY/..
+RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release 
+
+# Copy the current motion benchmaker files to the workspace
+RUN mkdir -p ./src/motion_bench_maker 
+ADD ./ ./src/motion_bench_maker/
 
 # Clone MotionBenchMaker and dependencies
-RUN git clone --depth 1 https://github.com/KavrakiLab/motion_bench_maker ./src/motion_bench_maker && \
-    git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \
+RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \
     rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \
     git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \
     git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \
+    git clone --depth 1 https://github.com/TAMS-Group/bio_ik ./src/bio_ik && \
     rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ 
-    catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release && \
     catkin build --limit-status-rate 0.001 --no-notify && \
     sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh
diff --git a/docker/build-docker.sh b/docker/build-docker.sh
index 0c0f706..3c9fa92 100755
--- a/docker/build-docker.sh
+++ b/docker/build-docker.sh
@@ -1,4 +1,4 @@
 #!/bin/bash
 
 cd "${0%/*}"
-docker build -t motion_bench_maker .
+docker build -f Dockerfile -t motion_bench_maker ../
diff --git a/docker/noetic-src/Dockerfile b/docker/noetic-src/Dockerfile
new file mode 100644
index 0000000..fb40a6e
--- /dev/null
+++ b/docker/noetic-src/Dockerfile
@@ -0,0 +1,51 @@
+FROM ros:noetic-ros-base
+MAINTAINER Constantinos Chamzas chamzas@rice.edu
+
+ENV ROS_UNDERLAY /ws/devel
+ENV CATKIN_WS $(realpath $ROS_UNDERLAY/..)
+
+# Dependencies
+RUN apt-get update && \
+    DEBIAN_FRONTEND=noninteractive apt-get install -y build-essential git python3-catkin-tools libhdf5-dev libpcl-dev zip wget libboost-all-dev libboost-serialization-dev libeigen3-dev libtinyxml2-dev libyaml-cpp-dev libhdf5-dev \
+    ros-${ROS_DISTRO}-eigen-conversions \
+    ros-${ROS_DISTRO}-tf-conversions \
+    ros-${ROS_DISTRO}-pcl-conversions \
+    ros-${ROS_DISTRO}-fcl
+
+# Needed to generate pointclouds 
+ENV NVIDIA_VISIBLE_DEVICES all
+ENV NVIDIA_DRIVER_CAPABILITIES all
+
+# Create catkin workspace 
+WORKDIR $ROS_UNDERLAY/..
+RUN catkin config --extend /opt/ros/$ROS_DISTRO --cmake-args -DCMAKE_BUILD_TYPE=Release 
+
+# Get & Install OMPL
+WORKDIR /ws/src
+RUN wget -O - https://github.com/ompl/ompl/archive/1.5.0.tar.gz | tar zxf -
+RUN catkin build ompl --start-with ompl --limit-status-rate 0.001 --no-notify
+
+# Get & Install MoveIt from Source
+WORKDIR /ws
+RUN wstool init src && \
+    wstool merge -t src https://raw.githubusercontent.com/ros-planning/moveit/noetic-devel/moveit.rosinstall && \
+    wstool update -t src && \
+    rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} && \
+    rm -r ./src/moveit/ && \  
+    git clone -b noetic-devel-add-bitstar-aitstar --depth 1 https://github.com/ChamzasKonstantinos/moveit ./src/moveit 
+
+RUN catkin build --limit-status-rate 0.001 --no-notify
+
+# Copy the current motion benchmaker files to the workspace
+RUN mkdir -p ./src/motion_bench_maker 
+ADD ./ ./src/motion_bench_maker/
+
+# Clone MotionBenchMaker and dependencies
+RUN git clone --depth 1 https://github.com/KavrakiLab/robowflex.git ./src/robowflex && \
+    rm -r ./src/robowflex/robowflex_dart ./src/robowflex/robowflex_tesseract ./src/robowflex/robowflex_visualization && \
+    git clone --depth 1 https://github.com/KavrakiLab/robowflex_resources ./src/robowflex_resources && \
+    git clone --depth 1 https://github.com/KavrakiLab/gl_depth_sim ./src/gl_depth_sim && \
+    git clone --depth 1 https://github.com/TAMS-Group/bio_ik ./src/bio_ik && \
+    rm -r ./src/gl_depth_sim/roscpp_gl_depth_sim_demos && \ 
+    catkin build --limit-status-rate 0.001 --no-notify && \
+    sed -i "s#/opt/ros/\$ROS_DISTRO/setup.bash#$ROS_UNDERLAY/setup.bash#g" /ros_entrypoint.sh
diff --git a/docker/noetic-src/build-docker.sh b/docker/noetic-src/build-docker.sh
new file mode 100755
index 0000000..649be8b
--- /dev/null
+++ b/docker/noetic-src/build-docker.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+
+cd "${0%/*}"
+docker build -f Dockerfile -t motion_bench_maker_moveit_src ../../
diff --git a/docker/noetic-src/run-docker.sh b/docker/noetic-src/run-docker.sh
new file mode 100755
index 0000000..b70e449
--- /dev/null
+++ b/docker/noetic-src/run-docker.sh
@@ -0,0 +1,12 @@
+#Run with gpu support
+
+if [ "$1" = "gpu" ] ; then
+  docker run --rm -it --gpus all -v /usr/share/glvnd/egl_vendor.d:/usr/share/glvnd/egl_vendor.d motion_bench_maker
+elif [ "$1" = "no-gpu" ] ; then
+  docker run --rm -it motion_bench_maker
+else
+  echo "You need to specify if you want gpu support or not. Syntax: ./run-docker.sh {gpu, no-gpu}"
+  exit
+fi
+
+#Run without gpu support
diff --git a/include/motion_bench_maker/octomap_generator.h b/include/motion_bench_maker/octomap_generator.h
index a7dc758..bef8adc 100644
--- a/include/motion_bench_maker/octomap_generator.h
+++ b/include/motion_bench_maker/octomap_generator.h
@@ -62,6 +62,11 @@ namespace robowflex
         // @brief loads a scene to the
 
         void loadScene(const SceneConstPtr &scene);
+        // Creates a pointcloud with points at max range. This can help create an octomap
+        // with correct free cells.
+
+        void setClipAtMaxRange(bool clip_at_max_range);
+        void clear();
 
         // pose The position of the camera in ROS standard coordinates (+Z down camera LoS)
         CloudXYZPtr generateCloud(const RobotPose &cam_pose);
@@ -72,19 +77,21 @@ namespace robowflex
                           const IO::RVIZHelperPtr &rviz = nullptr);
 
         CloudXYZPtr getLastPointCloud();
+        const gds::CameraProperties getCameraProperties();
+        occupancy_map_monitor::OccMapTreePtr getLastOctomapTree();
+        const occupancy_map_monitor::OccMapTreeConstPtr getLastOctomapTreeConst();
 
     private:
         gds::Mesh geomToMesh(const GeometryPtr &geom, const std::string &name);
         void parseCameraProporties(const std::string &config);
         gds::CameraProperties props_;
         Sensors sensors_;
+        bool clip_at_max_range_;
 
         ScenePtr cloudToOctomap(const RobotPose &cam_pose, pcl::PointCloud<pcl::PointXYZ>);
         std::shared_ptr<gds::SimDepthCamera> sim_;
         occupancy_map_monitor::OccMapTreePtr tree_;
         CloudXYZPtr fullCloud_;
-
-        octomap::KeyRay key_ray_;
     };
 }  // namespace robowflex
 
diff --git a/launch/benchmark.launch b/launch/benchmark.launch
index 28f39d3..2141158 100644
--- a/launch/benchmark.launch
+++ b/launch/benchmark.launch
@@ -9,7 +9,6 @@
   <arg name="dataset" default="package://motion_bench_maker/problems/shelf_fetch/" />
   <arg name="results" default="package://motion_bench_maker/results/shelf_fetch/" />
   <arg name="exp_name" default="results" />
-  <arg name="train" default="False" />
   <arg name="time" default="30" />
 
   <arg name="start" default="1" />
@@ -31,7 +30,6 @@
     <param name="end" value="$(arg end)" />
     <param name="runs" value="$(arg runs)" />
     <param name="time" value="$(arg time)" />
-    <param name="train" value="$(arg train)" />
     <rosparam param="planners" subst_value="True">[$(arg planners)]</rosparam>
     
   </node>
diff --git a/scripts/benchmark.cpp b/scripts/benchmark.cpp
index 8f9136d..273885d 100644
--- a/scripts/benchmark.cpp
+++ b/scripts/benchmark.cpp
@@ -28,7 +28,7 @@ int main(int argc, char **argv)
     std::string dataset, results, exp_name;
     std::vector<std::string> planners;
     double time, runs;
-    bool sensed, train;
+    bool sensed;
 
     int start, end;
 
@@ -46,7 +46,6 @@ int main(int argc, char **argv)
     error += !parser::get(exec_name, node, "end", end);
     error += !parser::get(exec_name, node, "runs", runs);
     error += !parser::get(exec_name, node, "sensed", sensed);
-    error += !parser::get(exec_name, node, "train", train);
 
     parser::shutdownIfError(exec_name, error);
 
diff --git a/scripts/train.cpp b/scripts/train.cpp
deleted file mode 100644
index 6494796..0000000
--- a/scripts/train.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/* Author: Constantinos Chamzas */
-
-// ROS
-#include <moveit_msgs/MoveItErrorCodes.h>
-#include <ros/ros.h>
-
-// Robowflex dataset
-#include <motion_bench_maker/setup.h>
-#include <motion_bench_maker/parser.h>
-
-// Robowflex library
-#include <robowflex_library/benchmarking.h>
-#include <robowflex_library/builder.h>
-#include <robowflex_library/scene.h>
-#include <robowflex_library/io.h>
-
-// Robowflex ompl
-#include <robowflex_ompl/ompl_interface.h>
-
-#include <ompl/tools/thunder/Thunder.h>
-#include <ompl/tools/lightning/Lightning.h>
-
-using namespace robowflex;
-
-void prerunCallback(PlanningQuery &query)
-{
-    const auto planner = dynamic_cast<OMPL::OMPLInterfacePlanner *>(query.planner.get());
-    if (planner)
-    {
-        // Get the name of the planner.
-        const auto &planner_name = query.request.planner_id;
-        if (planner_name == "thunder")
-        {
-            const auto &ss = dynamic_cast<ompl::tools::Thunder *>(planner->getLastSimpleSetup().get());
-            if (ss)
-            {
-                ss->setFilePath("thunder.db");
-                ss->setup();
-            }
-            else
-                ROS_ERROR("Failed to typecast Thunder simple Setup!");
-        }
-        else if (planner_name == "lightning")
-        {
-            const auto &ss = dynamic_cast<ompl::tools::Lightning *>(planner->getLastSimpleSetup().get());
-            if (ss)
-            {
-                ss->setFilePath("lightning.db");
-                ss->setup();
-            }
-            else
-                ROS_ERROR("Failed to typecast Thunder simple Setup!");
-        }
-        else
-        {
-            ROS_ERROR("Underlying planners is not lightning or thunder");
-        }
-    }
-}
-
-int main(int argc, char **argv)
-{
-    // Enables multiple instances running with the same name
-    ros::init(argc, argv, "benchmark");
-    ros::NodeHandle node("~");
-
-    std::string dataset, results, exp_name;
-    std::vector<std::string> planners;
-    double time, runs;
-    bool sensed;
-
-    int start, end;
-
-    std::string exec_name = "benchmark";
-
-    // Load ROS params
-    size_t error = 0;
-    // Parsing the parametes from the param server
-    error += !parser::get(exec_name, node, "dataset", dataset);
-    error += !parser::get(exec_name, node, "results", results);
-    error += !parser::get(exec_name, node, "exp_name", exp_name);
-    error += !parser::get(exec_name, node, "time", time);
-    error += !parser::get(exec_name, node, "planners", planners);
-    error += !parser::get(exec_name, node, "start", start);
-    error += !parser::get(exec_name, node, "end", end);
-    error += !parser::get(exec_name, node, "runs", runs);
-    error += !parser::get(exec_name, node, "sensed", sensed);
-
-    parser::shutdownIfError(exec_name, error);
-
-    auto setup = std::make_shared<Setup>(dataset);
-    auto robot = setup->getRobot();
-
-    // Benchmarker
-    Profiler::Options options;
-    options.metrics = Profiler::WAYPOINTS | Profiler::CORRECT | Profiler::LENGTH | Profiler::SMOOTHNESS;
-
-    Experiment experiment(exp_name, options, time, runs);
-    auto settings = OMPL::Settings();
-    settings.simplify_solutions = false;
-    settings.interpolate_solutions = false;
-
-    auto planner = setup->createPlanner("planner_name", settings);
-
-    int numSamples = (end > 1) ? end : setup->getNumSamples();
-    for (int i = start; i <= numSamples; i++)
-    {
-        const auto &scene = std::make_shared<Scene>(robot);
-        if (sensed)
-            setup->loadSensedScene(i, scene);
-        else
-            setup->loadGeometricScene(i, scene);
-
-        for (const auto &planner_name : planners)
-        {
-            const auto &request = std::make_shared<MotionRequestBuilder>(planner, setup->getGroup());
-            setup->loadRequest(i, request);
-            request->setAllowedPlanningTime(time);
-            request->setNumPlanningAttempts(1);
-
-            // Set the name of the planner
-            if (!request->setConfig(planner_name))
-                ROS_ERROR("Did not find planner %s", planner_name.c_str());
-
-            experiment.setPreRunCallback(prerunCallback)
-                experiment.addQuery(planner_name, scene, planner, request);
-        }
-    }
-
-    auto results_data = experiment.benchmark(1);
-
-    OMPLPlanDataSetOutputter output(IO::resolveParent(results));
-    output.dump(*results_data);
-
-    return 0;
-}
diff --git a/src/octomap_generator.cpp b/src/octomap_generator.cpp
index cf76579..3dd6dc9 100644
--- a/src/octomap_generator.cpp
+++ b/src/octomap_generator.cpp
@@ -36,6 +36,9 @@ OctomapGenerator::OctomapGenerator(const std::string &config)
     tree_ = std::make_shared<occupancy_map_monitor::OccMapTree>(sensors_.resolution);
     sim_ = std::make_shared<gds::SimDepthCamera>(props_);
     fullCloud_ = std::make_shared<CloudXYZ>();
+
+    // The default clip_at_max_range behaviour is false.
+    clip_at_max_range_ = false;
 };
 
 RobotPose OctomapGenerator::lookat(const Eigen::Vector3d &eye, const Eigen::Vector3d &origin)
@@ -84,30 +87,50 @@ void OctomapGenerator::loadScene(const SceneConstPtr &scene)
     {
         const auto &obj = scene->getObjectGeometry(name);
         const auto &mesh = geomToMesh(obj, name);
+        // TODO: does this need to be cleared everytime?
         sim_->add(name, mesh, toMatrix<Eigen::Isometry3d>(scene->getObjectPose(name)));
     }
 }
 
+void OctomapGenerator::setClipAtMaxRange(bool clip_at_max_range)
+{
+    clip_at_max_range_ = clip_at_max_range;
+}
+
+void OctomapGenerator::clear()
+{
+    tree_->clear();
+    fullCloud_->clear();
+}
+
 CloudXYZPtr OctomapGenerator::generateCloud(const RobotPose &cam_pose)
 {
     CloudXYZ cloud;
-    const auto &depth_img = sim_->render(toMatrix<Eigen::Isometry3d>(cam_pose));
+    auto depth_img = sim_->render(toMatrix<Eigen::Isometry3d>(cam_pose));
+
+    // Use z_far for areas with no objects
+    if (clip_at_max_range_)
+        for (auto &d : depth_img.data)
+            d = d == 0 ? props_.z_far : d;
+
     gds::toPointCloudXYZ(props_, depth_img, cloud);
 
     auto tr = cam_pose.translation();
+    auto qr = cam_pose.rotation();
     // cloud.sensor_origin_ = tr;
     cloud.sensor_origin_.x() = tr.x();
     cloud.sensor_origin_.y() = tr.y();
     cloud.sensor_origin_.z() = tr.z();
 
+    cloud.sensor_orientation_ = qr.cast<float>();
+
     return std::make_shared<CloudXYZ>(cloud);
 }
 
 bool OctomapGenerator::geomToSensed(const ScenePtr &geometric, const ScenePtr &sensed,
                                     const IO::RVIZHelperPtr &rviz)
 {
-    tree_->clear();
-    fullCloud_->clear();
+    clear();
     auto start = ros::WallTime::now();
 
     loadScene(geometric);
@@ -152,7 +175,8 @@ bool OctomapGenerator::updateOctoMap(const CloudXYZPtr &cloud, const RobotPose &
 
     // sensor origin is the cloud origin
     octomap::point3d sensor_origin(cso.x(), cso.y(), cso.z());
-    octomap::KeySet free_cells, occupied_cells;
+    octomap::KeySet free_cells, occupied_cells, clipped_cells;
+    octomap::KeyRay key_ray_;
 
     tree_->lockRead();
 
@@ -166,23 +190,28 @@ bool OctomapGenerator::updateOctoMap(const CloudXYZPtr &cloud, const RobotPose &
             if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z))
             {
                 // transform to camera frame
-                auto point = cam_pose * Eigen::Vector3d{p.x, p.y, p.z};
-                occupied_cells.insert(tree_->coordToKey(point.x(), point.y(), point.z()));
-                fullCloud_->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));
+                auto vec = Eigen::Vector3d{p.x, p.y, p.z};
+                auto point = cam_pose * vec;
+                // 0.999 is to account for numerical errors
+                if (vec.norm() < props_.z_far * (0.999))
+                {
+                    occupied_cells.insert(tree_->coordToKey(point.x(), point.y(), point.z()));
+                    fullCloud_->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));
+                }
+                else
+                    clipped_cells.insert(tree_->coordToKey(point.x(), point.y(), point.z()));
             }
         }
 
-        // TODO: Maybe I should remove this completelely
         /* compute the free cells along each ray that ends at an occupied cell */
         for (auto it = occupied_cells.begin(), end = occupied_cells.end(); it != end; ++it)
             if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
                 free_cells.insert(key_ray_.begin(), key_ray_.end());
 
         /* compute the free cells along each ray that ends at a clipped cell */
-        // for (octomap::KeySet::iterator it = clip_cells.begin(), end = clip_cells.end(); it != end;
-        // ++it)
-        //    if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
-        //        free_cells.insert(key_ray_.begin(), key_ray_.end());
+        for (auto it = clipped_cells.begin(), end = clipped_cells.end(); it != end; ++it)
+            if (tree_->computeRayKeys(sensor_origin, tree_->keyToCoord(*it), key_ray_))
+                free_cells.insert(key_ray_.begin(), key_ray_.end());
     }
 
     catch (...)
@@ -223,3 +252,18 @@ CloudXYZPtr OctomapGenerator::getLastPointCloud()
 {
     return fullCloud_;
 }
+
+const gds::CameraProperties OctomapGenerator::getCameraProperties()
+{
+    return props_;
+}
+
+occupancy_map_monitor::OccMapTreePtr OctomapGenerator::getLastOctomapTree()
+{
+    return tree_;
+}
+
+const occupancy_map_monitor::OccMapTreeConstPtr OctomapGenerator::getLastOctomapTreeConst()
+{
+    return collision_detection::OccMapTreeConstPtr(tree_);
+}