show both plots at the same time

flight_environment.py

@@ -150,7 +150,7 @@ class FlightEnvironment:
             ax.scatter(xs[0], ys[0], zs[0], s=40) 
             ax.scatter(xs[-1], ys[-1], zs[-1], s=40) 
         self.set_axes_equal(ax)
-        plt.show()
+        plt.show(block=False)
 
 
     def set_axes_equal(self,ax):

main.py

@@ -1,9 +1,9 @@
 from flight_environment import FlightEnvironment
-from planners.rrt_planner import rrt_planner
+from planners.path_planner import rrt_planner
 from planners.trajectory_generator import plan_trajectory_through_path, plot_trajectory
 from plot_plotly import plot_cylinders
 
-env = FlightEnvironment(50)
+env = FlightEnvironment(200)
 start = (1,2,0)
 goal = (18,18,3)
 
@@ -20,12 +20,13 @@ goal = (18,18,3)
 path = rrt_planner(env, start, goal, step_size=1)
 
 # --------------------------------------------------------------------------------------------------- #
+# 3D env plot, choose one - Plotly or Matplotlib - comment the other
 
-# Matplotlib (laggy)
+# Plotly
 plot_cylinders(env, path)
 
-# Plotly
-# env.plot_cylinders(path)
+# Matplotlib (laggy)
+#env.plot_cylinders(path)
 
 # --------------------------------------------------------------------------------------------------- #
 #   Call your trajectory planning algorithm here. The algorithm should

planners/path_planner.py

@@ -1,12 +1,99 @@
-"""
-In this file, you should implement your own path planning class or function.
-Within your implementation, you may call `env.is_collide()` and `env.is_outside()`
-to verify whether candidate path points collide with obstacles or exceed the
-environment boundaries.
-
-You are required to write the path planning algorithm by yourself. Copying or calling 
-any existing path planning algorithms from others is strictly
-prohibited. Please avoid using external packages beyond common Python libraries
-such as `numpy`, `math`, or `scipy`. If you must use additional packages, you
-must clearly explain the reason in your report.
-"""
+from dataclasses import dataclass
+import numpy as np
+import time
+
+def is_segment_collision_free(env, p1, p2, step=0.1):
+    """Return True if straight segment p1->p2 is free.
+    - env: FlightEnvironment instance
+    - p1,p2: iterable (x,y,z)
+    - step: distance between samples along the segment (meters)
+    """
+    dist = np.linalg.norm(p2 - p1)
+    if dist == 0:
+        return not env.is_collide(tuple(p1)) and not env.is_collide(tuple(p1))
+    n = max(1, int(np.ceil(dist / step)))
+    for i in range(n + 1):
+        t = i / n
+        p = p1 * (1 - t) + p2 * t
+        if env.is_outside(tuple(p)) or env.is_collide(tuple(p)):
+            return False
+    return True
+
+def rrt_planner(env, start, goal, max_iters=10000, step_size=1.0, goal_tolerance=0.8, goal_sample_rate=0.1):
+    """
+    Basic RRT planner in continuous 3D space.
+    Returns a path as an (N x 3) numpy array from start to goal (inclusive).
+    """
+    start = np.array(start, dtype=float)
+    goal = np.array(goal, dtype=float)
+
+    # quick checks
+    if env.is_outside(tuple(start)) or env.is_collide(tuple(start)):
+        raise RuntimeError("Start is invalid (outside or in collision).")
+    if env.is_outside(tuple(goal)) or env.is_collide(tuple(goal)):
+        raise RuntimeError("Goal is invalid (outside or in collision).")
+
+    # recorded statistics stored here, can be inspected later
+    stats = dict(collisions=0)
+    tstart = time.time()
+
+    @dataclass
+    class Node:
+        point: np.ndarray
+        parent: np.ndarray = None
+
+    # environment bounds
+    Xmax, Ymax, Zmax = env.space_size
+
+    nodes = [Node(start)]
+
+    for it in range(max_iters):
+        # sample random point sometimes, otherwise take goal (->bias)
+        if np.random.rand() < goal_sample_rate:
+            sample = goal.copy()
+        else:
+            sample = np.array([np.random.uniform(0, Xmax),
+                               np.random.uniform(0, Ymax),
+                               np.random.uniform(0, Zmax)])
+        # find nearest node
+        dists = [np.linalg.norm(node.point - sample) for node in nodes]
+        nearest_idx = int(np.argmin(dists))
+        nearest = nodes[nearest_idx]
+
+        direction = sample - nearest.point
+        norm = np.linalg.norm(direction)
+        if norm == 0:
+            continue
+        direction = direction / norm
+
+        new_point = nearest.point + direction * min(step_size, norm)
+
+        # ensure within bounds
+        if env.is_outside(tuple(new_point)):
+            continue
+
+        # check collision along segment
+        if not is_segment_collision_free(env, nearest.point, new_point, step=step_size / 5.0):
+            stats["collisions"]+=1
+            continue
+
+        new_node = Node(new_point, nearest)
+        nodes.append(new_node)
+
+        # check if we reached goal
+        if np.linalg.norm(new_point - goal) <= goal_tolerance:
+            # try connect directly to goal
+            if is_segment_collision_free(env, new_point, goal, step=step_size / 5.0):
+                goal_node = Node(goal, new_node)
+                nodes.append(goal_node)
+                # reconstruct path
+                path_nodes = []
+                cur = goal_node
+                while cur is not None:
+                    path_nodes.append(cur.point)
+                    cur = cur.parent
+                path = np.array(path_nodes[::-1])
+                print("reached goal", len(nodes), "nodes", len(path), "waypoints, avoided", stats['collisions'], "collisions", f'{time.time()-tstart:.2f}', "seconds")
+                return path
+
+    raise RuntimeError(f"RRT failed to find a path within the given iterations. {stats=}")

planners/rrt_planner.py

@@ -1,99 +0,0 @@
-from dataclasses import dataclass
-import numpy as np
-import time
-
-def is_segment_collision_free(env, p1, p2, step=0.1):
-    """Return True if straight segment p1->p2 is free.
-    - env: FlightEnvironment instance
-    - p1,p2: iterable (x,y,z)
-    - step: distance between samples along the segment (meters)
-    """
-    dist = np.linalg.norm(p2 - p1)
-    if dist == 0:
-        return not env.is_collide(tuple(p1)) and not env.is_collide(tuple(p1))
-    n = max(1, int(np.ceil(dist / step)))
-    for i in range(n + 1):
-        t = i / n
-        p = p1 * (1 - t) + p2 * t
-        if env.is_outside(tuple(p)) or env.is_collide(tuple(p)):
-            return False
-    return True
-
-def rrt_planner(env, start, goal, max_iters=10000, step_size=1.0, goal_tolerance=0.8, goal_sample_rate=0.1):
-    """
-    Basic RRT planner in continuous 3D space.
-    Returns a path as an (N x 3) numpy array from start to goal (inclusive).
-    """
-    start = np.array(start, dtype=float)
-    goal = np.array(goal, dtype=float)
-
-    # quick checks
-    if env.is_outside(tuple(start)) or env.is_collide(tuple(start)):
-        raise RuntimeError("Start is invalid (outside or in collision).")
-    if env.is_outside(tuple(goal)) or env.is_collide(tuple(goal)):
-        raise RuntimeError("Goal is invalid (outside or in collision).")
-
-    # recorded statistics stored here, can be inspected later
-    stats = dict(collisions=0)
-    tstart = time.time()
-
-    @dataclass
-    class Node:
-        point: np.ndarray
-        parent: np.ndarray = None
-
-    # environment bounds
-    Xmax, Ymax, Zmax = env.space_size
-
-    nodes = [Node(start)]
-
-    for it in range(max_iters):
-        # sample random point sometimes, otherwise take goal (->bias)
-        if np.random.rand() < goal_sample_rate:
-            sample = goal.copy()
-        else:
-            sample = np.array([np.random.uniform(0, Xmax),
-                               np.random.uniform(0, Ymax),
-                               np.random.uniform(0, Zmax)])
-        # find nearest node
-        dists = [np.linalg.norm(node.point - sample) for node in nodes]
-        nearest_idx = int(np.argmin(dists))
-        nearest = nodes[nearest_idx]
-
-        direction = sample - nearest.point
-        norm = np.linalg.norm(direction)
-        if norm == 0:
-            continue
-        direction = direction / norm
-
-        new_point = nearest.point + direction * min(step_size, norm)
-
-        # ensure within bounds
-        if env.is_outside(tuple(new_point)):
-            continue
-
-        # check collision along segment
-        if not is_segment_collision_free(env, nearest.point, new_point, step=step_size / 5.0):
-            stats["collisions"]+=1
-            continue
-
-        new_node = Node(new_point, nearest)
-        nodes.append(new_node)
-
-        # check if we reached goal
-        if np.linalg.norm(new_point - goal) <= goal_tolerance:
-            # try connect directly to goal
-            if is_segment_collision_free(env, new_point, goal, step=step_size / 5.0):
-                goal_node = Node(goal, new_node)
-                nodes.append(goal_node)
-                # reconstruct path
-                path_nodes = []
-                cur = goal_node
-                while cur is not None:
-                    path_nodes.append(cur.point)
-                    cur = cur.parent
-                path = np.array(path_nodes[::-1])
-                print("reached goal", len(nodes), "nodes", len(path), "waypoints", f'{time.time()-tstart:.2f}', "seconds")
-                return path
-
-    raise RuntimeError(f"RRT failed to find a path within the given iterations. {stats=}")