implement rrt planner

main.py

@@ -1,4 +1,6 @@
 from flight_environment import FlightEnvironment
+from planners.rrt_planner import rrt_planner
+from plot_plotly import plot_cylinders
 
 env = FlightEnvironment(50)
 start = (1,2,0)
@@ -13,12 +15,13 @@ goal = (18,18,3)
 #   - column 3 contains the z-coordinates of all path points
 # This `path` array will be provided to the `env` object for visualization.
 
-path = [[0,0,0],[1,1,1],[2,2,2],[3,3,3]]
+#path = [[0,0,0],[1,1,1],[2,2,2],[3,3,3]]
+path = rrt_planner(env, start, goal, step_size=1)
 
 # --------------------------------------------------------------------------------------------------- #
 
 
-env.plot_cylinders(path)
+plot_cylinders(env, path)
 
 
 # --------------------------------------------------------------------------------------------------- #

path_planner.py

@@ -1,24 +0,0 @@
-"""
-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.
-"""
-            
-
-
-
-
-
-
-
-
-
-
-

planners/path_planner.py

@@ -0,0 +1,109 @@
+import numpy as np
+"""
+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.
+"""
+
+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 with cylinder obstacles provided by env.
+    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
+    stats = dict(collisions=0)
+
+    @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 point
+        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:
+            print('norm0')
+            continue
+        direction = direction / norm
+
+        new_point = nearest.point + direction * min(step_size, norm)
+
+        # ensure within bounds
+        if env.is_outside(tuple(new_point)):
+            print('outside')
+            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])
+                return path
+
+    raise RuntimeError(f"RRT failed to find a path within the given iterations. {stats=}")

planners/rrt_planner.py

@@ -0,0 +1,98 @@
+from dataclasses import dataclass
+import numpy as np
+
+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 with cylinder obstacles provided by env.
+    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
+    stats = dict(collisions=0)
+
+    @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 point
+        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:
+            print('norm0')
+            continue
+        direction = direction / norm
+
+        new_point = nearest.point + direction * min(step_size, norm)
+
+        # ensure within bounds
+        if env.is_outside(tuple(new_point)):
+            print('outside')
+            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])
+                return path
+
+    raise RuntimeError(f"RRT failed to find a path within the given iterations. {stats=}")

plot_plotly.py

@@ -0,0 +1,136 @@
+import numpy as np
+import plotly.graph_objects as go
+import plotly.io as pio
+
+def plot_cylinders(env, path=None, save_html=True):
+    """
+    使用Plotly创建高性能交互式3D图形
+    """
+    cylinders = env.cylinders
+    space_size = env.space_size
+
+    fig = go.Figure()
+
+    # 添加圆柱体
+    for i, (cx, cy, h, r) in enumerate(cylinders):
+        # 创建圆柱体侧面
+        z = np.linspace(0, h, 20)
+        theta = np.linspace(0, 2 * np.pi, 30)
+        theta_grid, z_grid = np.meshgrid(theta, z)
+
+        x = cx + r * np.cos(theta_grid)
+        y = cy + r * np.sin(theta_grid)
+
+        fig.add_trace(go.Surface(
+            x=x, y=y, z=z_grid,
+            colorscale=[[0, 'lightblue'], [1, 'steelblue']],
+            opacity=0.7,
+            showscale=False,
+            name=f'Cylinder {i}',
+            hovertemplate=f'<b>Cylinder {i}</b><br>' +
+                          f'Center: ({cx:.1f}, {cy:.1f})<br>' +
+                          f'Height: {h:.1f}<br>' +
+                          f'Radius: {r:.1f}<extra></extra>'
+        ))
+
+        # 添加顶部
+        theta2 = np.linspace(0, 2 * np.pi, 30)
+        x_top = cx + r * np.cos(theta2)
+        y_top = cy + r * np.sin(theta2)
+        z_top = np.full_like(theta2, h)
+
+        # 创建顶部面
+        fig.add_trace(go.Mesh3d(
+            x=x_top, y=y_top, z=z_top,
+            color='steelblue',
+            opacity=0.7,
+            alphahull=0,
+            showscale=False
+        ))
+
+    # 添加路径
+    if path is not None:
+        path = np.array(path)
+        xs, ys, zs = path[:, 0], path[:, 1], path[:, 2]
+
+        fig.add_trace(go.Scatter3d(
+            x=xs, y=ys, z=zs,
+            mode='lines+markers',
+            line=dict(color='red', width=6),
+            marker=dict(
+                size=4,
+                color=zs,
+                colorscale='Viridis',
+                showscale=True,
+                colorbar=dict(title="高度")
+            ),
+            name='Path',
+            hovertemplate='<b>Path Point</b><br>' +
+                          'x: %{x:.2f}<br>' +
+                          'y: %{y:.2f}<br>' +
+                          'z: %{z:.2f}<extra></extra>'
+        ))
+
+        # 添加起点和终点
+        fig.add_trace(go.Scatter3d(
+            x=[xs[0]], y=[ys[0]], z=[zs[0]],
+            mode='markers',
+            marker=dict(size=10, color='green', symbol='circle'),
+            name='Start',
+            hovertemplate='<b>Start</b><br>' +
+                          f'Position: ({xs[0]:.2f}, {ys[0]:.2f}, {zs[0]:.2f})<extra></extra>'
+        ))
+
+        fig.add_trace(go.Scatter3d(
+            x=[xs[-1]], y=[ys[-1]], z=[zs[-1]],
+            mode='markers',
+            marker=dict(size=10, color='red', symbol='circle'),
+            name='End',
+            hovertemplate='<b>End</b><br>' +
+                          f'Position: ({xs[-1]:.2f}, {ys[-1]:.2f}, {zs[-1]:.2f})<extra></extra>'
+        ))
+
+    # 更新布局
+    fig.update_layout(
+        title=dict(
+            text='3D Environment with Path',
+            x=0.5,
+            font=dict(size=20)
+        ),
+        scene=dict(
+            xaxis=dict(
+                title='X',
+                range=[0, env.env_width],
+                backgroundcolor='rgb(240, 240, 240)',
+                gridcolor='white',
+                showbackground=True
+            ),
+            yaxis=dict(
+                title='Y',
+                range=[0, env.env_length],
+                backgroundcolor='rgb(240, 240, 240)',
+                gridcolor='white',
+                showbackground=True
+            ),
+            zaxis=dict(
+                title='Z',
+                range=[0, env.env_height],
+                backgroundcolor='rgb(240, 240, 240)',
+                gridcolor='white',
+                showbackground=True
+            ),
+            aspectmode='manual',
+            aspectratio=dict(x=1, y=env.env_length / env.env_width,
+                             z=env.env_height / env.env_width)
+        ),
+        showlegend=True,
+        width=1200,
+        height=800,
+        hovermode='closest'
+    )
+
+    # 保存为HTML（可选）
+    if save_html:
+        pio.write_html(fig, file='interactive_3d_plot.html', auto_open=True)
+
+    return fig