太棒了！我们来用 OMPL（Open Motion Planning Library） 实现一个 Informed RRT* 与 标准 RRT* 的对比 Demo，直观展示两者在路径优化速度和效率上的差异。

🎯 目标：在相同环境下规划路径，比较 RRT* 和 Informed RRT* 的收敛速度与路径质量随时间的变化。

✅ 第一步：安装 OMPL（Python 版）

# 推荐使用 conda 安装带 Python bindings 的 OMPL
conda install -c conda-forge ompl

或者从源码编译（支持更多算法）：
👉 https://github.com/ompl/ompl

✅ 第二步：Python Demo 代码（RRT* vs Informed RRT*）

import numpy as np
import matplotlib.pyplot as plt
from ompl import base as ob
from ompl import geometric as og

class InformedRRTStarDemo:
    def __init__(self):
        # 2D 状态空间
        self.space = ob.RealVectorStateSpace(2)
        self.space.setBounds(-5, 5)  # x, y 范围 [-5, 5]

        # 创建简单障碍物环境
        self.start = ob.State(self.space)
        self.start()[0] = -4.0
        self.start()[1] = -4.0

        self.goal = ob.State(self.space)
        self.goal()[0] = 4.0
        self.goal()[1] = 4.0

        # 空间信息
        self.si = ob.SpaceInformation(self.space)
        self.si.setStateValidityChecker(ob.StateValidityCheckerFn(self.isStateValid))
        self.si.setup()

        # 问题定义
        self.pdef = ob.ProblemDefinition(self.si)
        self.pdef.setStartAndGoalStates(self.start, self.goal)

        # 设置优化目标：最小化路径长度
        self.pdef.setOptimizationObjective(ob.PathLengthOptimizationObjective(self.si))

    def isStateValid(self, state):
        """避开原点附近的圆形障碍物"""
        x, y = state[0], state[1]
        return x**2 + y**2 >= 1.0  # 半径为1的圆形障碍

    def plan(self, planner_name, max_samplings=10000, intermediate_steps=50):
        """运行规划器并记录中间路径长度"""
        self.pdef.clearSolutionPaths()

        # 创建规划器
        if planner_name == "RRT*":
            planner = og.RRTstar(self.si)
        elif planner_name == "Informed RRT*":
            planner = og.InformedRRTstar(self.si)
        else:
            raise ValueError("Unsupported planner")

        planner.setProblemDefinition(self.pdef)
        planner.setup()

        # 设置参数
        planner.setRange(0.5)          # 步长
        planner.setGoalBias(0.05)      # 目标偏置
        planner.setRewireFactor(1.2)   # 启用重布线

        print(f"Running {planner_name}...")

        path_costs = []
        sampling_log = []

        for i in range(max_samplings):
            planner.iteration()

            # 每隔若干次采样记录一次当前最优路径长度
            if i % intermediate_steps == 0:
                if self.pdef.hasSolution():
                    sol_path = self.pdef.getSolutionPath()
                    simplified = ob.PathSimplifier(self.si)
                    simplified.simplify(sol_path)
                    cost = sol_path.length()
                    path_costs.append(cost)
                    sampling_log.append(i)

        solved = self.pdef.hasSolution()
        final_cost = self.pdef.getSolutionPath().length() if solved else float('inf')

        return {
            "name": planner_name,
            "solved": solved,
            "final_cost": final_cost,
            "path_costs": path_costs,
            "sampling_log": sampling_log,
            "num_samplings": max_samplings
        }

# === 主程序 ===
demo = InformedRRTStarDemo()

# 🚀 运行 RRT*
result_rrt