⛳
RRTによる2リンクアームの障害物回避
RRTによる経路計画
概要と背景
概要
この記事ではRapidly-exploring Random Tree (RRT)による経路計画に基づいて制御することで、障害物との干渉を回避するように手先を目標位置へ制御する方法について説明する。
背景
今回の記事を書こうと思い立ったいきさつとしてはロボットアームの模倣学習や強化学習を勉強するために、ロボットの制御の部分も理解しようと思ったことがきっかけです。このモチベーションから、以前微分逆運動学について調査し自分で実装してみたところ、微分逆運動学では目標姿勢との差を単に小さくするようなロジックなので、「そもそも、障害物があるような環境だとアームと衝突してしまうよな？」と思ったというところです。
想定読者
- 模倣学習/強化学習によるロボットの制御に興味がある方
- ロボットの障害物回避に興味がある方
ロボットの障害物回避
ロボットが障害物を回避しながら初期位置から目標位置へ姿勢を制御する方法に、経路計画によって初期位置から目標位置までの道筋を描き、その後PD制御のような制御則に従って作成した経路を追従することで障害物を回避しながら目標位置へ移動することができます。経路計画の段階で衝突判定を行いながら経路を探索することによって障害物との衝突を回避することができます。
経路計画とは
経路計画とは障害物と衝突しないように「初期姿勢」から「目標姿勢」に至るまでの軌跡を生成することである。
また、経路計画を行う際はどの空間で計画を実行するかによって種類がかかわる。例えば、ロボットの関節角空間における経路計画や手先座標系における経路計画などがある。
経路追従
経路追従とは経路計画アルゴリズムによって生成された軌跡を追従するためのアルゴリズムの総称である。ただし、経路追従はPD制御などの低レイヤの制御則によって実現される。ただし、今回の記事では経路追従については厳密に実装せず、疑似的に経路追従を行っているように可視化するにとどめる。
RRT: Rapidly-exploring Random Tree
経路計画のアルゴリズムは多様なものが提案されているが、本記事ではRapidly-exploring Random Tree (RRT)というアルゴリズムを紹介する。RRTでは、ランダムに探索点を決定し、徐々に探索済みの領域を広げながら目標点まで経路を伸ばすようなアルゴリズムである。また、探索済みの領域を広げる際に障害物との衝突判定を同時に行うことによって最終的に、障害物を回避する経路が見つかる。
アルゴリズム
RRTでは経路を木構造によって表現して、ランダムに探索点を追加していきながら目標姿勢までの経路を生成します。RRTのアルゴリズムでは大まかに以下の処理を目標点との誤差が閾値未満になるまで繰り返します。
- 初期点を選ぶ
- ランダムに新規探索点を選ぶ
- 探索した点のうち最も近いノードを木構造の中から選ぶ
- 選んだ最近傍点から新規探索点に経路を伸ばす
- 伸ばした経路の衝突判定を行う
- 衝突していない場合は、探索済みの点として追加
上記を踏まえると、RRTの大まかな処理手順は以下のフローチャートのようになります。
上記で説明しているようにRRTのアルゴリズムはルールベースなものであり、最適な経路である保証はされていません。しかし、処理内容は軽量で速度も速いということもあり、用いられることがあるそうです。
RRTのアルゴリズム詳細
経路計画部分
ここで探索済みの点を格納する配列を
また、探索点に目標点を選択する確率をεとする。
V=[q_initial],
E=[-1]
for 最大繰り返し数 do:
if random.sample(0, 1) < ε:
q_exp = goal_q # 探索点における各ジョイントの関節角
else:
# q_min: ジョイントがとりうる最小値
# q_max: ジョイントがとりうる最大値
q_exp = random.uniform(q_min, q_max)
nearest_index = sample_nearest(argmin([joint_distance(q_current, q_exp) for q in V]))
q_near=V[nearest_index]
q_new = steer(q_near, q_exp, step_size_radian) # 探索点に向けてわずかに進める
# 衝突判定
if robot_in_collision(q_near, q_new):
# 衝突すると判定された場合、この試行では何もしない
continue
V.append(q_new)
E.append(nearest_index)
経路の平滑化部分
以下にShortcut法による経路平滑化の疑似アルゴリズムを記載する。
smoothed = V.copy()
for 最大繰り返し数 do:
i, j = sort(random.choice(len(smoothed), sample_size=2)) # i < j
if robot_in_collision(i, j):
continue # 衝突する場合は短縮しない
# 衝突しないときは2点間に存在する経由点を削除
smoothed = smoothed[: i+1] + smoothed[j:]
RRTの実装
実装の目標
前提として今回実装したいものは、下記の画像のように2次元平面における2リンクアームの経路計画の結果と計画した経路に従った際の動作イメージを出力するプログラムである。
また、今回実装する経路計画は、関節角空間における経路計画である。
| 経路計画の結果 | 障害物回避のイメージ |
|---|---|
 |
 |
実装するもの
- 順運動学を解く関数:
- 逆運動学：
- RRTによる経路計画：
- Shortcutによる平滑化：
- 衝突判定：
- 描画関係：
- 障害物を表すクラス：
以降は今回の記事の主題である経路計画に関する部分と障害物回避の部分のみを説明する。
RRTによる経路計画の実装
def rrt_plan(
start_q: np.ndarray,
goal_q: np.ndarray,
seed: int = 0,
max_iter: int = 10_000,
step_size_rad: float = math.radians(8.0),
goal_sample_rate: float = 0.15,
goal_threshold_rad: float = math.radians(5.0),
) -> RRTResult:
rng = np.random.default_rng(seed)
if is_robot_in_collision(start_q, margin=COLLISION_MARGIN):
raise RuntimeError("Start configuration is in collision.")
if is_robot_in_collision(goal_q, margin=COLLISION_MARGIN):
raise RuntimeError("Goal configuration is in collision.")
nodes: List[np.ndarray] = [wrap_to_pi(start_q)] # 探索したノード
parents: List[int] = [-1] # 各ノードの親ノードのインデックス。ルートノードは-1。
for it in range(max_iter):
if rng.random() < goal_sample_rate:
q_rand = goal_q
else:
q_rand = rng.uniform(Q_MIN, Q_MAX)
nearest_index = int(np.argmin([joint_distance(q, q_rand) for q in nodes]))
q_near = nodes[nearest_index]
q_new = steer(q_near, q_rand, step_size_rad)
if is_robot_in_collision(q_new, margin=COLLISION_MARGIN):
continue
if is_edge_in_collision(q_near, q_new):
continue
nodes.append(q_new)
parents.append(nearest_index)
new_index = len(nodes) - 1
# Try to connect to the goal.
if joint_distance(q_new, goal_q) < goal_threshold_rad:
if not is_edge_in_collision(q_new, goal_q):
nodes.append(wrap_to_pi(goal_q))
parents.append(new_index)
goal_index = len(nodes) - 1
path = reconstruct_path(nodes, parents, goal_index)
return RRTResult(path=path, nodes=nodes, parents=parents, iterations=it + 1)
raise RuntimeError(f"RRT failed after {max_iter} iterations. Try another seed or more iterations.")
- 入力変数の種類:
-
start_q:初期状態の関節角 -
goal_q:世界座標系における目標位置を実現する各ジョイントの関節角 -
seed: ランダムサンプルを実行する際のシード値 -
max_iter: 経路探索を行う際の最大繰り返し数 -
step_size_rad: 経路探索を行う際のステップサイズ -
goal_sample_rate: 目標点に向けて直線的に経路を探索する確率 -
goal_threshold_rad: 目標点へ到達したと判定する際の閾値
nodesは探索済みの点を格納するための変数であり、parentsは親の探索済み点のインデックスを格納するための変数である。経路計画が完了するとparentsを逆順にたどりながら探索済み点を連結していき、最終的に連結した結果を逆順にすることで初期状態から目標状態への経路を作成することができる。また、is_robot_in_collisionでは、新しい探索点に到達した際にロボットの各ジョイントが障害物と衝突しているか判定し、is_edge_in_collisionでは、新しい探索点までの局所的な経路において、ロボットの各ジョイントが障害物に衝突しないかどうかを判定している。それぞれのメソッドについては以下に示す。
-
衝突判定に関するメソッド
def segments_intersect(a: np.ndarray, b: np.ndarray, c: np.ndarray, d: np.ndarray) -> bool:
"""Inclusive line-segment intersection test."""
o1 = orientation(a, b, c)
o2 = orientation(a, b, d)
o3 = orientation(c, d, a)
o4 = orientation(c, d, b)
if o1 * o2 < 0.0 and o3 * o4 < 0.0:
return True
if abs(o1) < 1e-9 and on_segment(a, b, c):
return True
if abs(o2) < 1e-9 and on_segment(a, b, d):
return True
if abs(o3) < 1e-9 and on_segment(c, d, a):
return True
if abs(o4) < 1e-9 and on_segment(c, d, b):
return True
return False
def segment_intersects_rect(p1: np.ndarray, p2: np.ndarray, obstacle: RectObstacle|CylinderObstacle) -> bool:
if obstacle.contains(p1) or obstacle.contains(p2):
return True
if isinstance(obstacle, RectObstacle):
corners = [
np.array([obstacle.xmin, obstacle.ymin]),
np.array([obstacle.xmax, obstacle.ymin]),
np.array([obstacle.xmax, obstacle.ymax]),
np.array([obstacle.xmin, obstacle.ymax]),
]
edges = list(zip(corners, corners[1:] + corners[:1]))
elif isinstance(obstacle, CylinderObstacle):
# 円柱障害物を多角形近似するための頂点数
n_approx = 16
angles = np.linspace(0, 2.0 * np.pi, n_approx, endpoint=False)
corners = [obstacle.center + obstacle.radius * np.array([math.cos(a), math.sin(a)]) for a in angles]
edges = list(zip(corners, corners[1:] + corners[:1]))
return any(segments_intersect(p1, p2, e1, e2) for e1, e2 in edges)
def is_robot_in_collision(q: np.ndarray, margin: float = 0.0) -> bool:
p0, p1, p2 = forward_kinematics(q)
for obs in OBSTACLES:
inflated = obs.inflated(margin)
if segment_intersects_rect(p0, p1, inflated):
return True
if segment_intersects_rect(p1, p2, inflated):
return True
return False
def is_edge_in_collision(q_from: np.ndarray, q_to: np.ndarray, resolution_rad: float = math.radians(2.0)) -> bool:
"""Check a joint-space edge by interpolating the joint angles."""
dq = angle_diff(q_to, q_from)
n_steps = max(2, int(math.ceil(np.linalg.norm(dq) / resolution_rad)))
for i in range(n_steps + 1):
alpha = i / n_steps
q = wrap_to_pi(q_from + alpha * dq)
if is_robot_in_collision(q, margin=COLLISION_MARGIN):
return True
return False
is_robot_in_collisionでは順運動学によって探索点における各ジョイントの位置を求め、そこからリンク全体の位置を求めリンクが障害物と干渉しているかどうかを調べている。また、is_edge_in_collisionでは新規探索点までの局所的な経路における経由点を密にサンプリングし、各位置におけるジョイントの角度をis_robot_in_collisionに入力して衝突判定を行っている。
Shortcut法による経路の平滑化
def shortcut_smooth_path(
path: List[np.ndarray],
seed: int = 1,
n_trials: int = 300,
) -> List[np.ndarray]:
"""
任意の点を直接結ぶエッジが衝突していない場合、それらの点を結ぶことで経路を短縮・平滑化する関数。
"""
if len(path) <= 2:
return path
rng = np.random.default_rng(seed)
smoothed = path.copy()
for _ in range(n_trials):
i, j = sorted(rng.choice(len(smoothed), size=2, replace=False))
if j <= i + 1:
continue
if not is_edge_in_collision(smoothed[i], smoothed[j]):
smoothed = smoothed[: i + 1] + smoothed[j:]
return smoothed
アルゴリズムの詳細で説明した通り、shortcut法では見つかった経路の経由点集合の中からランダムに2点を選択し、2点間を直接結ぶ局所的な経路を作成し、その局所的な経路における経由点での衝突判定を行い、障害物との干渉がない場合は選択した2点の間に存在するすべての経路点を削除し、2点間を直接結ぶ。
ここまでで経路計画にかかわる部分の実装に関する説明は以上になる。
以下に実際に作成した実装の全体像をいかに示す。
実装全体
from __future__ import annotations
import math
import os
from dataclasses import dataclass
from typing import List, Optional, Tuple
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation, PillowWriter
from matplotlib.patches import Rectangle
# ============================================================
# 1. Problem setting
# ============================================================
# ロボットのジオメトリを定義
L1 = 7.0 # リンク1の長さ
L2 = 5.0 # リンク2の長さ
BASE = np.array([0.0, 0.0]) # 世界座標系におけるベースリンクの位置
START_Q_DEG = np.array([90.0, -45.0]) # 関節角度の初期状態
TARGET_XY = np.array([10.0, -5.0]) # 目標手先座標
# RRT is performed in joint space.
Q_MIN = np.radians(np.array([-180.0, -180.0]))
Q_MAX = np.radians(np.array([180.0, 180.0]))
# A small margin makes paths less likely to graze obstacles.
COLLISION_MARGIN = 0.1 # 障害物の周りに追加するマージン（単位はリンク長の割合）
# 矩形は、左下頂点の座標と幅、高さをパラメータとして定義する。
@dataclass(frozen=True)
class RectObstacle:
x: float
y: float
w: float
h: float
@property
def xmin(self) -> float:
return self.x
@property
def xmax(self) -> float:
return self.x + self.w
@property
def ymin(self) -> float:
return self.y
@property
def ymax(self) -> float:
return self.y + self.h
def inflated(self, margin: float) -> "RectObstacle":
return RectObstacle(
self.x - margin,
self.y - margin,
self.w + 2.0 * margin,
self.h + 2.0 * margin,
)
def contains(self, p: np.ndarray) -> bool:
# 任意の座標 pが矩形の障害物内に存在するかどうかを判定する関数
return self.xmin <= p[0] <= self.xmax and self.ymin <= p[1] <= self.ymax
@dataclass(frozen=True)
class CylinderObstacle:
center_x: float
center_y: float
radius_value: float
@property
def center(self) -> np.ndarray:
return np.array([self.center_x, self.center_y])
@property
def radius(self) -> float:
return self.radius_value
def inflated(self, margin: float) -> "CylinderObstacle":
return CylinderObstacle(
self.center_x,
self.center_y,
self.radius_value + margin,
)
def contains(self, p: np.ndarray) -> bool:
return np.linalg.norm(p - self.center) <= self.radius_value
OBSTACLES: List[RectObstacle] = [
RectObstacle(-5.0, -5.0, 3.0, 6.0),
RectObstacle(-6.0, 6.0, 5.0, 1.0),
RectObstacle(0.0, -4.2, 6.0, 1.0),
RectObstacle(9.2, -2.0, 3.0, 4.0),
CylinderObstacle(7.0, 5.0, 1.5),
CylinderObstacle(7.0, -4.0, .8)
]
# ============================================================
# 2. Kinematics
# ============================================================
def wrap_to_pi(q: np.ndarray) -> np.ndarray:
"""
関節角を指定した範囲に変換する関数。ここでは、角度を[-pi, pi)の範囲に変換。
"""
return (np.asarray(q) + np.pi) % (2.0 * np.pi) - np.pi
def angle_diff(q_to: np.ndarray, q_from: np.ndarray) -> np.ndarray:
"""
目標関節角q_toと現在の関節角q_fromの角度差を計算する関数。
"""
return wrap_to_pi(np.asarray(q_to) - np.asarray(q_from))
def joint_distance(q1: np.ndarray, q2: np.ndarray) -> float:
"""関節角空間でのユークリッド距離を計算する関数。"""
return float(np.linalg.norm(angle_diff(q1, q2)))
def forward_kinematics(q: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
"""
順運動学を解いて、ベース、中間ジョイント、手先の位置を取得するメソッド
"""
q1, q2 = q
p0 = BASE
p1 = np.array([L1 * math.cos(q1), L1 * math.sin(q1)])
p2 = p1 + np.array([L2 * math.cos(q1 + q2), L2 * math.sin(q1 + q2)])
return p0, p1, p2
def inverse_kinematics_2link(
target_xy: np.ndarray,
elbow_sign: int,
) -> Optional[np.ndarray]:
"""
2リンクアームの逆運動学を解く関数。
elbow_sign:
- +1: エルボーアップ（肘が上に曲がる解）
- -1: エルボーダウン（肘が下に曲がる解）
"""
x, y = float(target_xy[0]), float(target_xy[1])
r2 = x * x + y * y
c2 = (r2 - L1 * L1 - L2 * L2) / (2.0 * L1 * L2)
if c2 < -1.0 or c2 > 1.0:
return None
c2 = float(np.clip(c2, -1.0, 1.0))
q2 = elbow_sign * math.acos(c2)
q1 = math.atan2(y, x) - math.atan2(L2 * math.sin(q2), L1 + L2 * math.cos(q2))
return wrap_to_pi(np.array([q1, q2]))
def choose_collision_free_ik(target_xy: np.ndarray) -> np.ndarray:
"""
目標座標に対して衝突のない逆運動学解を選択する関数。
"""
candidates = []
for elbow_sign in (+1, -1):
q = inverse_kinematics_2link(target_xy, elbow_sign)
if q is not None:
candidates.append(q)
if not candidates:
raise RuntimeError("Target is outside the reachable workspace.")
collision_free = [q for q in candidates if not is_robot_in_collision(q, margin=COLLISION_MARGIN)]
if not collision_free:
msg = "Target is reachable by IK, but all IK solutions are in collision."
raise RuntimeError(msg)
# In this example, the elbow-down solution is collision-free.
return collision_free[-1]
# ============================================================
# 3. Collision checking
# ============================================================
def orientation(a: np.ndarray, b: np.ndarray, c: np.ndarray) -> float:
return float((b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]))
def on_segment(a: np.ndarray, b: np.ndarray, c: np.ndarray) -> bool:
return (
min(a[0], b[0]) - 1e-9 <= c[0] <= max(a[0], b[0]) + 1e-9
and min(a[1], b[1]) - 1e-9 <= c[1] <= max(a[1], b[1]) + 1e-9
)
def segments_intersect(a: np.ndarray, b: np.ndarray, c: np.ndarray, d: np.ndarray) -> bool:
"""Inclusive line-segment intersection test."""
o1 = orientation(a, b, c)
o2 = orientation(a, b, d)
o3 = orientation(c, d, a)
o4 = orientation(c, d, b)
if o1 * o2 < 0.0 and o3 * o4 < 0.0:
return True
if abs(o1) < 1e-9 and on_segment(a, b, c):
return True
if abs(o2) < 1e-9 and on_segment(a, b, d):
return True
if abs(o3) < 1e-9 and on_segment(c, d, a):
return True
if abs(o4) < 1e-9 and on_segment(c, d, b):
return True
return False
def segment_intersects_rect(p1: np.ndarray, p2: np.ndarray, obstacle: RectObstacle|CylinderObstacle) -> bool:
if obstacle.contains(p1) or obstacle.contains(p2):
return True
if isinstance(obstacle, RectObstacle):
corners = [
np.array([obstacle.xmin, obstacle.ymin]),
np.array([obstacle.xmax, obstacle.ymin]),
np.array([obstacle.xmax, obstacle.ymax]),
np.array([obstacle.xmin, obstacle.ymax]),
]
edges = list(zip(corners, corners[1:] + corners[:1]))
elif isinstance(obstacle, CylinderObstacle):
# 円柱障害物を多角形近似するための頂点数
n_approx = 16
angles = np.linspace(0, 2.0 * np.pi, n_approx, endpoint=False)
corners = [obstacle.center + obstacle.radius * np.array([math.cos(a), math.sin(a)]) for a in angles]
edges = list(zip(corners, corners[1:] + corners[:1]))
return any(segments_intersect(p1, p2, e1, e2) for e1, e2 in edges)
def is_robot_in_collision(q: np.ndarray, margin: float = 0.0) -> bool:
p0, p1, p2 = forward_kinematics(q)
for obs in OBSTACLES:
inflated = obs.inflated(margin)
if segment_intersects_rect(p0, p1, inflated):
return True
if segment_intersects_rect(p1, p2, inflated):
return True
return False
def is_edge_in_collision(q_from: np.ndarray, q_to: np.ndarray, resolution_rad: float = math.radians(2.0)) -> bool:
"""Check a joint-space edge by interpolating the joint angles."""
dq = angle_diff(q_to, q_from)
n_steps = max(2, int(math.ceil(np.linalg.norm(dq) / resolution_rad)))
for i in range(n_steps + 1):
alpha = i / n_steps
q = wrap_to_pi(q_from + alpha * dq)
if is_robot_in_collision(q, margin=COLLISION_MARGIN):
return True
return False
# ============================================================
# 4. RRT planner in joint space
# ============================================================
@dataclass
class RRTResult:
path: List[np.ndarray]
nodes: List[np.ndarray]
parents: List[int]
iterations: int
def steer(q_from: np.ndarray, q_to: np.ndarray, step_size_rad: float) -> np.ndarray:
# q_fromからq_toに向かって、step_size_radだけ関節空間で移動した新しい関節角度を返す関数
# 一気に動かすと衝突が発生する可能性があるため。
dq = angle_diff(q_to, q_from)
dist = np.linalg.norm(dq)
if dist <= step_size_rad:
return wrap_to_pi(q_to)
return wrap_to_pi(q_from + step_size_rad * dq / dist)
def reconstruct_path(nodes: List[np.ndarray], parents: List[int], goal_index: int) -> List[np.ndarray]:
path = []
idx = goal_index
while idx != -1:
path.append(nodes[idx])
idx = parents[idx]
return path[::-1]
def rrt_plan(
start_q: np.ndarray,
goal_q: np.ndarray,
seed: int = 0,
max_iter: int = 10_000,
step_size_rad: float = math.radians(8.0),
goal_sample_rate: float = 0.15,
goal_threshold_rad: float = math.radians(5.0),
) -> RRTResult:
rng = np.random.default_rng(seed)
if is_robot_in_collision(start_q, margin=COLLISION_MARGIN):
raise RuntimeError("Start configuration is in collision.")
if is_robot_in_collision(goal_q, margin=COLLISION_MARGIN):
raise RuntimeError("Goal configuration is in collision.")
nodes: List[np.ndarray] = [wrap_to_pi(start_q)] # 探索したノード
parents: List[int] = [-1] # 各ノードの親ノードのインデックス。ルートノードは-1。
for it in range(max_iter):
if rng.random() < goal_sample_rate:
q_rand = goal_q
else:
q_rand = rng.uniform(Q_MIN, Q_MAX)
nearest_index = int(np.argmin([joint_distance(q, q_rand) for q in nodes]))
q_near = nodes[nearest_index]
q_new = steer(q_near, q_rand, step_size_rad)
if is_robot_in_collision(q_new, margin=COLLISION_MARGIN):
continue
if is_edge_in_collision(q_near, q_new):
continue
nodes.append(q_new)
parents.append(nearest_index)
new_index = len(nodes) - 1
# Try to connect to the goal.
if joint_distance(q_new, goal_q) < goal_threshold_rad:
if not is_edge_in_collision(q_new, goal_q):
nodes.append(wrap_to_pi(goal_q))
parents.append(new_index)
goal_index = len(nodes) - 1
path = reconstruct_path(nodes, parents, goal_index)
return RRTResult(path=path, nodes=nodes, parents=parents, iterations=it + 1)
raise RuntimeError(f"RRT failed after {max_iter} iterations. Try another seed or more iterations.")
def shortcut_smooth_path(
path: List[np.ndarray],
seed: int = 1,
n_trials: int = 300,
) -> List[np.ndarray]:
"""
任意の点を直接結ぶエッジが衝突していない場合、それらの点を結ぶことで経路を短縮・平滑化する関数。
"""
if len(path) <= 2:
return path
rng = np.random.default_rng(seed)
smoothed = path.copy()
for _ in range(n_trials):
i, j = sorted(rng.choice(len(smoothed), size=2, replace=False))
if j <= i + 1:
continue
if not is_edge_in_collision(smoothed[i], smoothed[j]):
smoothed = smoothed[: i + 1] + smoothed[j:]
return smoothed
def densify_path(path: List[np.ndarray], resolution_rad: float = math.radians(1.5)) -> List[np.ndarray]:
"""
RRTで得た疎な経路を密に補完するメソッド
"""
dense = []
for q0, q1 in zip(path[:-1], path[1:]):
dq = angle_diff(q1, q0)
n_steps = max(2, int(math.ceil(np.linalg.norm(dq) / resolution_rad)))
for i in range(n_steps):
alpha = i / n_steps
dense.append(wrap_to_pi(q0 + alpha * dq))
dense.append(wrap_to_pi(path[-1]))
return dense
# ============================================================
# 5. Visualization
# ============================================================
def draw_obstacles(ax: plt.Axes) -> None:
for obs in OBSTACLES:
if isinstance(obs, RectObstacle):
patch = Rectangle(
(obs.x, obs.y),
obs.w,
obs.h,
facecolor="0.7",
edgecolor="0.35",
alpha=0.9,
)
ax.add_patch(patch)
elif isinstance(obs, CylinderObstacle):
circle = plt.Circle(
(obs.center_x, obs.center_y),
obs.radius,
facecolor="0.7",
edgecolor="0.35",
alpha=0.9,
)
ax.add_patch(circle)
else:
raise ValueError(f"Unknown obstacle type: {type(obs)}")
def set_workspace_axes(ax: plt.Axes, title: str) -> None:
ax.set_title(title)
ax.set_aspect("equal", adjustable="box")
ax.set_xlim(-7, 13)
ax.set_ylim(-7, 13)
ax.grid(True)
def end_effector_xy(q: np.ndarray) -> np.ndarray:
return forward_kinematics(q)[2]
def save_rrt_plot(
result: RRTResult,
dense_path: List[np.ndarray],
output_path: str,
) -> None:
fig, ax = plt.subplots(figsize=(8, 7))
draw_obstacles(ax)
set_workspace_axes(ax, "RRT Path Planning")
# RRT tree, projected from joint space into end-effector workspace.
for i, parent in enumerate(result.parents):
if parent == -1:
continue
p_parent = end_effector_xy(result.nodes[parent])
p_child = end_effector_xy(result.nodes[i])
ax.plot(
[p_parent[0], p_child[0]],
[p_parent[1], p_child[1]],
color="green",
alpha=0.25,
linewidth=0.8,
)
ee_path = np.array([end_effector_xy(q) for q in dense_path])
ax.plot(ee_path[:, 0], ee_path[:, 1], "r-", linewidth=3, label="path")
start_xy = end_effector_xy(result.path[0])
goal_xy = end_effector_xy(result.path[-1])
ax.plot(start_xy[0], start_xy[1], "bo", markersize=12, label="start")
ax.plot(goal_xy[0], goal_xy[1], "r*", markersize=18, label="goal")
ax.legend(loc="upper right")
fig.tight_layout()
fig.savefig(output_path, dpi=160)
plt.close(fig)
def save_animation(
dense_path: List[np.ndarray],
mp4_path: str,
) -> None:
ee_trace = []
fig, ax = plt.subplots(figsize=(6, 6))
draw_obstacles(ax)
set_workspace_axes(ax, "2-link arm: RRT obstacle avoidance")
target_handle = ax.plot(TARGET_XY[0], TARGET_XY[1], "rx", markersize=10, mew=2, label="TARGET")[0]
link_line, = ax.plot([], [], "o-", linewidth=3, markersize=6, label="LINK")
trace_line, = ax.plot([], [], "-", linewidth=2, label="Trajectory")
title = ax.text(0.5, 1.02, "", transform=ax.transAxes, ha="center")
ax.legend(loc="upper right")
def init():
link_line.set_data([], [])
trace_line.set_data([], [])
title.set_text("")
return link_line, trace_line, title, target_handle
def update(frame: int):
q = dense_path[frame]
p0, p1, p2 = forward_kinematics(q)
xs = [p0[0], p1[0], p2[0]]
ys = [p0[1], p1[1], p2[1]]
link_line.set_data(xs, ys)
ee_trace.append(p2.copy())
trace = np.array(ee_trace)
trace_line.set_data(trace[:, 0], trace[:, 1])
return link_line, trace_line, title, target_handle
anim = FuncAnimation(
fig,
update,
frames=len(dense_path),
init_func=init,
interval=50,
blit=False,
)
# MP4 requires ffmpeg. If it is unavailable, only the GIF is saved.
try:
anim.save(mp4_path, fps=20)
except Exception as exc: # pragma: no cover - environment dependent
print(f"[INFO] MP4 was not saved because ffmpeg is unavailable or failed: {exc}")
plt.close(fig)
# ============================================================
# 6. Main
# ============================================================
def main() -> None:
output_dir = os.path.join(os.path.dirname(__file__), "outputs")
os.makedirs(output_dir, exist_ok=True)
start_q = wrap_to_pi(np.radians(START_Q_DEG)) # degree形式をradian形式に変換して、[-pi, pi)の範囲にラップする
goal_q = choose_collision_free_ik(TARGET_XY)
print("start_q [deg] =", np.degrees(start_q))
print("goal_q [deg] =", np.degrees(goal_q))
print("target_xy =", TARGET_XY)
# rrtの実行による経路計画
result = rrt_plan(
start_q=start_q,
goal_q=goal_q,
seed=0,
max_iter=20_000,
step_size_rad=math.radians(8.0),
goal_sample_rate=0.15,
goal_threshold_rad=math.radians(1.0),
)
print(f"RRT succeeded: iterations={result.iterations}, nodes={len(result.nodes)}, raw_path={len(result.path)}")
# rrtの経路計画の結果を平滑化
smoothed_path = shortcut_smooth_path(result.path, seed=1, n_trials=300)
dense_path = densify_path(smoothed_path, resolution_rad=math.radians(1.5))
rrt_plot_path = os.path.join(output_dir, "rrt_path_planning.png")
mp4_path = os.path.join(output_dir, "two_link_rrt_animation.mp4")
save_rrt_plot(result, dense_path, rrt_plot_path)
save_animation(dense_path, mp4_path)
print("saved:")
print(" -", rrt_plot_path)
if os.path.exists(mp4_path):
print(" -", mp4_path)
if __name__ == "__main__":
main()
最後に
今回はRRTによる経路計画を説明し、生成された経路に従ってアームを動かせば、障害物を回避できることを確認した。今回は経路追従の部分は実際には行っていないことや、シンプルな2次元平面上での2リンクアームの経路計画しか行っていません。
今後は以前の記事のようにmujocoをシミュレーションのバックエンドに6自由度アームの経路計画と制御に取り組んでみたいと思います。
さらに、今回は環境のモデルが既知という仮定で、どこが障害物か？という情報が事前に取得できるという前提で説明しました。しかし、実際はSLAMなどの技術と組み合わせて環境のモデルを推定し、そのうえで経路計画や経路追従を行う必要があるため、それらの技術とのインテグレーションまで行いたいと思います！
また、今回の記事以外にロボットアームの制御や強化学習関連で以下のような記事を書いています。興味があれば覗いていただけると幸いです！
制御関連
強化学習関連
以上、ここまで読んでいただきありがとうございます！
少しでもロボットの制御やロボットの強化学習に関して興味のある方の手助けになれば幸いです！
Discussion