Also include self-collisions to make RRT look nicer

examples/example_rrt.py

@@ -13,6 +13,38 @@
 def prepare_scene(visual_model, collision_model):
     """Helper function to create a collision scene for this example."""
 
+    # Add self collisions
+    self_collision_pair_names = [
+        ("panda_link0_0", "panda_link2_0"),
+        ("panda_link0_0", "panda_link3_0"),
+        ("panda_link0_0", "panda_link4_0"),
+        ("panda_link0_0", "panda_link5_0"),
+        ("panda_link0_0", "panda_link6_0"),
+        ("panda_link0_0", "panda_link7_0"),
+        ("panda_link1_0", "panda_link3_0"),
+        ("panda_link1_0", "panda_link4_0"),
+        ("panda_link1_0", "panda_link5_0"),
+        ("panda_link1_0", "panda_link6_0"),
+        ("panda_link1_0", "panda_link7_0"),
+        ("panda_link2_0", "panda_link4_0"),
+        ("panda_link2_0", "panda_link5_0"),
+        ("panda_link2_0", "panda_link6_0"),
+        ("panda_link2_0", "panda_link7_0"),
+        ("panda_link3_0", "panda_link5_0"),
+        ("panda_link3_0", "panda_link6_0"),
+        ("panda_link3_0", "panda_link7_0"),
+        ("panda_link4_0", "panda_link6_0"),
+        ("panda_link4_0", "panda_link7_0"),
+        ("panda_link5_0", "panda_link7_0"),
+    ]
+    for pair in self_collision_pair_names:
+        collision_model.addCollisionPair(
+            pinocchio.CollisionPair(
+                collision_model.getGeometryId(pair[0]),
+                collision_model.getGeometryId(pair[1]),
+            )
+        )
+
     # Add collision objects
     ground_plane = pinocchio.GeometryObject(
         "ground_plane",

src/pyroboplan/planning/rrt.py

@@ -161,9 +161,9 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
                 ):
                     goal_found = True
 
-                # Switch to the other tree
-                if options.bidirectional_rrt:
-                    start_tree_phase = not start_tree_phase
+            # Switch to the other tree next iteration, if bidirectional mode is enabled.
+            if options.bidirectional_rrt:
+                start_tree_phase = not start_tree_phase
 
         # Back out the path by traversing the parents from the goal.
         self.latest_path = []