衝突回避の経路計画 — RRTとPRMをわかりやすく解説して実装する

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from collections import defaultdict

# --- 環境設定 ---
class Environment2D:
    """2D平面上の障害物環境"""
    def __init__(self, x_range, y_range, obstacles):
        """
        x_range: (xmin, xmax)
        y_range: (ymin, ymax)
        obstacles: [(x, y, w, h), ...] 矩形障害物のリスト
        """
        self.x_range = x_range
        self.y_range = y_range
        self.obstacles = obstacles  # (x, y, width, height)

    def is_collision_free(self, point):
        """点が障害物と衝突しないか判定"""
        x, y = point
        if x < self.x_range[0] or x > self.x_range[1]:
            return False
        if y < self.y_range[0] or y > self.y_range[1]:
            return False
        for (ox, oy, ow, oh) in self.obstacles:
            if ox <= x <= ox + ow and oy <= y <= oy + oh:
                return False
        return True

    def is_edge_collision_free(self, p1, p2, resolution=0.5):
        """2点間の直線経路が衝突しないか判定（離散化チェック）"""
        dist = np.linalg.norm(np.array(p2) - np.array(p1))
        n_checks = max(int(dist / resolution), 2)
        for i in range(n_checks + 1):
            t = i / n_checks
            point = (1 - t) * np.array(p1) + t * np.array(p2)
            if not self.is_collision_free(point):
                return False
        return True

# 環境の作成
obstacles = [
    (20, 20, 10, 30),   # 障害物1
    (50, 10, 10, 40),   # 障害物2
    (35, 60, 25, 8),    # 障害物3
    (70, 30, 8, 35),    # 障害物4
    (10, 70, 30, 5),    # 障害物5
]
env = Environment2D((0, 100), (0, 100), obstacles)
start = np.array([5.0, 5.0])
goal = np.array([95.0, 95.0])

print(f"環境サイズ: {env.x_range} x {env.y_range}")
print(f"障害物数: {len(env.obstacles)}")
print(f"始点: {start}, 終点: {goal}")
print(f"始点は衝突フリー: {env.is_collision_free(start)}")
print(f"終点は衝突フリー: {env.is_collision_free(goal)}")

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle

class RRT:
    """基本的なRRTアルゴリズム"""
    def __init__(self, env, start, goal, step_size=5.0,
                 goal_bias=0.1, max_iter=3000, goal_threshold=5.0):
        self.env = env
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.step_size = step_size
        self.goal_bias = goal_bias
        self.max_iter = max_iter
        self.goal_threshold = goal_threshold

        # 木構造: nodes[i] = 座標, parent[i] = 親ノードのインデックス
        self.nodes = [self.start.copy()]
        self.parent = [-1]  # ルートノードの親は-1

    def random_sample(self):
        """ランダムサンプリング（ゴールバイアス付き）"""
        if np.random.random() < self.goal_bias:
            return self.goal.copy()
        x = np.random.uniform(self.env.x_range[0], self.env.x_range[1])
        y = np.random.uniform(self.env.y_range[0], self.env.y_range[1])
        return np.array([x, y])

    def nearest_node(self, point):
        """木の中で最近傍ノードのインデックスを返す"""
        dists = [np.linalg.norm(node - point) for node in self.nodes]
        return int(np.argmin(dists))

    def steer(self, from_node, to_point):
        """from_nodeからto_pointの方向にstep_sizeだけ進んだ点を返す"""
        direction = to_point - from_node
        dist = np.linalg.norm(direction)
        if dist <= self.step_size:
            return to_point.copy()
        return from_node + self.step_size * direction / dist

    def plan(self):
        """RRTで経路計画を実行"""
        for i in range(self.max_iter):
            # ステップ1: ランダムサンプリング
            q_rand = self.random_sample()

            # ステップ2: 最近傍探索
            nearest_idx = self.nearest_node(q_rand)
            q_near = self.nodes[nearest_idx]

            # ステップ3: 拡張
            q_new = self.steer(q_near, q_rand)

            # ステップ4: 衝突チェックと追加
            if self.env.is_edge_collision_free(q_near, q_new):
                self.nodes.append(q_new)
                self.parent.append(nearest_idx)

                # ステップ5: 終了判定
                if np.linalg.norm(q_new - self.goal) < self.goal_threshold:
                    # ゴールへの接続
                    if self.env.is_edge_collision_free(q_new, self.goal):
                        self.nodes.append(self.goal.copy())
                        self.parent.append(len(self.nodes) - 2)
                        path = self._extract_path(len(self.nodes) - 1)
                        print(f"経路発見! イテレーション: {i+1}, "
                              f"ノード数: {len(self.nodes)}")
                        return path
        print("経路が見つかりませんでした")
        return None

    def _extract_path(self, goal_idx):
        """ゴールから始点まで親をたどって経路を抽出"""
        path = []
        idx = goal_idx
        while idx != -1:
            path.append(self.nodes[idx])
            idx = self.parent[idx]
        path.reverse()
        return path

# RRTの実行
np.random.seed(42)
obstacles = [
    (20, 20, 10, 30),
    (50, 10, 10, 40),
    (35, 60, 25, 8),
    (70, 30, 8, 35),
    (10, 70, 30, 5),
]
env = Environment2D((0, 100), (0, 100), obstacles)
start = np.array([5.0, 5.0])
goal = np.array([95.0, 95.0])

rrt = RRT(env, start, goal, step_size=5.0, goal_bias=0.1, max_iter=3000)
path = rrt.plan()

# 可視化
fig, ax = plt.subplots(1, 1, figsize=(8, 8))
for (ox, oy, ow, oh) in env.obstacles:
    ax.add_patch(Rectangle((ox, oy), ow, oh,
                            facecolor='gray', edgecolor='black', alpha=0.7))
# 木の描画
for i in range(1, len(rrt.nodes)):
    parent_idx = rrt.parent[i]
    p1 = rrt.nodes[parent_idx]
    p2 = rrt.nodes[i]
    ax.plot([p1[0], p2[0]], [p1[1], p2[1]], 'c-', linewidth=0.3, alpha=0.5)

# 経路の描画
if path is not None:
    path_arr = np.array(path)
    ax.plot(path_arr[:, 0], path_arr[:, 1], 'r-', linewidth=2.5, label='RRT Path')

ax.plot(*start, 'go', markersize=12, label='Start')
ax.plot(*goal, 'r*', markersize=15, label='Goal')
ax.set_xlim(env.x_range)
ax.set_ylim(env.y_range)
ax.set_aspect('equal')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('RRT Path Planning in 2D')
ax.legend(loc='upper left')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
import heapq

class PRM:
    """確率的ロードマップ法（PRM）"""
    def __init__(self, env, n_samples=300, k_neighbors=10):
        self.env = env
        self.n_samples = n_samples
        self.k_neighbors = k_neighbors
        self.nodes = []
        self.edges = {}  # {node_idx: [(neighbor_idx, distance), ...]}

    def build_roadmap(self):
        """前処理フェーズ: ロードマップの構築"""
        # ステップ1: ランダムサンプリング
        samples = []
        while len(samples) < self.n_samples:
            x = np.random.uniform(self.env.x_range[0], self.env.x_range[1])
            y = np.random.uniform(self.env.y_range[0], self.env.y_range[1])
            point = np.array([x, y])
            if self.env.is_collision_free(point):
                samples.append(point)
        self.nodes = samples
        self.edges = {i: [] for i in range(len(self.nodes))}

        # ステップ2&3: 近傍接続と衝突チェック
        n_edges = 0
        for i in range(len(self.nodes)):
            # k近傍を計算
            dists = [(j, np.linalg.norm(self.nodes[i] - self.nodes[j]))
                     for j in range(len(self.nodes)) if j != i]
            dists.sort(key=lambda x: x[1])
            neighbors = dists[:self.k_neighbors]

            for j, d in neighbors:
                if self.env.is_edge_collision_free(self.nodes[i], self.nodes[j]):
                    # 双方向エッジ
                    if (j, d) not in self.edges[i]:
                        self.edges[i].append((j, d))
                        self.edges[j].append((i, d))
                        n_edges += 1

        print(f"ロードマップ構築完了: ノード数={len(self.nodes)}, "
              f"エッジ数={n_edges}")

    def query(self, start, goal):
        """クエリフェーズ: 始点と終点を接続して経路を検索"""
        # 始点と終点をロードマップに追加
        start = np.array(start)
        goal = np.array(goal)
        start_idx = len(self.nodes)
        goal_idx = len(self.nodes) + 1
        self.nodes.append(start)
        self.nodes.append(goal)
        self.edges[start_idx] = []
        self.edges[goal_idx] = []

        # 始点・終点を近傍ノードに接続
        for idx in [start_idx, goal_idx]:
            dists = [(j, np.linalg.norm(self.nodes[idx] - self.nodes[j]))
                     for j in range(len(self.nodes) - 2)]
            dists.sort(key=lambda x: x[1])
            for j, d in dists[:self.k_neighbors]:
                if self.env.is_edge_collision_free(self.nodes[idx],
                                                   self.nodes[j]):
                    self.edges[idx].append((j, d))
                    self.edges[j].append((idx, d))

        # ダイクストラ法で最短経路を探索
        path = self._dijkstra(start_idx, goal_idx)

        # 追加したノードとエッジを削除（ロードマップを元に戻す）
        for idx in [start_idx, goal_idx]:
            for j, d in self.edges[idx]:
                self.edges[j] = [(k, dd) for k, dd in self.edges[j]
                                 if k != idx]
            del self.edges[idx]
        self.nodes = self.nodes[:-2]

        return path

    def _dijkstra(self, start_idx, goal_idx):
        """ダイクストラ法で最短経路を探索"""
        dist = {start_idx: 0}
        prev = {start_idx: None}
        pq = [(0, start_idx)]

        while pq:
            d, u = heapq.heappop(pq)
            if u == goal_idx:
                # 経路を復元
                path = []
                node = goal_idx
                while node is not None:
                    path.append(self.nodes[node])
                    node = prev[node]
                path.reverse()
                return path
            if d > dist.get(u, float('inf')):
                continue
            for v, w in self.edges.get(u, []):
                new_dist = d + w
                if new_dist < dist.get(v, float('inf')):
                    dist[v] = new_dist
                    prev[v] = u
                    heapq.heappush(pq, (new_dist, v))

        print("PRM: 経路が見つかりませんでした")
        return None

# PRMの実行
np.random.seed(42)
obstacles = [
    (20, 20, 10, 30),
    (50, 10, 10, 40),
    (35, 60, 25, 8),
    (70, 30, 8, 35),
    (10, 70, 30, 5),
]
env = Environment2D((0, 100), (0, 100), obstacles)
start = np.array([5.0, 5.0])
goal = np.array([95.0, 95.0])

prm = PRM(env, n_samples=300, k_neighbors=10)
prm.build_roadmap()
path_prm = prm.query(start, goal)

# 可視化
fig, ax = plt.subplots(1, 1, figsize=(8, 8))
for (ox, oy, ow, oh) in env.obstacles:
    ax.add_patch(Rectangle((ox, oy), ow, oh,
                            facecolor='gray', edgecolor='black', alpha=0.7))
# ロードマップの描画
for i, neighbors in prm.edges.items():
    for j, d in neighbors:
        if j > i:
            p1, p2 = prm.nodes[i], prm.nodes[j]
            ax.plot([p1[0], p2[0]], [p1[1], p2[1]],
                    'c-', linewidth=0.3, alpha=0.4)
# サンプル点の描画
for node in prm.nodes:
    ax.plot(node[0], node[1], 'c.', markersize=2)

# 経路の描画
if path_prm is not None:
    path_arr = np.array(path_prm)
    ax.plot(path_arr[:, 0], path_arr[:, 1], 'r-',
            linewidth=2.5, label='PRM Path')
    print(f"PRM経路長: {sum(np.linalg.norm(path_arr[i+1]-path_arr[i]) for i in range(len(path_arr)-1)):.1f}")

ax.plot(*start, 'go', markersize=12, label='Start')
ax.plot(*goal, 'r*', markersize=15, label='Goal')
ax.set_xlim(env.x_range)
ax.set_ylim(env.y_range)
ax.set_aspect('equal')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('PRM Path Planning in 2D')
ax.legend(loc='upper left')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle

class RRTStar:
    """RRT*アルゴリズム（漸近最適性を持つ改良版RRT）"""
    def __init__(self, env, start, goal, step_size=5.0,
                 goal_bias=0.1, max_iter=3000, goal_threshold=5.0,
                 gamma=50.0):
        self.env = env
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.step_size = step_size
        self.goal_bias = goal_bias
        self.max_iter = max_iter
        self.goal_threshold = goal_threshold
        self.gamma = gamma
        self.dim = 2

        self.nodes = [self.start.copy()]
        self.parent = [-1]
        self.cost = [0.0]  # ルートからの累積コスト

    def random_sample(self):
        if np.random.random() < self.goal_bias:
            return self.goal.copy()
        x = np.random.uniform(self.env.x_range[0], self.env.x_range[1])
        y = np.random.uniform(self.env.y_range[0], self.env.y_range[1])
        return np.array([x, y])

    def nearest_node(self, point):
        dists = [np.linalg.norm(node - point) for node in self.nodes]
        return int(np.argmin(dists))

    def steer(self, from_node, to_point):
        direction = to_point - from_node
        dist = np.linalg.norm(direction)
        if dist <= self.step_size:
            return to_point.copy()
        return from_node + self.step_size * direction / dist

    def near_nodes(self, point, n):
        """半径r_n内の近傍ノードのインデックスを返す"""
        r_n = min(self.gamma * (np.log(n) / n) ** (1 / self.dim),
                  self.step_size * 3)
        indices = []
        for i, node in enumerate(self.nodes):
            if np.linalg.norm(node - point) < r_n:
                indices.append(i)
        return indices

    def plan(self):
        """RRT*で経路計画を実行"""
        best_goal_idx = None
        best_goal_cost = float('inf')

        for iteration in range(self.max_iter):
            q_rand = self.random_sample()
            nearest_idx = self.nearest_node(q_rand)
            q_near = self.nodes[nearest_idx]
            q_new = self.steer(q_near, q_rand)

            if not self.env.is_edge_collision_free(q_near, q_new):
                continue

            # --- RRT*の追加処理 ---
            new_idx = len(self.nodes)
            n = len(self.nodes)

            # ChooseParent: 近傍からコスト最小の親を選択
            near_indices = self.near_nodes(q_new, n)
            best_parent = nearest_idx
            best_cost = (self.cost[nearest_idx]
                         + np.linalg.norm(q_new - q_near))

            for ni in near_indices:
                edge_cost = np.linalg.norm(q_new - self.nodes[ni])
                candidate_cost = self.cost[ni] + edge_cost
                if (candidate_cost < best_cost
                    and self.env.is_edge_collision_free(
                        self.nodes[ni], q_new)):
                    best_parent = ni
                    best_cost = candidate_cost

            self.nodes.append(q_new)
            self.parent.append(best_parent)
            self.cost.append(best_cost)

            # Rewire: 近傍ノードの親を再配線
            for ni in near_indices:
                edge_cost = np.linalg.norm(q_new - self.nodes[ni])
                new_cost = self.cost[new_idx] + edge_cost
                if (new_cost < self.cost[ni]
                    and self.env.is_edge_collision_free(
                        q_new, self.nodes[ni])):
                    self.parent[ni] = new_idx
                    self._propagate_cost(ni)

            # ゴール到達判定
            if np.linalg.norm(q_new - self.goal) < self.goal_threshold:
                if self.env.is_edge_collision_free(q_new, self.goal):
                    goal_cost = (self.cost[new_idx]
                                 + np.linalg.norm(q_new - self.goal))
                    if goal_cost < best_goal_cost:
                        best_goal_cost = goal_cost
                        best_goal_idx = new_idx

        if best_goal_idx is not None:
            # ゴールノードを追加
            self.nodes.append(self.goal.copy())
            self.parent.append(best_goal_idx)
            self.cost.append(best_goal_cost)
            path = self._extract_path(len(self.nodes) - 1)
            print(f"RRT* 経路発見! コスト: {best_goal_cost:.1f}, "
                  f"ノード数: {len(self.nodes)}")
            return path

        print("RRT*: 経路が見つかりませんでした")
        return None

    def _propagate_cost(self, idx):
        """再配線後のコストを子孫に伝播"""
        parent_idx = self.parent[idx]
        self.cost[idx] = (self.cost[parent_idx]
                          + np.linalg.norm(self.nodes[idx]
                                           - self.nodes[parent_idx]))
        for i in range(len(self.nodes)):
            if self.parent[i] == idx:
                self._propagate_cost(i)

    def _extract_path(self, goal_idx):
        path = []
        idx = goal_idx
        while idx != -1:
            path.append(self.nodes[idx])
            idx = self.parent[idx]
        path.reverse()
        return path

# RRTとRRT*の比較実行
np.random.seed(42)
obstacles = [
    (20, 20, 10, 30),
    (50, 10, 10, 40),
    (35, 60, 25, 8),
    (70, 30, 8, 35),
    (10, 70, 30, 5),
]
env = Environment2D((0, 100), (0, 100), obstacles)
start = [5.0, 5.0]
goal = [95.0, 95.0]

rrt = RRT(env, start, goal, step_size=5.0, goal_bias=0.1, max_iter=3000)
path_rrt = rrt.plan()

np.random.seed(42)
rrt_star = RRTStar(env, start, goal, step_size=5.0, goal_bias=0.1,
                   max_iter=3000, gamma=50.0)
path_rrt_star = rrt_star.plan()

# 比較可視化
fig, axes = plt.subplots(1, 2, figsize=(16, 8))

for ax, planner, path, title in [
    (axes[0], rrt, path_rrt, 'RRT'),
    (axes[1], rrt_star, path_rrt_star, 'RRT*')
]:
    for (ox, oy, ow, oh) in env.obstacles:
        ax.add_patch(Rectangle((ox, oy), ow, oh,
                                facecolor='gray', edgecolor='black',
                                alpha=0.7))
    for i in range(1, len(planner.nodes)):
        parent_idx = planner.parent[i]
        p1 = planner.nodes[parent_idx]
        p2 = planner.nodes[i]
        ax.plot([p1[0], p2[0]], [p1[1], p2[1]],
                'c-', linewidth=0.3, alpha=0.4)

    if path is not None:
        path_arr = np.array(path)
        path_length = sum(np.linalg.norm(path_arr[i+1] - path_arr[i])
                          for i in range(len(path_arr) - 1))
        ax.plot(path_arr[:, 0], path_arr[:, 1], 'r-', linewidth=2.5,
                label=f'Path (length={path_length:.1f})')

    ax.plot(*start, 'go', markersize=12, label='Start')
    ax.plot(*goal, 'r*', markersize=15, label='Goal')
    ax.set_xlim(env.x_range)
    ax.set_ylim(env.y_range)
    ax.set_aspect('equal')
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_title(title)
    ax.legend(loc='upper left')
    ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle

class BiRRT:
    """双方向RRT（BiRRT）"""
    def __init__(self, env, start, goal, step_size=5.0,
                 max_iter=3000, connect_dist=5.0):
        self.env = env
        self.start = np.array(start)
        self.goal = np.array(goal)
        self.step_size = step_size
        self.max_iter = max_iter
        self.connect_dist = connect_dist

        # 始点側の木
        self.tree_a_nodes = [self.start.copy()]
        self.tree_a_parent = [-1]
        # 終点側の木
        self.tree_b_nodes = [self.goal.copy()]
        self.tree_b_parent = [-1]

    def nearest_node(self, nodes, point):
        dists = [np.linalg.norm(node - point) for node in nodes]
        return int(np.argmin(dists))

    def steer(self, from_node, to_point):
        direction = to_point - from_node
        dist = np.linalg.norm(direction)
        if dist <= self.step_size:
            return to_point.copy()
        return from_node + self.step_size * direction / dist

    def extend(self, nodes, parent, q_rand):
        """木を1ステップ拡張"""
        nearest_idx = self.nearest_node(nodes, q_rand)
        q_near = nodes[nearest_idx]
        q_new = self.steer(q_near, q_rand)
        if self.env.is_edge_collision_free(q_near, q_new):
            nodes.append(q_new)
            parent.append(nearest_idx)
            return len(nodes) - 1, q_new
        return None, None

    def connect(self, nodes, parent, q_target):
        """q_targetに到達するまで木を伸ばし続ける"""
        while True:
            nearest_idx = self.nearest_node(nodes, q_target)
            q_near = nodes[nearest_idx]
            q_new = self.steer(q_near, q_target)
            if not self.env.is_edge_collision_free(q_near, q_new):
                return False
            nodes.append(q_new)
            parent.append(nearest_idx)
            if np.linalg.norm(q_new - q_target) < self.connect_dist:
                return True

    def plan(self):
        """BiRRTで経路計画を実行"""
        tree_a_nodes = self.tree_a_nodes
        tree_a_parent = self.tree_a_parent
        tree_b_nodes = self.tree_b_nodes
        tree_b_parent = self.tree_b_parent

        for i in range(self.max_iter):
            # ランダムサンプリング
            x = np.random.uniform(self.env.x_range[0], self.env.x_range[1])
            y = np.random.uniform(self.env.y_range[0], self.env.y_range[1])
            q_rand = np.array([x, y])

            # 木Aを拡張
            new_idx, q_new = self.extend(tree_a_nodes, tree_a_parent,
                                         q_rand)
            if q_new is not None:
                # 木Bをq_newに向かって接続試行
                if self.connect(tree_b_nodes, tree_b_parent, q_new):
                    # 接続成功 — 経路を構築
                    path_a = self._extract_path_forward(
                        tree_a_nodes, tree_a_parent, new_idx)
                    path_b = self._extract_path_forward(
                        tree_b_nodes, tree_b_parent,
                        len(tree_b_nodes) - 1)
                    path_b.reverse()
                    path = path_a + path_b
                    print(f"BiRRT 経路発見! イテレーション: {i+1}")
                    return path

            # 木の役割を交換
            tree_a_nodes, tree_b_nodes = tree_b_nodes, tree_a_nodes
            tree_a_parent, tree_b_parent = tree_b_parent, tree_a_parent

        print("BiRRT: 経路が見つかりませんでした")
        return None

    def _extract_path_forward(self, nodes, parent, idx):
        """指定インデックスからルートまで経路を抽出"""
        path = []
        while idx != -1:
            path.append(nodes[idx])
            idx = parent[idx]
        path.reverse()
        return path

# BiRRTの実行
np.random.seed(42)
obstacles = [
    (20, 20, 10, 30),
    (50, 10, 10, 40),
    (35, 60, 25, 8),
    (70, 30, 8, 35),
    (10, 70, 30, 5),
]
env = Environment2D((0, 100), (0, 100), obstacles)
start = [5.0, 5.0]
goal = [95.0, 95.0]

birrt = BiRRT(env, start, goal, step_size=5.0, max_iter=3000)
path_birrt = birrt.plan()

# 可視化
fig, ax = plt.subplots(1, 1, figsize=(8, 8))
for (ox, oy, ow, oh) in env.obstacles:
    ax.add_patch(Rectangle((ox, oy), ow, oh,
                            facecolor='gray', edgecolor='black', alpha=0.7))

# 木Aの描画（青系）
for i in range(1, len(birrt.tree_a_nodes)):
    pi = birrt.tree_a_parent[i]
    p1 = birrt.tree_a_nodes[pi]
    p2 = birrt.tree_a_nodes[i]
    ax.plot([p1[0], p2[0]], [p1[1], p2[1]], 'b-',
            linewidth=0.3, alpha=0.5)

# 木Bの描画（緑系）
for i in range(1, len(birrt.tree_b_nodes)):
    pi = birrt.tree_b_parent[i]
    p1 = birrt.tree_b_nodes[pi]
    p2 = birrt.tree_b_nodes[i]
    ax.plot([p1[0], p2[0]], [p1[1], p2[1]], 'g-',
            linewidth=0.3, alpha=0.5)

if path_birrt is not None:
    path_arr = np.array(path_birrt)
    path_length = sum(np.linalg.norm(path_arr[i+1] - path_arr[i])
                      for i in range(len(path_arr) - 1))
    ax.plot(path_arr[:, 0], path_arr[:, 1], 'r-', linewidth=2.5,
            label=f'BiRRT Path (length={path_length:.1f})')

ax.plot(*start, 'go', markersize=12, label='Start')
ax.plot(*goal, 'r*', markersize=15, label='Goal')
ax.set_xlim(env.x_range)
ax.set_ylim(env.y_range)
ax.set_aspect('equal')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_title('Bidirectional RRT (BiRRT)')
ax.legend(loc='upper left')
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle, Rectangle
from matplotlib.collections import LineCollection

class TwoLinkArm:
    """2リンク平面ロボットアーム"""
    def __init__(self, l1=1.0, l2=0.8, base=(0, 0)):
        self.l1 = l1  # リンク1の長さ
        self.l2 = l2  # リンク2の長さ
        self.base = np.array(base)

    def forward_kinematics(self, theta1, theta2):
        """順運動学: 関節角度から各関節・手先の位置を計算"""
        # 第1関節の先端（第2関節の位置）
        joint1 = self.base + np.array([
            self.l1 * np.cos(theta1),
            self.l1 * np.sin(theta1)
        ])
        # 手先の位置
        end_effector = joint1 + np.array([
            self.l2 * np.cos(theta1 + theta2),
            self.l2 * np.sin(theta1 + theta2)
        ])
        return self.base, joint1, end_effector


class ArmEnvironment:
    """ロボットアーム用の衝突判定環境"""
    def __init__(self, arm, obstacles):
        """
        arm: TwoLinkArmインスタンス
        obstacles: [(cx, cy, r), ...] 円形障害物のリスト
        """
        self.arm = arm
        self.obstacles = obstacles  # (中心x, 中心y, 半径)

    def is_collision_free(self, config):
        """コンフィギュレーションが衝突しないか判定"""
        theta1, theta2 = config
        base, joint1, end_eff = self.arm.forward_kinematics(theta1, theta2)

        # 各リンクを線分として障害物との距離をチェック
        for (cx, cy, r) in self.obstacles:
            center = np.array([cx, cy])
            # リンク1: base -> joint1
            if self._segment_circle_intersect(base, joint1, center, r):
                return False
            # リンク2: joint1 -> end_effector
            if self._segment_circle_intersect(joint1, end_eff, center, r):
                return False
        return True

    def _segment_circle_intersect(self, p1, p2, center, radius):
        """線分と円の交差判定"""
        d = p2 - p1
        f = p1 - center
        a = np.dot(d, d)
        b = 2 * np.dot(f, d)
        c = np.dot(f, f) - radius ** 2
        discriminant = b ** 2 - 4 * a * c
        if discriminant < 0:
            return False
        discriminant = np.sqrt(discriminant)
        t1 = (-b - discriminant) / (2 * a)
        t2 = (-b + discriminant) / (2 * a)
        # 線分上（0 <= t <= 1）で交差するか
        if (0 <= t1 <= 1) or (0 <= t2 <= 1):
            return True
        if t1 < 0 and t2 > 1:
            return True
        return False

    def is_edge_collision_free(self, config1, config2, n_checks=20):
        """2つのコンフィギュレーション間の補間経路が衝突しないか"""
        for i in range(n_checks + 1):
            t = i / n_checks
            config = (1 - t) * np.array(config1) + t * np.array(config2)
            if not self.is_collision_free(config):
                return False
        return True


# ロボットアームと環境の設定
arm = TwoLinkArm(l1=1.0, l2=0.8, base=(0, 0))
obstacles_arm = [
    (0.8, 0.8, 0.25),   # 障害物1
    (-0.5, 1.0, 0.2),   # 障害物2
    (0.3, -0.6, 0.3),   # 障害物3
]
arm_env = ArmEnvironment(arm, obstacles_arm)

# C空間の衝突マップを可視化
resolution = 200
theta1_range = np.linspace(0, 2 * np.pi, resolution)
theta2_range = np.linspace(0, 2 * np.pi, resolution)
cspace_map = np.zeros((resolution, resolution))

for i, t1 in enumerate(theta1_range):
    for j, t2 in enumerate(theta2_range):
        if not arm_env.is_collision_free((t1, t2)):
            cspace_map[j, i] = 1  # 障害物

fig, axes = plt.subplots(1, 2, figsize=(14, 6))

# 作業空間の可視化
ax = axes[0]
theta_vis = np.linspace(0, 2 * np.pi, 100)
for (cx, cy, r) in obstacles_arm:
    circle = Circle((cx, cy), r, facecolor='gray',
                    edgecolor='black', alpha=0.7)
    ax.add_patch(circle)

# アームの例示（2つの姿勢）
for theta1, theta2, color in [(0.5, 1.0, 'blue'), (2.0, -0.5, 'orange')]:
    base, j1, ee = arm.forward_kinematics(theta1, theta2)
    ax.plot([base[0], j1[0], ee[0]], [base[1], j1[1], ee[1]],
            '-o', color=color, linewidth=3, markersize=6, alpha=0.8)

ax.set_xlim(-2.0, 2.0)
ax.set_ylim(-2.0, 2.0)
ax.set_aspect('equal')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_title('Work Space')
ax.grid(True, alpha=0.3)

# C空間の可視化
ax = axes[1]
ax.imshow(cspace_map, origin='lower', cmap='Greys', alpha=0.7,
          extent=[0, 360, 0, 360])
ax.set_xlabel(r'$\theta_1$ [deg]')
ax.set_ylabel(r'$\theta_2$ [deg]')
ax.set_title('C-Space Obstacle Map')
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.show()

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle

class RRT_CSpace:
    """C空間上でのRRT（2リンクアーム用）"""
    def __init__(self, arm_env, start_config, goal_config,
                 step_size=0.2, goal_bias=0.1,
                 max_iter=5000, goal_threshold=0.2):
        self.arm_env = arm_env
        self.start = np.array(start_config)
        self.goal = np.array(goal_config)
        self.step_size = step_size
        self.goal_bias = goal_bias
        self.max_iter = max_iter
        self.goal_threshold = goal_threshold

        self.nodes = [self.start.copy()]
        self.parent = [-1]

    def random_sample(self):
        if np.random.random() < self.goal_bias:
            return self.goal.copy()
        t1 = np.random.uniform(0, 2 * np.pi)
        t2 = np.random.uniform(0, 2 * np.pi)
        return np.array([t1, t2])

    def nearest_node(self, point):
        dists = [np.linalg.norm(node - point) for node in self.nodes]
        return int(np.argmin(dists))

    def steer(self, from_node, to_point):
        direction = to_point - from_node
        dist = np.linalg.norm(direction)
        if dist <= self.step_size:
            return to_point.copy()
        return from_node + self.step_size * direction / dist

    def plan(self):
        for i in range(self.max_iter):
            q_rand = self.random_sample()
            nearest_idx = self.nearest_node(q_rand)
            q_near = self.nodes[nearest_idx]
            q_new = self.steer(q_near, q_rand)

            if self.arm_env.is_edge_collision_free(q_near, q_new):
                self.nodes.append(q_new)
                self.parent.append(nearest_idx)

                if np.linalg.norm(q_new - self.goal) < self.goal_threshold:
                    if self.arm_env.is_edge_collision_free(q_new, self.goal):
                        self.nodes.append(self.goal.copy())
                        self.parent.append(len(self.nodes) - 2)
                        path = self._extract_path(len(self.nodes) - 1)
                        print(f"C空間RRT 経路発見! イテレーション: {i+1}")
                        return path
        print("C空間RRT: 経路が見つかりませんでした")
        return None

    def _extract_path(self, goal_idx):
        path = []
        idx = goal_idx
        while idx != -1:
            path.append(self.nodes[idx])
            idx = self.parent[idx]
        path.reverse()
        return path

# 関節空間でのRRT実行
np.random.seed(42)
arm = TwoLinkArm(l1=1.0, l2=0.8, base=(0, 0))
obstacles_arm = [
    (0.8, 0.8, 0.25),
    (-0.5, 1.0, 0.2),
    (0.3, -0.6, 0.3),
]
arm_env = ArmEnvironment(arm, obstacles_arm)

start_config = np.array([0.5, 1.0])
goal_config = np.array([2.5, 4.5])

rrt_cspace = RRT_CSpace(arm_env, start_config, goal_config,
                         step_size=0.2, goal_bias=0.1, max_iter=5000)
path_cspace = rrt_cspace.plan()

# C空間上の経路可視化
fig, axes = plt.subplots(1, 2, figsize=(14, 6))

# C空間のプロット
ax = axes[0]
resolution = 200
theta1_range = np.linspace(0, 2 * np.pi, resolution)
theta2_range = np.linspace(0, 2 * np.pi, resolution)
cspace_map = np.zeros((resolution, resolution))
for i, t1 in enumerate(theta1_range):
    for j, t2 in enumerate(theta2_range):
        if not arm_env.is_collision_free((t1, t2)):
            cspace_map[j, i] = 1

ax.imshow(cspace_map, origin='lower', cmap='Greys', alpha=0.5,
          extent=[0, 2*np.pi, 0, 2*np.pi])

# RRTの木を描画
for i in range(1, len(rrt_cspace.nodes)):
    pi = rrt_cspace.parent[i]
    p1 = rrt_cspace.nodes[pi]
    p2 = rrt_cspace.nodes[i]
    ax.plot([p1[0], p2[0]], [p1[1], p2[1]], 'c-',
            linewidth=0.3, alpha=0.4)

if path_cspace is not None:
    path_arr = np.array(path_cspace)
    ax.plot(path_arr[:, 0], path_arr[:, 1], 'r-', linewidth=2.5,
            label='RRT Path')
    ax.plot(path_arr[0, 0], path_arr[0, 1], 'go', markersize=10,
            label='Start')
    ax.plot(path_arr[-1, 0], path_arr[-1, 1], 'r*', markersize=12,
            label='Goal')

ax.set_xlabel(r'$\theta_1$ [rad]')
ax.set_ylabel(r'$\theta_2$ [rad]')
ax.set_title('RRT in C-Space')
ax.legend()
ax.grid(True, alpha=0.3)

# 作業空間上でアームの動きをスナップショットで可視化
ax = axes[1]
for (cx, cy, r) in obstacles_arm:
    circle = Circle((cx, cy), r, facecolor='gray',
                    edgecolor='black', alpha=0.7)
    ax.add_patch(circle)

if path_cspace is not None:
    n_snapshots = min(10, len(path_cspace))
    indices = np.linspace(0, len(path_cspace) - 1,
                          n_snapshots, dtype=int)
    colors = plt.cm.coolwarm(np.linspace(0, 1, n_snapshots))

    for k, idx in enumerate(indices):
        t1, t2 = path_cspace[idx]
        base, j1, ee = arm.forward_kinematics(t1, t2)
        ax.plot([base[0], j1[0], ee[0]], [base[1], j1[1], ee[1]],
                '-o', color=colors[k], linewidth=2, markersize=4,
                alpha=0.6)

    # 始点と終点のアームを強調
    t1_s, t2_s = path_cspace[0]
    base, j1, ee = arm.forward_kinematics(t1_s, t2_s)
    ax.plot([base[0], j1[0], ee[0]], [base[1], j1[1], ee[1]],
            '-o', color='green', linewidth=3, markersize=8,
            label='Start config')

    t1_g, t2_g = path_cspace[-1]
    base, j1, ee = arm.forward_kinematics(t1_g, t2_g)
    ax.plot([base[0], j1[0], ee[0]], [base[1], j1[1], ee[1]],
            '-o', color='red', linewidth=3, markersize=8,
            label='Goal config')

ax.set_xlim(-2.0, 2.0)
ax.set_ylim(-2.0, 2.0)
ax.set_aspect('equal')
ax.set_xlabel('x [m]')
ax.set_ylabel('y [m]')
ax.set_title('Arm Motion in Work Space')
ax.legend(loc='lower left')
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.show()