はじめに
私がロボットに関して興味を持っており，ロボットの経路を作成したいと思ったところ，RRT (Rapidly-exploring Random Tree) と呼ばれる経路生成の手法に辿り着いた．
本記事では，経路生成の古典的手法である RRT (Rapidly-exploring Random Tree) を改良した RRT*-Smart をPythonより，実装してみる．
前記事では，球の干渉物が存在する3次元空間を探索する RRT* のアニメーションを実装した．
前記事の内容を下記に記します．
・https://qiita.com/haruhiro1020/items/ad6d86c65adde3e2d1e6
本記事では，円形と長方形の干渉物が存在する2次元空間を探索する RRT*-Smart を実装する．
RRT*-Smart はRRT* の探索範囲を効率化(Intelligent Sampling) + 経路最適化(Path Optimization) を実装したアルゴリズムである．
RRTとRRT* の違いは以下記事で説明したため，本記事では割愛する．
・https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b
RRT*-Smart は下記を参考にした．
参考文献：
J.Nasir, F.Islam et al. "RRT*-SMART: A Rapid Convergence Implementation of RRT*", International Journal of Advanced Robotic Systems 2013
本記事では，RRT* とRRT*-Smart の違いを説明したのち，ソースコードおよび性能評価を実施する．
本記事で経路生成したい環境は下図の通りである．
2軸ロボットアームに関しては，下記リンクに記載．
・https://qiita.com/haruhiro1020/items/b42725df00e13ddcb5af
python-fclに関しては，下記リンクに記載．
・https://qiita.com/haruhiro1020/items/d364e4c56b1570e1423a
本記事で実装すること
・円形と長方形の干渉物が存在する2次元空間で，2軸ロボットアームを使用してRRT*-Smart による経路生成
・RRT* とRRT*-Smart の経路生成結果をアニメーションで表現する
本記事では実装できないこと(将来実装したい内容)
・球と直方体の干渉物が存在する3次元空間でのRRT*-Smart による経路生成
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
・python-fcl (4.1.2) (干渉判定用ライブラリ)
経路生成手法のまとめ
経路生成手法をまとめた記事を展開いたします．本記事以外の手法も記載してますので，参照いただけますと幸いです．
(https://qiita.com/haruhiro1020/items/000b3287fce082ae4283)
ソースコードに関して
投稿しているソースコードは綺麗でないため，随時更新しようと考えております．
また，処理時間を考慮した作りになっていないため，処理時間が高速となる作り方を勉強した後にソースコードを更新しようと考えております．
経路生成アルゴリズムに関して
経路生成のアルゴリズムを説明する．
RRTに関して
RRTアルゴリズムは以下記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)
RRT* に関して
RRT* アルゴリズムは以下記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/7c5bdbe530711c9d331b)
RRT*-Smart に関して
RRT*-Smart アルゴリズムに関して説明する．
参考文献に記載されているRRT*-Smart のアルゴリズムを抜粋して，下記に記す．
RRT* との差異を赤枠で記載した．
各赤枠内の内容を説明する．
赤枠①：
4行目では一定回数(if条件成立時)で，サンプリング手法を変更する．Intelligent Sampling(効率化した探索範囲内でのサンプリング)というサンプリング手法を実装する．Intelligent Samplingに関しては後ほど詳細を説明するが，簡単に言うと$z_{beacons}$を中心とした球内を探索範囲とするサンプリング手法である．4行目のnやbに関しても後ほど説明する．
6行目のelseの場合は，RRT*と同様のランダムサンプリングを実装する．
赤枠②：
15行目では始点から終点までの経路生成に初めて成功したら，16行目でnにiを代入する．要するにnは始点から終点までの経路生成に初めて成功した探索回数が保存される．
17行目ではPath Optimization(経路最適化)と呼ばれる手法を実装して，ツリーの更新および始点から終点までの経路の最小コストを取得する．Path Optimizationに関しては後ほど詳細を説明するが，簡単に言うとあるノードの親ノードをあるノードの親ノードの親ノードに修正して，経路を最適化する手法である．
18行目では始点から終点までの経路の最小コストが以前の最小コストよりも小さければ，19行目のPath Optimization(経路最適化)を実装して，Intelligent Samplingで使用する$z_{beacons}$を計算する．
RRT*-Smart は始点から終点までの経路が初めて生成できるまでは，RRT* と同様の処理を実装する．初めて経路が生成できたら，一定回数でIntelligent Sampling，Path Optimizationを実装する．始点から終点までの最小コストが以前よりも小さくなれば，Intelligent Samplingで使用する$z_{beacons}$を更新する．
RRT*-Smart は下図のようなアルゴリズムである．
上図まではRRT* アルゴリズムである．
下図からは始点から終点までの経路が初めて生成できた後の処理であり，RRT*-Smart 特有アルゴリズムである．
Intelligent SamplingやPath Optimizationに関しての詳細に関しては，次で説明する．
RRT*-Smart のおおまかなアルゴリズムは上図の通りとなる．
Intelligent Sampling
Intelligent Sampling について説明する．
Intelligent Sampling はサンプリングする時の探索範囲を効率的にするための手法である．
RRT* でのサンプリングは，全探索範囲の中をランダムで探索している．
Intelligent Sampling は$z_{beacons}$と呼ばれる，複数の経由点から1点を選択して，選択した点を中心点として，半径$R_{beacons}$の球内をサンプリングする．
ここで，$z_{beacons}$は始点から終点までの経路の内，最小コストとなる経路の経由点(始点・終点を除く)を指している．
ここからは，複数の経由点が保存されている$z_{beacons}$から，1点を選択する方法および球の半径$R_{beacons}$を決める方法を説明する．
z_{beacons}から1点を選択する方法
複数の経由点が保存されている$z_{beacons}$から，1点を選択する方法を説明する．
案としては下表の3点が挙げられる．
| 案 | 概要 | メリット | デメリット |
|---|---|---|---|
| 1 | ランダム | 選択する点の偏りが少ない | 効率的ではない |
| 2 | 干渉物に近い点 | 干渉物近傍を最適化できる | 選択する点の偏りが大きい |
| 3 | ランダム + 干渉物に近い点(探索回数が多くなるほど干渉物に近い点を選択する確率を高くする) | 選択する点の偏りが少なく，干渉物近傍も最適化できる | 確率の調整が困難 |
上表より本記事では，案3を採用する．
下式のような確率でランダムおよび干渉物に近い点を選択する．
\displaylines{
p = min(0.8, 0.002 * (i - n) \\
p ... 干渉物に近い点を選択する確率 \\
1 - p ... ランダムで選択する確率 \\
i ... RRT*-Smart の探索回数 \\
n ... 始点から終点までの経路生成に成功した時の探索回数 \\
}
初めのうちは$p$が小さいため，ランダムで選択する確率は高くなる．
探索回数を増やしていくと$p$が大きくなるため，干渉物に近い点を選択する確率は高くなる．
次に，干渉物に近い点をどうやって計算するかについて説明する．
python-fcl(干渉判定用ライブラリ)を使用して，干渉物との距離を計算する．python-fclに干渉物との距離を計算できるメソッドがあるため，使用する．
python-fclの使用方法は，下記リンクに記載した．
(python-fclのリンク https://qiita.com/haruhiro1020/items/d364e4c56b1570e1423a)
球の半径$R_{beacons}$を決める方法
球の半径$R_{beacons}$を決める方法を説明する．
案としては下表の3点が挙げられる．
| 案 | 概要 | メリット | デメリット |
|---|---|---|---|
| 1 | 小さい値の固定値 | 局所的な改善ができる | 環境の大きさに応じて，毎回手動で修正 |
| 2 | 始点から終点までの経路コスト * 係数(0.2程度) | 半径が自動で修正 | 経路コストが変わらないときは固定値 |
| 3 | 始点から終点までの経路コスト * 係数(0.2程度) * (1 - min(0.5, (i - n) * 0.001)) | 探索回数が増えると半径が自動で小さくなる | パラメータ調整が困難 |
上表より本記事では，案3を採用する(案3の$i$は探索回数で，$n$は始点から終点までの経路が初めて生成できた探索回数である)．
これらより，Intelligent Sampling を実装していく．
Path Optimization
Path Optimization について説明する．
Path Optimization は経路を最適化するための手法である．
手法およびアルゴリズムを論文から抜粋したのが，下図である．
あるノード$z_{node}$の親ノード($z_{parent}$)を親ノードの親ノード($z_{parent-to-parent}$)にしても干渉がなければ，親ノードを修正するアルゴリズムである．
これを全ノードに対して，実装するのがPath Optimization である．
RRT*-Smartに関する理論の説明をしたため，次はソースコードの説明を実施する．
ソースコードの説明
Pythonにて，RRT*-Smart のソースコードを作成する．
定数の定義 (constant.py)
複数ファイルで使用する定数を定義したファイルは下記の通りです．
constant.py
# 複数ファイルで使用する定数の定義
from enum import Enum
from enum import auto
# 次元数を定義
DIMENTION_NONE = -1 # 未定義
DIMENTION_2D = 2 # 2次元
DIMENTION_3D = 3 # 3次元
# ノードの最短ノード要素とコスト要素を定義
RRT_NEAR_NODE_IDX = -1 # RRTでの最短ノード要素
RRT_CONNECT_NEAR_NODE_IDX = -1 # RRT-Connectでの最短ノード要素
RRT_STAR_NEAR_NODE_IDX = -3 # RRT*での最短ノード勝訴
RRT_STAR_COST_IDX = -2 # RRT*でのコスト要素
RRT_STAR_RADIUS_IDX = -1 # RRT*での半径要素
INFORMED_RRT_STAR_NEAR_NODE_IDX = -3 # Informed RRT*での最短ノード勝訴
INFORMED_RRT_STAR_COST_IDX = -2 # Informed RRT*でのコスト要素
INFORMED_RRT_STAR_RADIUS_IDX = -1 # Informed RRT*での半径要素
RRT_STAR_SMART_NEAR_NODE_IDX = -3 # RRT*-Smartでの最短ノード勝訴
RRT_STAR_SMART_COST_IDX = -2 # RRT*-Smartでのコスト要素
RRT_STAR_SMART_RADIUS_IDX = -1 # RRT*-Smartでの半径要素
# 回転軸
ROTATION_X_AXIS = "rot_x" # x軸周りに回転
ROTATION_Y_AXIS = "rot_y" # y軸周りに回転
ROTATION_Z_AXIS = "rot_z" # z軸周りに回転
# ツリーの初期ノードの親ノード
INITIAL_NODE_NEAR_NODE = -1 # 初期ノードに親ノードが存在しないから-1
# 補間時の分割する時の距離
DEVIDED_DISTANCE_JOINT = 0.1 # 関節補間時の距離 [rad]
DEVIDED_DISTANCE_POSIION = 0.1 # 位置補間時の距離 [m]
# 干渉物の名称を定義
class INTERFERENCE(Enum):
"""
干渉物の名称を定義
"""
NONE = "" # 未定義
# 2次元の干渉物 ↓
CIRCLE = "circle" # 円形の干渉物
RECTANGLE = "rectangle" # 長方形の干渉物
LINE2D = "line2D" # 2次元の直線
# 2次元の干渉物 ↑
# 3次元の干渉物 ↓
BALL = "ball" # 球の干渉物
CUBOID = "cuboid" # 直方体の干渉物
LINE3D = "line3D" # 3次元の直線
# 3次元の干渉物 ↑
# 0割を防ぐための定数
EPSILON = 1e-6
# プロットする時にグラフ(静止画)とするかアニメーションを定義
PLOT_NONE = 20 # プロットしない
PLOT_GRAPH = 21 # グラフ
PLOT_ANIMATION = 22 # アニメーション
# 補間方法の定義
class INTERPOLATION(Enum):
"""
補間方法
"""
JOINT = 10 # 関節補間
POSITION = auto() # 位置補間
LINEAR = auto() # 直線補間
CUBIC = auto() # 3次補間
QUINTIC = auto() # 5次補間
# RRTで作成した経路の修正方法を定義
class MODIFICATION(Enum):
"""
修正方法
"""
# 修正方法が単一
PRUNING_START = "pruning_start" # 始点からのPruning
PRUNING_END = "pruning_end" # 終点からのPruning
PRUNING_RANDOM = "pruning_random" # ランダム点のPruning
SHORTCUT_NORMAL = "shortcut" # Shortcut
SHORTCUT_PARTIAL = "partial_shortcut" # Partial Shortcut
# 修正方法の組み合わせ
PARTIAL_START = "partial_start" # Partial Shortcut + 始点からのPruning
RRT*-Smart による経路生成 (rrt.py)
上で説明した，RRT*-Smart ソースコードは下記の通りです．
下記の RRTStarSmart クラスを新規作成した．
RRT* クラスを継承している．
rrt.py
# 経路生成手法であるRRT (Rapidly-exploring Random Tree) の実装
# ライブラリの読み込み
import numpy as np
import time
import os
import fcl
# 自作モジュールの読み込み
from constant import * # 定数
class Tree:
"""
ツリークラス
プロパティ
_nodes(numpy.ndarray): ノード
_near_node_idx(int): _nodes内の最短ノードを保存している列番号
メソッド
public
nodes(): _nodesプロパティのゲッター
add_node(): ノードの追加
get_near_node(): 最短距離のノードを取得
"""
# 定数の定義
def __init__(self, near_node_idx):
"""
コンストラクタ
"""
# プロパティの初期化
self._nodes = []
self._near_node_idx = near_node_idx
@property
def nodes(self):
"""
_nodesプロパティのゲッター
"""
return self._nodes
def reset(self):
"""
データの初期化
"""
if len(self._nodes) != 0:
# 何かしらのデータが保存
del self._nodes
self._nodes =[]
def add_node(self, node):
"""
ノードの追加
パラメータ
node(numpy.ndarray): ノード
"""
if len(self._nodes) == 0: # 初回だけ実行
self._nodes = node
else:
self._nodes = np.append(self._nodes, node, axis=0)
def _chk_node_exist(self):
"""
ノードが存在するかの確認
"""
if len(self._nodes) == 0:
# 存在しない
raise ValueError("self._nodes is not exist")
def get_near_node(self, pos):
"""
最短距離のノードを取得
パラメータ
pos(numpy.ndarray): 位置
戻り値
min_dist_idx(int): 最短距離のノード番号
"""
# ノードの存在確認
self._chk_node_exist()
# ノードから位置を取得
nodes_pos = self._nodes[:, :self._near_node_idx]
# 差分を計算
difference = nodes_pos - pos
# 距離を計算 (各ノードとの距離を算出するため，引数にaxis=1を与えた)
distance = np.linalg.norm(difference, axis=1)
# 最短距離ノードを取得
min_dist_idx = np.argmin(distance)
return min_dist_idx
def get_near_node_list(self, pos, radius):
"""
ノードと近傍ノードをリストで取得
パラメータ
pos(numpy.ndarray): ノード位置
radius(float): 半径
戻り値
near_node_list(list): 近傍ノードリスト
"""
# ノードの存在確認
self._chk_node_exist()
near_node_list = []
# ツリー内全ノード位置を取得
all_node_pos = self._nodes[:, :self._near_node_idx]
# ノードとツリー内全ノードの差分を計算
difference = all_node_pos - pos
# 差分から距離(ユークリッド距離)を計算
distance = np.linalg.norm(difference, axis=1)
# 距離が一定以内のノードだけを保存
near_node_list = [idx for idx, dist in enumerate(distance) if dist <= radius]
return near_node_list
class RRTRobot:
"""
RRTにロボットを追加したクラス
プロパティ
_pathes(numpy.ndarray): 始点から終点までの経路 (PruningやShortcut済みの経路)
_start_tree(Tree): 始点ツリー
_start_random_tree(Tree): ランダムな位置を保存した始点ツリー
_strict_min_pos(numpy.ndarray): 探索の最小範囲
_strict_max_pos(numpy.ndarray): 探索の最大範囲
_environment(environment.py内のクラス): 経路生成したい環境
_debug(bool): デバッグフラグ (グラフやアニメーションでデータを確認したいかどうか)
_robot(robot.py内のクラス): ロボットクラス
_moving_value(float): 1回あたりの移動量 [rad] or [m]
_before_modify_pathes(numpy.ndarray): PruningやShortcutなどの修正前の経路
_pruning_pathes(numpy.ndarray): Pruning後の経路
_shortcut_pathes(numpy.ndarray): Shortcut後の経路
_partial_shortcut_pathes(numpy.ndarray): Partial Shortcut後の経路
メソッド
public
planning(): 経路生成の実装
pathes(): _pathesプロパティのゲッター
start_tree(): _treeプロパティのゲッター
start_random_tree(): _random_treeプロパティのゲッター
strict_min_pos(): _strict_min_posプロパティのゲッター
strict_max_pos(): _strict_max_posプロパティのゲッター
environment(): _environmentプロパティのゲッター
protected
_reset(): データの初期化
_add_node(): ツリーにノードを追加する
_chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
_fin_planning(): 経路生成の終了処理
_calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
_strict_planning_pos(): 探索範囲を制限する
_get_random_pos(): ランダムな位置を取得
_line_interference(): 2点間の干渉チェック
_is_interference_pos(): 干渉判定処理
_save(): データの保存
"""
# 定数の定義
# ファイル名の定義
_FILE_NAME_PATHES = "rrt_pathes.csv" # _pathesプロパティを保存するファイル名
_FILE_NAME_START_TREE = "rrt_start_tree.csv" # _start_treeプロパティを保存するファイル名
_FILE_NAME_START_RANDOM_TREE = "rrt_start_random_tree.csv" # _start_random_treeプロパティを保存するファイル名
_FILE_NAME_BEFORE_MODIFY_PATHES = "rrt_before_modify_pathes.csv" # _before_modify_pathesプロパティを保存するファイル名
_FILE_NAME_PRUNING_PATHES = "rrt_pruning_pathes.csv" # _pruning_pathesプロパティを保存するファイル名
_FILE_NAME_SHORTCUT_PATHES = "rrt_shortcut_pathes.csv" # _shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_PARTIAL_SHORTCUT_PATHES = "rrt_partial_shortcut_pathes.csv" # _partial_shortcut_pathesプロパティを保存するファイル名
# 毎処理保存するファイル名の定義
_FILE_NAME_PRUING_EACH_PATHES = "rrt_pruning_each_pathes" # Pruning実施毎の経路を保存
_FILE_NAME_SHORTCUT_EACH_PATHES = "rrt_shortcut_each_pathes" # Shortcut実施毎の経路を保存
_FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = "rrt_partial_shortcut_each_pathes" # Partial Shortcut実施毎の経路を保存
# フォルダー名の定義
_FOLODER_NAME_PRUNING = "Pruning" # Pruning結果を保存するフォルダ
_FOLODER_NAME_SHORTCUT = "Shortcut" # Shortcut結果を保存するフォルダ
_FOLODER_NAME_PARTIAL_SHORTCUT = "PartialShortcut" # PartialShortcut結果を保存するフォルダ
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX # ノード内の最短ノード要素
# 干渉判定の定義
_DEVIDED_DISTANCE_POS = 0.01 # 2点間を分割する際の基準距離 [m]
_DEVIDED_DISTANCE_JOINT = 0.005 # 2点間を分割する際の基準距離 [rad]
_INTERFERENCE_MARGIN = 0.05 # 干渉物とのマージン (最短距離がマージンよりも小さかったら干渉ありとする) [m]
# 探索に関する定義
_MOVING_VALUE_JOINT = 0.1 # 1回の移動量 [rad] (ロボットの関節空間)
_MOVING_VALUE_POS = 0.2 # 1回の移動量 [m] (ロボットの位置空間)
_STRICT_PLANNING_ROB_JOINT = np.pi / 4 # 探索範囲の制限 [rad] (ロボットの関節空間)
_STRICT_PLANNING_ROB_POS = 1.0 # 探索範囲の制限 [m] (ロボットの位置空間)
_TIMEOUT_VALUE = 10 # タイムアウト時間 [second]
_GOAL_SAMPLE_RATE = 0.1 # ランダムな値を取るときに，終点を選択する確率
def __init__(self):
"""
コンストラクタ
"""
self._start_tree = Tree(self._NODE_NEAR_NODE_IDX)
self._start_random_tree = Tree(self._NODE_NEAR_NODE_IDX)
self._pathes = []
self._environment = None
self._debug = False
self._robot = None
self._interpolation = INTERPOLATION.JOINT
self._moving_value = self._MOVING_VALUE_JOINT
# PruningやShortcut関連のプロパティの初期化
self._before_modify_pathes = []
self._pruning_pathes = []
self._shortcut_pathes = []
self._partial_shortcut_pathes = []
# 作成したいフォルダーをリストにまとめる
folder_names = [self._FOLODER_NAME_PRUNING, self._FOLODER_NAME_SHORTCUT, self._FOLODER_NAME_PARTIAL_SHORTCUT]
for folder_name in folder_names:
self._make_folder(folder_name)
def _make_folder(self, folder_name):
"""
フォルダーの作成
パラメータ
folder_name(str): 作成したいフォルダー名
"""
# フォルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
os.makedirs(folder_name, exist_ok=True)
def _reset_folder(self, folder_name):
"""
フォルダー内のファイルを全削除
パラメータ
folder_name(str): 全削除したいフォルダー名
"""
for entry in os.listdir(folder_name):
full_path = os.path.join(folder_name, entry)
if os.path.isfile(full_path) or os.path.islink(full_path):
os.remove(full_path) # 通常ファイル・シンボリックリンクを削除
@property
def start_tree(self):
"""
_start_treeプロパティのゲッター
"""
return self._start_tree
@property
def start_random_tree(self):
"""
_start_random_treeプロパティのゲッター
"""
return self._start_random_tree
@property
def pathes(self):
"""
_pathesプロパティのゲッター
"""
return self._pathes
@property
def strict_min_pos(self):
"""
_strict_min_posプロパティのゲッター
"""
return self._strict_min_pos
@property
def strict_max_pos(self):
"""
_strict_max_posプロパティのゲッター
"""
return self._strict_max_pos
def _add_node_start_tree(self, pos, random_pos, near_node):
"""
始点ツリーにノードを追加
パラメータ
pos(numpy.ndarray): 位置
random_pos(numpy.ndarray): ランダムな位置
near_node(int): 最短ノード
"""
# _start_treeにノードを追加
node = np.append(pos, near_node).reshape(1, -1)
self._start_tree.add_node(node)
# デバッグ有効の時しか，ランダムなツリーにデータを保存したくない (処理時間の短縮)
if self._debug:
# _start_random_treeにノードを追加
random_node = np.append(random_pos, near_node).reshape(1, -1)
self._start_random_tree.add_node(random_node)
def _reset(self):
"""
データの初期化
"""
self._start_tree.reset()
self._start_random_tree.reset()
if len(self._pathes) != 0:
# 何かしらのデータが保存
del self._pathes
self._pathes = []
if self._environment is not None:
del self._environment
self._environment = None
if self._robot is not None:
del self._robot
self._robot = None
self._interpolation = INTERPOLATION.JOINT
def _set_robot(self, robot, interpolation):
"""
経路生成したいロボット情報を設定
パラメータ
robot(Robot): ロボットクラス
interpolation(int): 補間の種類 (関節補間/位置補間)
"""
self._robot = robot
if interpolation == INTERPOLATION.POSITION:
# 位置空間
self._interpolation = INTERPOLATION.POSITION
self._moving_value = self._MOVING_VALUE_POS
else:
# 関節空間
self._interpolation = INTERPOLATION.JOINT
self._moving_value = self._MOVING_VALUE_JOINT
def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
"""
経路生成
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
environment(Base2DEnv): 環境
robot(Robot): ロボットクラス
interpolation(int): 補間方法 (関節空間/位置空間)
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
debug(bool): デバッグフラグ
戻り値
result(bool): True / False = 成功 / 失敗
"""
# 処理結果
result = False
# デバッグフラグの更新
self._debug = debug
# データの初期化
self._reset()
# 始点と終点の次元数が一致しているかの確認
start_pos_dim = np.size(start_pos)
end_pos_dim = np.size(end_pos)
if start_pos_dim != end_pos_dim:
# 次元数が異なるので異常
raise ValueError(f"start_pos_dim and end_pos_dim are not matched. start_pos_dim is {start_pos_dim}, end_pos_dim is {end_pos_dim}")
self._dim = start_pos_dim
# 環境を設定
self._environment = environment
# ロボットの設定
self._set_robot(robot, interpolation)
# 始点と終点の干渉判定
if self._interference_pos(start_pos):
# 干渉ありまたは逆運動学の解が求まらない
raise ValueError(f"start_pos is interference. please change start_pos")
if self._interference_pos(end_pos):
# 干渉ありまたは逆運動学の解が求まらない
raise ValueError(f"end_pos is interference. please change end_pos")
# 始点ノードを設定
self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE)
# 探索する範囲を設定
self._strict_planning_pos(start_pos, end_pos)
# 経路生成は一定時間内に終了させる
start_time = time.time()
counter = 0
# 終点までの経路生成が終わるまでループ
while True:
# for _ in range(1000):
counter += 1
# ランダムな値を取得
random_pos = self._get_random_pos(end_pos)
# ランダムな値と最短ノードを計算
near_node = self._start_tree.get_near_node(random_pos)
# 最短ノードの位置
near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
# 最短ノードからランダムな値方向へ新しいノード(位置)を作成
new_pos = self._calc_new_pos(random_pos, near_node_pos)
# 新規ノードと最短ノード間での干渉判定
is_interference = self._line_interference(new_pos, near_node_pos)
if not is_interference:
# 干渉なしのため，ノードを設定する
self._add_node_start_tree(new_pos, random_pos, near_node)
# 終点との距離が一定範囲内であるかの確認
if self._chk_end_pos_dist(new_pos, end_pos):
# 一定範囲内のため，経路生成の完了
result = True
break
now_time = time.time()
if now_time - start_time >= self._TIMEOUT_VALUE:
# タイムアウト
break
if result:
# 経路生成に成功のため，経路生成の終了処理
self._fin_planning(start_pos, end_pos)
# PathPruningの実施
self._modify_path(modification)
# ファイルに保存する
self._save()
return result
def _modify_path(self, modification):
"""
始点から終点までの経路を修正
パラメータ
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
"""
if modification is None:
# 経路を修正しない
return
# 修正前の経路を保存
self._before_modify_pathes = np.copy(self._pathes)
if modification == MODIFICATION.PRUNING_START:
# 始点からPruning
self._path_pruning_from_start()
elif modification == MODIFICATION.PRUNING_END:
# 終点からPruning
self._path_pruning_from_end()
elif modification == MODIFICATION.PRUNING_RANDOM:
# ランダム点のPruning
self._path_pruning_random()
elif modification == MODIFICATION.SHORTCUT_NORMAL:
# Shortcut
self._path_shortcut()
elif modification == MODIFICATION.SHORTCUT_PARTIAL:
# Partial Shortcut
self._path_partial_shortcut()
elif modification == MODIFICATION.PARTIAL_START:
# Partial Shortcut + 始点からPruning
self._path_partial_shortcut()
self._path_pruning_from_start()
else:
# 異常
raise ValueError(f"modification is abnormal. modification is {modification}")
def _get_shortcut_two_nodes(self):
"""
Shortcutで使用する2つのノードを取得
戻り値
two_nodes(list): 2つのノード
"""
two_nodes = []
# 削除したいノードを2つ選択する
node1 = np.random.randint(0, self._pathes.shape[0] - 1)
node2 = np.random.randint(0, self._pathes.shape[0] - 1)
# 同じノードであれば，ノード選択に戻る
if node1 == node2:
return two_nodes
# node1に小さいノード番号を保存
if node1 > node2:
node1, node2 = node2, node1
# 隣接するノードであれば，ノード選択に戻る (隣接していれば，Shortcutする理由がない)
if node1 + 1 == node2:
return two_nodes
# 2つのノードを保存
two_nodes = [node1, node2]
return two_nodes
def _path_partial_shortcut(self):
"""
Shortcut (経路上に2点を作成して, 干渉なければ2点内のノードを全削除)
self._pathesプロパティにPruning後の経路が保存される
"""
# 要素番号を保存
n_success = 0
continueous_fail = 0 # 連続失敗回数
# 以前保存してある，Pruningファイルを全削除する
self._reset_folder(self._FOLODER_NAME_PARTIAL_SHORTCUT)
# 位置を構成する要素数を取得
n_element = self._pathes.shape[1]
for _ in range(500):
# ループ回数
# if continueous_fail > 20:
# # 連続で失敗したため，これ以上はPartial Shortcutできないと判断
# break
# 位置の中から，1要素をランダムに取得する (全要素を選択する確率は同じ)
element = np.random.randint(0, n_element - 1)
# 削除したいノードを2つ選択する
two_nodes = self._get_shortcut_two_nodes()
if len(two_nodes) != 2:
# ノード選択に失敗したため，ループ処理の先頭に戻る
continueous_fail += 1
continue
# 2点間のデータを全部取得
partial_pathes = self._pathes[two_nodes[0]: two_nodes[1] + 1].copy()
# 2点で変更したい要素データの差分を計算
diff_data = partial_pathes[-1, element] - partial_pathes[0, element]
for i in range(1, partial_pathes.shape[0] - 1):
partial_pathes[i + 1, element] = partial_pathes[0, element] + diff_data * i / partial_pathes.shape[0]
# 変更した経路間での干渉判定
is_collision = False
for i in range(0, partial_pathes.shape[0] - 1):
if self._line_interference(partial_pathes[i], partial_pathes[i + 1]):
# 干渉ありのため，連続失敗回数の更新
is_collision = True
break
if is_collision:
# 干渉あり
continueous_fail += 1
else:
# 干渉なし
self._pathes[two_nodes[0]: two_nodes[1] + 1] = partial_pathes
continueous_fail = 0
# Shortcutの結果を随時ファイル保存
n_success += 1
if self._debug:
# ファイル名の作成
file_name = f"{self._FOLODER_NAME_PARTIAL_SHORTCUT}/{self._FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES}_{n_success}.csv"
self._save_numpy_data_to_txt(self._pathes, file_name)
# Shortcut後の経路を保存
self._partial_shortcut_pathes = np.copy(self._pathes)
def _path_shortcut(self):
"""
Shortcut (経路上に2点を作成して, 干渉なければ2点内のノードを全削除)
self._pathesプロパティにPruning後の経路が保存される
"""
# 要素番号を保存
n_success = 0
continueous_fail = 0 # 連続失敗回数
# 以前保存してある，Pruningファイルを全削除する
self._reset_folder(self._FOLODER_NAME_SHORTCUT)
for _ in range(100):
# ループ回数
if continueous_fail > 10:
# 連続で失敗したため，これ以上はShortcutできないと判断
break
# 削除したいノードを2つ選択する
two_nodes = self._get_shortcut_two_nodes()
if len(two_nodes) != 2:
# ノード選択に失敗したため，ループ処理の先頭に戻る
continueous_fail += 1
continue
if not self._line_interference(self._pathes[two_nodes[0]], self._pathes[two_nodes[1]]):
# 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
self._pathes = np.delete(self._pathes, range(two_nodes[0] + 1, two_nodes[1] - 1), axis=0)
# 連続失敗回数を初期化
continueous_fail = 0
# Shortcutの結果を随時ファイル保存
n_success += 1
if self._debug:
# ファイル名の作成
file_name = f"{self._FOLODER_NAME_SHORTCUT}/{self._FILE_NAME_SHORTCUT_EACH_PATHES}_{n_success}.csv"
self._save_numpy_data_to_txt(self._pathes, file_name)
else:
# 干渉ありのため，連続失敗回数の更新
continueous_fail += 1
# Shortcut後の経路を保存
self._shortcut_pathes = np.copy(self._pathes)
def _path_pruning_random(self):
"""
Pruning (経路の経由点をランダム点の削除)
self._pathesプロパティにPruning後の経路が保存される
"""
# 要素番号を保存
idx = -1
n_success = 0
continueous_fail = 0 # 連続失敗回数
# 以前保存してある，Pruningファイルを全削除する
self._reset_folder(self._FOLODER_NAME_PRUNING)
for i in range(100):
# ループ回数
# 削除したいノード番号をランダムで選択 (始点と終点を含まないように-2)
idx = np.random.randint(0, self._pathes.shape[0] - 2)
if not self._line_interference(self._pathes[idx], self._pathes[idx - 2]):
# 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
self._pathes = np.delete(self._pathes, idx - 1, axis=0)
# 連続失敗回数を初期化
continueous_fail = 0
# Pruningの結果を随時ファイル保存
n_success += 1
if self._debug:
# ファイル名の作成
file_name = f"{self._FOLODER_NAME_PRUNING}/{self._FILE_NAME_PRUING_EACH_PATHES}_{n_success}.csv"
self._save_numpy_data_to_txt(self._pathes, file_name)
else:
# 干渉ありのため，連続失敗回数の更新
continueous_fail += 1
if continueous_fail > 5:
# 連続で失敗したら，削除できるノードは存在しないと考えられる
break
# Pruning後の経路を保存
self._pruning_pathes = np.copy(self._pathes)
def _path_pruning_from_end(self):
"""
Pruning (経路の経由点を終点方向から順番に削除)
self._pathesプロパティにPruning後の経路が保存される
"""
# 要素番号を保存
idx = -1
count = 0
# 以前保存してある，Pruningファイルを全削除する
self._reset_folder(self._FOLODER_NAME_PRUNING)
# 経由点数分ループする (始点と終点を含まないために，-2をする)
while abs(idx) < self._pathes.shape[0] - 2:
# (idx+1)要素前後のノード間で干渉がないかの確認
if not self._line_interference(self._pathes[idx], self._pathes[idx - 2]):
# 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
self._pathes = np.delete(self._pathes, idx - 1, axis=0)
else:
# 干渉ありのため，次要素を選択
idx -= 1
# Pruningの結果を随時ファイル保存
count += 1
if self._debug:
# ファイル名の作成
file_name = f"{self._FOLODER_NAME_PRUNING}/{self._FILE_NAME_PRUING_EACH_PATHES}_{count}.csv"
self._save_numpy_data_to_txt(self._pathes, file_name)
# Pruning後の経路を保存
self._pruning_pathes = np.copy(self._pathes)
def _path_pruning_from_start(self):
"""
Pruning (経路の経由点を始点方向から順番に削除)
self._pathesプロパティにPruning後の経路が保存される
"""
# 要素番号を保存
idx = 0
count = 0
# 以前保存してある，Pruningファイルを全削除する
self._reset_folder(self._FOLODER_NAME_PRUNING)
# 経由点数分ループする (始点と終点を含まないために，-2をする)
while idx < self._pathes.shape[0] - 2:
# (idx+1)要素前後のノード間で干渉がないかの確認
if not self._line_interference(self._pathes[idx], self._pathes[idx + 2]):
# 干渉なしのため，(idx+1)要素を削除 (行列の行を削除するため，axis=0を引数に与える)
self._pathes = np.delete(self._pathes, idx + 1, axis=0)
else:
# 干渉ありのため，次要素を選択
idx += 1
# Pruningの結果を随時ファイル保存
count += 1
if self._debug:
# ファイル名の作成
file_name = f"{self._FOLODER_NAME_PRUNING}/{self._FILE_NAME_PRUING_EACH_PATHES}_{count}.csv"
self._save_numpy_data_to_txt(self._pathes, file_name)
# Pruning後の経路を保存
self._pruning_pathes = np.copy(self._pathes)
def _line_interference(self, pos1, pos2):
"""
2点間の干渉チェック
パラメータ
pos1(numpy.ndarray): 位置1/関節1
pos2(numpy.ndarray): 位置2/関節2
戻り値
is_interference(bool): True / False = 干渉あり / 干渉なし
"""
is_interference = False
# 差分を計算
difference = pos2 - pos1
# 距離を計算
distance = np.linalg.norm(difference)
if self._interpolation == INTERPOLATION.POSITION:
# 位置空間 (分割時の距離を保存)
devided_dist = self._DEVIDED_DISTANCE_POS
else:
# 関節空間 (分割時の距離を保存)
devided_dist = self._DEVIDED_DISTANCE_JOINT
# 2点間の分割数を算出
n_devided = max(int(distance / devided_dist), 1)
for i in range(n_devided + 1):
direct_pos = i / n_devided * difference
devided_pos = pos1 + direct_pos
if self._interference_pos(devided_pos):
# 干渉あり
is_interference = True
break
return is_interference
def _interference_pos(self, pos):
"""
干渉判定
パラメータ
pos(numpy.ndarray): 位置/関節
戻り値
is_interference(bool): True / False = 干渉あり / 干渉なし
"""
is_interference = False
if self._interpolation == INTERPOLATION.POSITION:
# 位置空間の場合は，逆運動学を計算
try:
thetas = self._robot.inverse_kinematics(pos)
# ロボットの位置を更新
self._robot.update(thetas)
# 干渉判定
if self._environment.is_collision_dist(self._robot.manager, margin=self._INTERFERENCE_MARGIN):
# 干渉あり
is_interference = True
except Exception as e:
# 逆運動学の解が得られないから，干渉ありとする
is_interference = True
else:
# 関節空間
# ロボットの位置を更新
self._robot.update(pos)
# 干渉判定
if self._environment.is_collision_dist(self._robot.manager, margin=self._INTERFERENCE_MARGIN):
# 干渉あり
is_interference = True
return is_interference
def _chk_end_pos_dist(self, pos, end_pos):
"""
終点との距離が一定範囲内であるかの確認
パラメータ
pos(numpy.ndarray): ノード位置
end_pos(numpy.ndarray): 経路生成の終点
戻り値
is_near(bool): True / False = 一定範囲内である / でない
"""
is_near = False
# 距離を計算
dist = np.linalg.norm(end_pos - pos)
# 一定範囲内であるかの確認
if dist <= self._moving_value:
if not self._line_interference(pos, end_pos):
# 干渉しない
is_near = True
return is_near
def _fin_planning(self, start_pos, end_pos):
"""
経路生成の終了処理
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
"""
# 始点から終点までの経路に関係するノードを選択
revers_path = end_pos.reshape(1, -1)
near_node = -1
while True:
# 終点から始点方向へノードを取得
node = self._start_tree.nodes[near_node]
pos = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
# 浮動小数型になっているので，整数型に型変換
near_node = int(node[self._NODE_NEAR_NODE_IDX])
revers_path = np.append(revers_path, pos, axis=0)
if near_node == INITIAL_NODE_NEAR_NODE:
# 始点ノードまで取得できたため，処理終了
break
# 経路が終点からの順番になっているため，始点から終点とする
self._pathes = revers_path[::-1]
# ツリーに終点を追加 (要素番号を指定するため -1)
self._add_node_start_tree(end_pos, end_pos, self._start_tree.nodes.shape[0] - 1)
def _calc_new_pos(self, random_pos, near_node_pos):
"""
最短ノードからランダムな値方向へ新しいノード(位置)を作成
パラメータ
random_pos(numpy.ndarray): ランダムな位置
near_node_pos(numpy.ndarray): 最短ノード位置
戻り値
new_pos(numpy.ndarray): 新しいノード
"""
# 方向を計算
direction = random_pos - near_node_pos
# 方向の大きさを1にする
norm_direction = direction / (np.linalg.norm(direction) + EPSILON)
# 新しいノードを作成
new_pos = near_node_pos + norm_direction * self._moving_value
return new_pos
def _strict_planning_pos(self, start_pos, end_pos):
"""
探索範囲を制限する
パラメータ
start_pos(numpy.ndarray): 始点
end_pos(numpy.ndarray): 終点
"""
all_pos = np.array([start_pos, end_pos])
# 各列の最大/最小値を取得
min_pos = np.min(all_pos, axis=0)
max_pos = np.max(all_pos, axis=0)
if self._interpolation == INTERPOLATION.POSITION:
# 位置空間の探索
strict_planning_pos = self._STRICT_PLANNING_ROB_POS
else:
# 関節空間の探索
strict_planning_pos = self._STRICT_PLANNING_ROB_JOINT
self._strict_min_pos = min_pos - strict_planning_pos
self._strict_max_pos = max_pos + strict_planning_pos
def _get_random_pos(self, target_pos):
"""
ランダムな位置を取得
パラメータ
target_pos(numpy.ndarray): 目標点
戻り値
random_pos(numpy.ndarray): ランダムな位置
"""
# 乱数を取って，目標点を選択するかランダムを選択するか
select_goal = np.random.rand()
if select_goal < self._GOAL_SAMPLE_RATE:
# 目標点を選択s
random_pos = target_pos
else:
random_pos = np.random.uniform(self._strict_min_pos, self._strict_max_pos)
return random_pos
def _save(self):
"""
生成した経路をファイル保存
"""
if self._debug:
# 始点から終点までの修正済みの経路をファイル保存
self._save_numpy_data_to_txt(self._pathes, self._FILE_NAME_PATHES)
# 始点のツリーを保存
self._save_numpy_data_to_txt(self._start_tree.nodes, self._FILE_NAME_START_TREE)
# 始点のランダムツリーを保存
self._save_numpy_data_to_txt(self._start_random_tree.nodes, self._FILE_NAME_START_RANDOM_TREE)
# 修正前の始点から終点までの経路を保存
self._save_numpy_data_to_txt(self._before_modify_pathes, self._FILE_NAME_BEFORE_MODIFY_PATHES)
# Pruning後の経路を保存
self._save_numpy_data_to_txt(self._pruning_pathes, self._FILE_NAME_PRUNING_PATHES)
#
self._save_numpy_data_to_txt(self._shortcut_pathes, self._FILE_NAME_SHORTCUT_PATHES)
self._save_numpy_data_to_txt(self._partial_shortcut_pathes, self._FILE_NAME_SHORTCUT_PATHES)
def _save_numpy_data_to_txt(self, data, file_name):
"""
Numpyデータをテキストファイルに保存
パラメータ
data(numpy.ndarray): ファイル保存したいデータ
file_name(str): 保存したいファイル名
"""
# 引数の確認
if len(data) == 0:
# データが存在しないため，処理終了
return
if not file_name:
# ファイル名が存在しないため，処理終了
return
np.savetxt(file_name, data)
class CostedTree(Tree):
"""
コスト付きツリークラス
プロパティ
_nodes(numpy.ndarray): ノード
_near_node_idx(int): 最短ノード列番号 (_nodesの中に最短ノードも保存している)
_cost_idx(int): コスト列番号 (_nodesの中にコストも保存している)
_radius_idx(int): 探索半径の列番号 (_nodesの中に探索半径も保存している)
メソッド
public
nodes(): _nodesプロパティのゲッター
add_node(): ノードの追加
get_near_node(): 最短距離のノードを取得
"""
# 定数の定義
_INITIAL_COST = 0.0 # 初期コスト
def __init__(self, near_node_idx, cost_idx, radius_idx):
"""
コンストラクタ
"""
# プロパティの初期化
self._nodes = []
self._near_node_idx = near_node_idx
self._cost_idx = cost_idx
self._radius_idx = radius_idx
def calc_cost(self, node_idx, pos):
"""
コストの計算
パラメータ
node_idx(int): ノード番号
pos(numpy.ndarray): 位置
戻り値
cost(float): コスト
"""
# ノードの存在確認
self._chk_node_exist()
# ノードから位置・コストを取得
node = self._nodes[node_idx]
node_pos = node[:self._near_node_idx]
node_cost = node[self._cost_idx]
# 距離を計算
distance = np.linalg.norm(node_pos - pos)
# コストを計算
cost = node_cost + distance
return cost
def get_near_node(self, pos):
"""
最短距離のノードを取得
パラメータ
pos(numpy.ndarray): 位置
戻り値
min_dist_idx(int): 最短距離のノード番号
"""
# ノードの存在確認
self._chk_node_exist()
# ノードから位置を取得
nodes_pos = self._nodes[:, :self._near_node_idx]
# 差分を計算
difference = nodes_pos - pos
# 距離を計算
distance = np.linalg.norm(difference, axis=1)
# 最短距離ノードを取得
min_dist_idx = np.argmin(distance)
return min_dist_idx
def chg_node_info(self, node_idx, near_node, cost):
"""
ノード情報を変更
パラメータ
node_idx(int): 変更したいノード番号
near_node(int): 変更後の最短ノード
cost(float): 変更後のコスト
"""
self._nodes[node_idx, self._near_node_idx] = near_node
self._nodes[node_idx, self._cost_idx] = cost
class RRTStarRobot(RRTRobot):
"""
RRT*クラス
プロパティ
_pathes(numpy.ndarray): 始点から終点までの経路
_debug_pathes(numpy.ndarray): デバッグ用経路
_debug_random_pathes(numpy.ndarray): デバッグ用ランダム位置
_strict_min_pos(numpy.ndarray): 探索の最小範囲
_strict_max_pos(numpy.ndarray): 探索の最大範囲
_interference_obj(list): 干渉物
_max_iterate(int): 最大探索回数
_count_success(int): ノード追加の成功回数
_near_node_radius(float): 近傍ノードの探索半径
publicメソッド (全てのクラスから参照可能)
planning(): 経路生成の実装
count_success(int): _count_successプロパティのゲッター
protectedメソッド (自クラスまたは子クラスが参照可能)
_calc_cost(): コストの計算
_get_near_node_list(): ノードと近傍ノードをリストで取得
_get_min_cost_node(): コストが最小となる近傍ノードを取得
_update_near_node(): 近傍ノード内の親ノードを更新
_fin_planning(): 経路生成の終了処理
_save_current_tree(): 現在のツリーをファイル化
"""
# 定数の定義
# ファイル名の定義
_FILE_NAME_PATHES = "rrt_star_pathes.csv" # _pathesプロパティを保存するファイル名
_FILE_NAME_DEBUG_PATHES = "rrt_star_debug_pathes.csv" # _debug_pathesプロパティを保存するファイル名
_FILE_NAME_DEBUG_RANDOM_PATHES = "rrt_star_debug_random_pathes.csv" # _debug_random_pathesプロパティを保存するファイル名
# フォルダー名の定義
_FOLDER_NAME_CURRENT_TREE = "rrt_star_result" # ノード追加時の現在のツリーを保存するフォルダ名
# 毎処理保存するファイル名の定義
_FILE_NAME_CURRENT_TREE = "rrt_start_current_tree" # ノード追加時の現在のツリーを保存するファイル名
_FILE_NAME_RANDOM_TREE = "rrt_start_random_tree" # ノード追加時の現在のランダムツリーを保存するファイル名
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_STAR_NEAR_NODE_IDX # ノード内の最短ノード要素
_NODE_COST_IDX = RRT_STAR_COST_IDX # ノード内のコスト要素
_NODE_RADIUS_IDX = RRT_STAR_RADIUS_IDX # ノード内の半径要素
# その他
_MAX_ITERATE = 500 # 最大探索回数
_TIMEOUT_VALUE = 1000 # タイムアウト時間 [second]
def __init__(self):
"""
コンストラクタ
"""
self._start_tree = CostedTree( self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX, self._NODE_RADIUS_IDX)
self._start_random_tree = CostedTree(self._NODE_NEAR_NODE_IDX, self._NODE_COST_IDX, self._NODE_RADIUS_IDX)
self._pathes = []
self._interference_obj = None
self._debug = False
self._max_iterate = self._MAX_ITERATE
self._environment = None
self._robot = None
self._near_node_radius = 0
# フォルダを作成しておく (存在済みの場合はエラーを発生させない(引数 exist_ok をTrueとする))
os.makedirs(self._FOLDER_NAME_CURRENT_TREE, exist_ok=True)
@property
def max_iterate(self):
"""
_max_iterateプロパティのゲッター
"""
return self._max_iterate
@max_iterate.setter
def max_iterate(self, value):
"""
_max_iterateプロパティのセッター
パラメータ
value(int): 設定したい値
"""
# 負の値では何もしない
if value > 0:
self._max_iterate = value
@property
def near_node_radius(self):
"""
_near_node_radiusプロパティのゲッター
"""
return self._near_node_radius
@near_node_radius.setter
def near_node_radius(self, value):
"""
_near_node_radiusプロパティのセッター
パラメータ
value(float): 設定したい値
"""
# 負の値では，何もしない
if value > 0:
self._near_node_radius = value
@property
def count_success(self):
"""
_count_successプロパティのゲッター
"""
return self._count_success
@property
def file_name_current_tree(self):
"""
_FILE_NAME_CURRENT_TREE定数のゲッター
"""
return self._FILE_NAME_CURRENT_TREE
@property
def file_name_random_tree(self):
"""
_FILE_NAME_RANDOM_TREE定数のゲッター
"""
return self._FILE_NAME_RANDOM_TREE
@property
def foler_name_current_tree(self):
"""
_FOLDER_NAME_CURRENT_TREE定数のゲッター
"""
return self._FOLDER_NAME_CURRENT_TREE
def _add_node_start_tree(self, pos, random_pos, near_node, cost, radius):
"""
始点ツリーにノードを追加
パラメータ
pos(numpy.ndarray): 位置
random_pos(numpy.ndarray): ランダムな位置
near_node(int): 最短ノード
"""
array_data = np.array([near_node, cost, radius])
# _start_treeにノードを追加
node = np.append(pos, array_data).reshape(1, -1)
self._start_tree.add_node(node)
# # デバッグフラグが有効の時しか，ランダムなツリーにデータを保存したくない
# if self._debug:
# _start_random_treeにノードを追加
random_node = np.append(random_pos, array_data).reshape(1, -1)
self._start_random_tree.add_node(random_node)
def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
"""
経路生成
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
environment(Base2DEnv): 環境
robot(Robot): ロボットクラス
interpolation(int): 補間方法 (関節空間/位置空間)
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
debug(bool): デバッグフラグ
戻り値
result(bool): True / False = 成功 / 失敗
"""
# 処理結果
result = False
# デバッグフラグの更新
self._debug = debug
# データの初期化
self._reset()
# 始点と終点の次元数が一致しているかの確認
start_pos_dim = np.size(start_pos)
end_pos_dim = np.size(end_pos)
if start_pos_dim != end_pos_dim:
# 次元数が異なるので異常
raise ValueError(f"start_pos_dim and end_pos_dim are not matched. start_pos_dim is {start_pos_dim}, end_pos_dim is {end_pos_dim}")
self._dim = start_pos_dim
# 環境を設定
self._environment = environment
# ロボットの設定
self._set_robot(robot, interpolation)
# 始点と終点の干渉判定
if self._interference_pos(start_pos):
# 干渉ありまたは逆運動学の解が求まらない
raise ValueError(f"start_pos is interference. please change start_pos")
if self._interference_pos(end_pos):
# 干渉ありまたは逆運動学の解が求まらない
raise ValueError(f"end_pos is interference. please change end_pos")
# 始点ノードを設定
self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE, 0.0, 0.0)
# 探索する範囲を設定
self._strict_planning_pos(start_pos, end_pos)
# 近傍ノードを定義する半径
self._near_node_radius = self._moving_value * 20
# 経路生成は一定時間内に終了させる
start_time = time.time()
self._count_success = 0 # ノード追加に成功した回数
# 全探索回数ループ
for i in range(self._max_iterate):
# ランダムな値を取得
random_pos = self._get_random_pos(end_pos)
# ランダムな値と最短ノードを計算
near_node = self._start_tree.get_near_node(random_pos)
# 最短ノード位置を取得
near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
# 最短ノードからランダムな値方向へ新しいノード(位置)を作成
new_pos = self._calc_new_pos(random_pos, near_node_pos)
# 新規ノードと最短ノード間での干渉判定
is_interference = self._line_interference(new_pos, near_node_pos)
if not is_interference:
# 干渉なしのため，データを設定する
# 新規箇所 ↓
# 新規ノードのコストを計算
new_cost = self._start_tree.calc_cost(near_node, new_pos)
# 近傍ノードを全部取得
near_node_list, threshold = self._get_near_node_list(new_pos)
# 近傍ノードからコストが最小となるノードを取得
min_cost_node, min_cost = self._get_min_cost_node(new_pos, near_node_list, near_node, new_cost)
# 近傍ノード内でコストが小さくなれば，最短ノードを更新
self._update_near_node(new_pos, near_node_list, min_cost)
# 新規箇所 ↑
# ノードを設定
self._add_node_start_tree(new_pos, random_pos, min_cost_node, min_cost, threshold)
# 終点との距離が一定範囲内であるかの確認
if self._chk_end_pos_dist(new_pos, end_pos):
# 一定範囲内のため，戻り値を成功に更新
result = True
self._count_success += 1
# 現在のツリーをファイル化する
self._save_current_tree()
# 干渉の有無に関わらずにタイムアウトの確認はする
now_time = time.time()
if now_time - start_time >= self._TIMEOUT_VALUE:
# タイムアウト
break
if result:
# 経路生成に成功のため，経路生成の終了処理
self._fin_planning(start_pos, end_pos)
# PathPruningの実施
self._modify_path(modification)
return result
def _calc_cost(self, pos, near_node):
"""
ノードのコストを計算
パラメータ
pos(numpy.ndarray): 新規ノード位置
near_node(int): 新規ノードとの最短ノード
戻り値
cost(float): コスト
"""
# 最短ノードの情報を保存
node = self._start_tree.nodes[near_node]
# node = self._debug_pathes[near_node]
node_pos = node[:self._NODE_NEAR_NODE_IDX]
node_near_node = node[self._NODE_NEAR_NODE_IDX]
node_cost = node[self._NODE_COST_IDX]
# 最短ノードと新規ノードのコスト(距離)を算出
cost = np.linalg.norm(node_pos - pos)
cost += node_cost
return cost
def _get_near_node_list(self, pos):
"""
ノードと近傍ノードをリストで取得
パラメータ
pos(numpy.ndarray): ノード位置
fin_flg(bool): 経路生成の終了フラグ
戻り値
near_node_list(list): 近傍ノードリスト
threshold(float): 近傍判定の閾値
"""
# 閾値はノード数に依存する
n_node = self._start_tree.nodes.shape[0]
threshold = self._near_node_radius * np.power(np.log(n_node) / n_node, 1 / self._dim)
# 閾値以内の距離であるノードを全部取得
near_node_list = self._start_tree.get_near_node_list(pos, threshold)
return near_node_list, threshold
def _get_min_cost_node(self, pos, near_node_list, near_node, near_node_cost):
"""
コストが最小となる近傍ノードを取得
パラメータ
pos(numpy.ndarray): ノード位置
near_node_list(list): 近傍ノードリスト
near_node(int): 最短ノード
near_node_cost(float): 最短ノード追加時のコスト
戻り値
min_cost_node(int): コスト最小ノード
min_cost(float): コスト最小値
"""
# 戻り値の初期化
min_cost = near_node_cost
min_cost_node = near_node
if not near_node_list:
# 近傍ノードがないため，処理終了
return min_cost_node, min_cost
for node_idx in near_node_list:
# 近傍ノードに関する情報を保存
# node = self._debug_pathes[node_idx]
node = self._start_tree.nodes[node_idx]
node_pos = node[:self._NODE_NEAR_NODE_IDX]
node_cost = node[self._NODE_COST_IDX]
# コスト(距離)を計算
distance = np.linalg.norm(node_pos - pos)
# 2点間の干渉チェック
is_interference = self._line_interference(node_pos, pos)
if not is_interference:
# 干渉なし
new_cost = distance + node_cost
if new_cost < min_cost:
# 小さくなるため，最短ノードを修正
min_cost_node = node_idx
min_cost = new_cost
return min_cost_node, min_cost
def _update_near_node(self, pos, near_node_list, cost):
"""
近傍ノード内の親ノードを更新
パラメータ
pos(numpy.ndarray): ノード位置
near_node_list(list): 近傍ノードリスト
cost(float): コスト
"""
if not near_node_list:
# 近傍ノードがないため，処理終了
return
# 新規ノードのインデックス番号を保存
# new_node_idx = self._debug_pathes.shape[0]
new_node_idx = self._start_tree.nodes.shape[0]
for node_idx in near_node_list:
# 近傍ノードに関する情報を保存
# node = self._debug_pathes[node_idx]
node = self._start_tree.nodes[node_idx]
node_pos = node[:self._NODE_NEAR_NODE_IDX]
node_near_node = node[self._NODE_NEAR_NODE_IDX]
node_cost = node[self._NODE_COST_IDX]
# コスト(距離)を計算
distance = np.linalg.norm(node_pos - pos)
# 2点間の干渉チェック
is_interference = self._line_interference(node_pos, pos)
if not is_interference:
# 干渉なし
new_cost = distance + cost
if new_cost < node_cost:
# 小さくなるため，最短ノードを修正
self._start_tree.chg_node_info(node_idx, new_node_idx, new_cost)
# self._start_tree.[node_idx, self._NODE_NEAR_NODE_IDX] = new_node_idx
# self._debug_pathes[node_idx, self._NODE_COST_IDX] = new_cost
def _fin_planning(self, start_pos, end_pos):
"""
経路生成の終了処理
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
"""
# 始点から終点までの経路に関係するノードを選択
revers_path = end_pos.reshape(1, -1)
# 終点との最短ノードを取得
near_node = self._start_tree.get_near_node(end_pos)
# 最短ノードとのコストを計算
near_node_cost = self._calc_cost(end_pos, near_node)
# 近傍ノードを全部取得
near_node_list, threshold = self._get_near_node_list(end_pos)
# 近傍ノードからコストが最小となるノードを取得
min_cost_node, min_cost = self._get_min_cost_node(end_pos, near_node_list, near_node, near_node_cost)
near_node = min_cost_node
while True:
# 終点から始点方向へノードを取得
node = self._start_tree.nodes[near_node]
pos = node[:self._NODE_NEAR_NODE_IDX].reshape(1, -1)
# 浮動小数型になっているので，整数型に型変換
near_node = int(node[self._NODE_NEAR_NODE_IDX])
revers_path = np.append(revers_path, pos, axis=0)
if near_node == INITIAL_NODE_NEAR_NODE:
# 始点ノードまで取得できたため，処理終了
break
# 経路が終点からの順番になっているため，始点から終点とする
self._pathes = revers_path[::-1]
# デバッグ用の経路に終点を追加
self._add_node_start_tree(start_pos, start_pos, self._start_tree.nodes.shape[0] - 1, 0.0, threshold)
def _save_current_tree(self):
"""
現在のツリーをファイル化
"""
# 新規ノードが追加できたら，self._count_successはインクリメントされていく
file_name = f"{self._FOLDER_NAME_CURRENT_TREE}/{self._FILE_NAME_CURRENT_TREE}_{self._count_success}.csv"
# np.savetxt(file_name, self._debug_pathes)
np.savetxt(file_name, self._start_tree.nodes)
file_name = f"{self._FOLDER_NAME_CURRENT_TREE}/{self._FILE_NAME_RANDOM_TREE}_{self._count_success}.csv"
# np.savetxt(file_name, self._debug_random_pathes)
np.savetxt(file_name, self._start_random_tree.nodes)
# 新規追加 ↓
class RRTStarSmartRobot(RRTStarRobot):
"""
RRT*-Smart (RRT*の改良版 (サンプリングの改良 + 経路の最適化処理を追加))
RRT*ではランダムな値をサンプルしているため，効率的ではない
RRT*-Smartは始点から終点までの経路生成が完了したら確率で利用または探索を実施
制限時は，ビーコンを採用する．
プロパティ
_max_iterate(int): 最大探索回数
_count_success(int): ノード追加の成功回数
_near_node_radius(float): 近傍ノードの探索半径
_spheres(numpy.ndarray): 球
メソッド
public
planning(): 経路生成の実装
spheres(): _spheresプロパティのゲッター
protected
_get_random_pos(): ランダムな値を取得
_get_unit_ball(): 単位球内の位置を取得
_set_sphere(): 球情報を設定
"""
# 定数の定義
# ファイル名の定義
_FILE_NAME_PATHES = "rrt_star_smart_pathes.csv" # _pathesプロパティを保存するファイル名
_FILE_NAME_DEBUG_PATHES = "rrt_star_smart_debug_pathes.csv" # _debug_pathesプロパティを保存するファイル名
_FILE_NAME_DEBUG_RANDOM_PATHES = "rrt_star_smart_debug_random_pathes.csv" # _debug_random_pathesプロパティを保存するファイル名
# フォルダー名の定義
_FOLDER_NAME_CURRENT_TREE = "rrt_star_smart_result" # ノード追加時の現在のツリーを保存するフォルダ名
# 毎処理保存するファイル名の定義
_FILE_NAME_CURRENT_TREE = "rrt_star_smart_current_tree" # ノード追加時の現在のツリーを保存するファイル名
_FILE_NAME_RANDOM_TREE = "rrt_star_smart_random_tree" # ノード追加時の現在のランダムツリーを保存するファイル名
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_STAR_NEAR_NODE_IDX # ノード内の最短ノード要素
_NODE_COST_IDX = RRT_STAR_COST_IDX # ノード内のコスト要素
_NODE_RADIUS_IDX = RRT_STAR_RADIUS_IDX # ノード内の半径要素
# その他
_MAX_ITERATE = 1000 # 最大探索回数
_TIMEOUT_VALUE = 1000 # タイムアウト時間 [second]
_MAX_COST = 1e10 # 最大コスト
# RRT*-Smart特有
# Intelligent Samplingに関する定数
_INTERFERENCE_PROB_MAX = 0.8 # 干渉物に近い点を選択する最大確率
_INTERFERENCE_PROB_ITER_COEF = 0.002 # 干渉物に近い点を選択時の探索回数の係数
_INTELLIGENT_SAMPLING_FREQ = _MAX_ITERATE // 10 # 探索の10回に1回の頻度でIntelligent Sampling
# Path Optimizationに関する定数
_PATH_OPT_FREQ = 50 # Path Optimizationの実装頻度
_PATH_OPT_RADIUS_COEF = 0.2 # 球半径の係数
_PATH_OPT_ITER_COEF = 0.001 # 球半径の探索回数に関する係数
_PATH_OPT_ITER_MAX = 0.5 # 球半径の探索回数に応じた最大値
def __init__(self):
"""
コンストラクタ
"""
# 親クラスの初期化処理
super().__init__()
self._spheres = []
@property
def spheres(self):
"""
_spheresプロパティのゲッター
"""
return self._spheres
def planning(self, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
"""
経路生成
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
environment(Base2DEnv): 環境
robot(Robot): ロボットクラス
interpolation(int): 補間方法 (関節空間/位置空間)
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
debug(bool): デバッグフラグ
戻り値
result(bool): True / False = 成功 / 失敗
"""
# 処理結果
result = False
# デバッグフラグの更新
self._debug = debug
# データの初期化
self._reset()
# 始点と終点の次元数が一致しているかの確認
start_pos_dim = np.size(start_pos)
end_pos_dim = np.size(end_pos)
if start_pos_dim != end_pos_dim:
# 次元数が異なるので異常
raise ValueError(f"start_pos_dim and end_pos_dim are not matched. start_pos_dim is {start_pos_dim}, end_pos_dim is {end_pos_dim}")
self._dim = start_pos_dim
# 環境を設定
self._environment = environment
# ロボットの設定
self._set_robot(robot, interpolation)
# 始点と終点の干渉判定
if self._interference_pos(start_pos):
# 干渉ありまたは逆運動学の解が求まらない
raise ValueError(f"start_pos is interference. please change start_pos")
if self._interference_pos(end_pos):
# 干渉ありまたは逆運動学の解が求まらない
raise ValueError(f"end_pos is interference. please change end_pos")
# 始点ノードを設定
self._add_node_start_tree(start_pos, start_pos, INITIAL_NODE_NEAR_NODE, 0.0, 0.0)
# 探索する範囲を設定
self._strict_planning_pos(start_pos, end_pos)
# 球を保存
self._set_sphere(np.zeros(self._dim).reshape(1, -1), 0.0)
# 近傍ノードを定義する半径
self._near_node_radius = self._moving_value * 20
# 経路生成は一定時間内に終了させる
start_time = time.time()
self._count_success = 0 # ノード追加に成功した回数
first_success_count = 0 # 初めて経路生成に成功したループ数
# 始点から，終点までの経路が生成できるノードを保存
planning_nodes = []
# 1つ前のコスト
prev_cost = self._MAX_COST
# ビーコン
beacon_nodes = []
# 全探索回数ループ
for i in range(self._max_iterate):
# RRT*との差異 ↓
# ビーコン位置と半径
beacon_radius = 0.0
beacon_pos = np.zeros(self._dim)
if result:
# 始点から終点までの経路が生成できた
# Intelligent Samplingの確率は初めて経路生成ができてからのループ回数に応じて変化する
iteration = i - first_success_count
if iteration % self._INTELLIGENT_SAMPLING_FREQ == 0:
# Intelligent Sampling実施
if beacon_nodes:
random_pos, beacon_radius, beacon_pos = self._intelligent_sampling(beacon_nodes, prev_cost, iteration)
else:
# 基本的にはありえないが念の為，ランダムサンプリングとして考える
random_pos = self._get_random_pos(end_pos)
# RRT*との差異 ↑
else:
# RRT*と同様でランダムサンプリング
random_pos = self._get_random_pos(end_pos)
else:
# RRT*と同様でランダムサンプリング
random_pos = self._get_random_pos(end_pos)
# ランダムな値と最短ノードを計算
near_node = self._start_tree.get_near_node(random_pos)
# 最短ノード位置を取得
near_node_pos = self._start_tree.nodes[near_node, :self._NODE_NEAR_NODE_IDX]
# 最短ノードからランダムな値方向へ新しいノード(位置)を作成
new_pos = self._calc_new_pos(random_pos, near_node_pos)
# 新しい点と干渉物との干渉判定
is_interference = self._line_interference(new_pos, near_node_pos)
if not is_interference: # 干渉なしのため，データを設定する
# 新規ノードのコストを計算
new_cost = self._calc_cost(new_pos, near_node)
# 近傍ノードを全部取得
near_node_list, threshold = self._get_near_node_list(new_pos)
# 近傍ノードからコストが最小となるノードを取得
min_cost_node, min_cost = self._get_min_cost_node(new_pos, near_node_list, near_node, new_cost)
# 近傍ノード内でコストが小さくなれば，最短ノードを更新
self._update_near_node(new_pos, near_node_list, min_cost)
# ノードを設定
self._add_node_start_tree(new_pos, random_pos, min_cost_node, min_cost, threshold)
# 球の保存
self._set_sphere(beacon_pos, beacon_radius)
# 終点との距離が一定範囲内であるかの確認
if self._chk_end_pos_dist(new_pos, end_pos):
# RRT*との差異 ↓
if result != True:
# 初めての成功である
first_success_count = i
# ノード保存
planning_nodes.append(self._start_tree.nodes.shape[0] - 1)
# RRT*との差異 ↑
# 戻り値を成功に更新
result = True
# 経路の最適化
if result == True: # 始点から終点までの経路が未作成
# 経路最適化
if i == first_success_count or i % self._PATH_OPT_FREQ == 0:
# 初めて経路生成ができた or 経路最適化の実施回数
now_cost, _ = self._path_optimization(planning_nodes)
if now_cost < prev_cost:
# コストが小さくなったので，新規ノードをビーコンとする
_, beacon_nodes = self._path_optimization(planning_nodes)
# 前回コストの更新
prev_cost = now_cost
self._count_success += 1
# 現在のツリーをファイル化する
self._save_current_tree()
# 干渉の有無に関わらずにタイムアウトの確認はする
now_time = time.time()
if now_time - start_time >= self._TIMEOUT_VALUE:
# タイムアウト
break
if result:
# 経路生成に成功のため，経路生成の終了処理
self._fin_planning(start_pos, end_pos)
# PathPruningの実施
self._modify_path(modification)
return result
# RRT*との差異 ↓
def _path_optimization(self, planning_nodes):
"""
経路の最適化 (親ノードの最適化)
パラメータ
planning_nodes(numpy.ndarray): 始点と終点を繋ぐ，終点側のノード
戻り値
min_cost(float): 始点から終点までの経路での最短コスト
min_cost_nodes(list): 採点コストとなるノードのリスト
"""
min_cost = self._MAX_COST
min_cost_nodes = []
# ノード数を計算
n_node = self._start_tree.nodes.shape[0]
# 最新のノードから始点のノードへ順番に親ノードを最適化する
for i in reversed(range(1, n_node)):
# 始点のノードを選択するまでループ
current_node = i
while True:
# 現在ノードの位置
now_node_pos = self._start_tree.nodes[current_node, :self._NODE_NEAR_NODE_IDX]
# 親ノード
parent_node = int(self._start_tree.nodes[current_node, self._NODE_NEAR_NODE_IDX])
# 親ノードが始点なら，終了 (始点には親ノードが存在しないから，これ以上最適化できない)
if parent_node == 0:
break
# 親ノードの親ノード
parent_parent_node = int(self._start_tree.nodes[parent_node, self._NODE_NEAR_NODE_IDX])
# 親ノードの親ノードの位置
parent_parent_node_pos = self._start_tree.nodes[parent_parent_node, :self._NODE_NEAR_NODE_IDX]
# 親ノードの親ノードのコスト
parent_parent_cost = self._start_tree.nodes[parent_parent_node, self._NODE_COST_IDX]
# 現在ノードと親ノードの親ノードの距離
distance = np.linalg.norm(parent_parent_node_pos - now_node_pos)
if not self._line_interference(now_node_pos, parent_parent_node_pos):
# 親ノードの更新
# コスト更新
new_cost = parent_parent_cost + distance
# 親ノードを更新
self._start_tree.chg_node_info(current_node, parent_parent_node, new_cost)
# 今ノードを更新
current_node = parent_node
if planning_nodes:
# 始点から終点までの経路が存在するため，最小コストを計算
min_node = 0
for node in planning_nodes:
# コストを取得
cost = self._start_tree.nodes[node, self._NODE_COST_IDX]
if cost < min_cost:
min_cost = cost
min_node = node
# 最小コストの経路で始点を除いた経由点を全部保存する
min_cost_nodes.append(min_node)
while True:
# 親ノード
parent_node = int(self._start_tree.nodes[min_node, self._NODE_NEAR_NODE_IDX])
# 始点確認
if parent_node == 0:
# 始点
break
# 保存
min_cost_nodes.append(parent_node)
min_node = parent_node
return min_cost, min_cost_nodes
def _intelligent_sampling(self, beacon_nodes, min_cost, iteration):
"""
Intelligent Samplingによる位置を計算
パラメータ
beacon_nodes(numpy.ndarray): ビーコンとなり得るノード
min_cost(float): 始点から終点までの最短コスト
iterateion(int): 探索回数 - 始点から終点までの経路生成に成功した時の探索回数
戻り値
random_pos(numpy.ndarray): ランダムな位置
beacon_radius(float): ビーコンの球の半径
beacon_node_pos(numpy.ndarray): ビーコンの位置
"""
# 干渉物に最短のノードを選択する確率の計算
p = min(self._INTERFERENCE_PROB_MAX, self._INTERFERENCE_PROB_ITER_COEF * iteration)
if np.random.rand() < p:
# 干渉物に最短のノードをビーコンとして選択
min_distance = 1e10
beacon_node = beacon_nodes[0]
for node in beacon_nodes:
# ノード位置を取得
node_pos = self._start_tree.nodes[node, :self._NODE_NEAR_NODE_IDX]
# 距離を計算
self._interference_pos(node_pos)
distance = self._environment.distance
if distance < min_distance:
beacon_node = node
min_distance = distance
else:
# ランダムなノードをビーコンとして選択
beacon_node = np.random.choice(beacon_nodes)
# ビーコンの位置を取得
beacon_node_pos = self._start_tree.nodes[beacon_node, :self._NODE_NEAR_NODE_IDX]
# ビーコンの球の半径をコスト依存
beacon_radius = min_cost * self._PATH_OPT_RADIUS_COEF * (1 - min(self._PATH_OPT_ITER_MAX, iteration * self._PATH_OPT_ITER_COEF))
# 単位球内の位置をランダムで取得
unit_ball = self._get_unit_ball()
# 単位球に半径と中心位置を追加して，ランダムな位置とする
random_pos = beacon_node_pos + unit_ball * beacon_radius
return random_pos, beacon_radius, beacon_node_pos
def _get_unit_ball(self):
"""
単位球内の位置を取得
戻り値
position(numpy.ndarray): 単位球内の位置
"""
lenth_of_one_side = 1.0
while True:
# -1から1の範囲で，適当な座標を取得
position = np.random.uniform(np.ones(self._dim) * lenth_of_one_side * -1, np.ones(self._dim) * lenth_of_one_side)
# 距離を計算
distance = np.linalg.norm(position)
if distance <= 1:
# 単位球の中をプロット
# 単位球の中をプロットする確率は 単位球の体積 / 一辺が2の立方体の体積
break
else:
# 単位球の外をプロット
# プロットできなかった時は，辺の長さを小さくして，単位球の中をプロットしやすくする (whileループから早く抜けたいから)
lenth_of_one_side -= 0.0001
return position
def _set_sphere(self, center, radius):
"""
球情報を設定する
パラメータ
center(numpy.ndarray): 球の中心点
radius(float): 球の半径
"""
sphere = np.append(center.reshape(1, -1), radius).reshape(1, -1)
if len(self._spheres) == 0: # 初回だけ実施
self._spheres = sphere
else:
self._spheres = np.append(self._spheres, sphere, axis=0)
# RRT*との差異 ↑
# 新規追加 ↑
経路生成の環境 (environment.py)
本記事で使用する環境に関するクラスを作成した．
Robot2DEnv()クラスを使用する．
environment.py
# 環境の作成
# 標準ライブラリの読み込み
import numpy as np
# サードパーティーの読み込み
import fcl
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import MyRotation # 回転行列
class Base2DEnv:
"""
経路生成時の2次元環境
プロパティ
_distance(float): 干渉物との最短距離
_inteferences(dist): 干渉物の情報 (名称(円や長方形など) + 形状(半径，中心点など))
メソッド
public
inteferences(): _inteferencesプロパティのゲッター
distance(): _distanceプロパティのゲッター
is_collision(): 干渉判定
is_collision_dist(): 干渉判定 + 最短距離
"""
# 定数の定義
_Z_VALUE = 0.0 # z軸方向の値
def __init__(self):
"""
コンストラクタ
"""
objects = []
self._interferences = {}
self._rot = MyRotation()
# 円形の干渉物を定義 (円の位置 (x, y) と半径を定義)
circles = [[0.8, 0.8, 0.3], [1.2, 0.8, 0.3], [1.2, 1.2, 0.3], [0.8, 1.2, 0.3]]
for x, y, radius in circles:
# fclに円が定義されていないため，楕円のz軸を0として円とする
circle = fcl.Ellipsoid(radius, radius, self._Z_VALUE)
# 円の中心点が原点であるため，中心点を平行移動させる
translation = fcl.Transform(np.array([x, y, self._Z_VALUE]))
obj = fcl.CollisionObject(circle, translation)
# モデルを追加
objects.append(obj)
# 円形の干渉物を保存
self._interferences[INTERFERENCE.CIRCLE] = circles
# # 長方形の干渉物を定義
# 長方形の長さ (x, y) と中心点の位置 (x, y)
# rectangles = [[0.2, 0.3, 0.6, 0.6], [0.4, 0.5, 0.8, 1.0], [1.0, 1.0, 0.8, 0.8]]
# for x, y, center_x, center_y in rectangles:
# # 直方体の長さ (x, y, z) を設定
# rectangle = fcl.Box(x, y, self._Z_VALUE)
# # 長方形の中心点を設定．無回転とする
# translation = fcl.Transform(np.array([center_x, center_y, self._Z_VALUE]))
# obj = fcl.CollisionObject(rectangle, translation)
# # モデルを追加
# objects.append(obj)
# # 長方形の干渉物を保存
# self._interferences[INTERFERENCE.RECTANGLE] = rectangles
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(objects)
self._manager.setup()
# 最短距離を更新
self._distance = 0.0
@property
def distance(self):
"""
_distanceプロパティのゲッター
"""
return self._distance
@property
def interferences(self):
"""
_interferencesプロパティのゲッター (描画で使用する)
"""
return self._interferences
def is_collision(self, obj):
"""
干渉判定
パラメータ
obj(fcl.CollisionObject): 干渉物
戻り値
is_collision(bool): True / False = 干渉あり / 干渉なし
"""
# 衝突リクエスト
# num_max_contacts ... 最大の衝突点数
# enable_contact ... 衝突点情報の有無
request = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True)
col_data = fcl.CollisionData(request=request)
# 本環境と干渉物の干渉判定
self._manager.collide(obj, col_data, fcl.defaultCollisionCallback)
return col_data.result.is_collision
def is_collision_dist(self, obj, margin=0.0):
"""
干渉判定 + 最短距離
パラメータ
obj(fcl.CollisionObject): 干渉物
margin(float): 干渉判定のマージン
戻り値
collision(bool): True / False = 干渉あり / 干渉なし
"""
dist_data = fcl.DistanceData()
# 最短距離の結果を保存する
self._manager.distance(obj, dist_data, fcl.defaultDistanceCallback)
min_dist = dist_data.result.min_distance
# 最短距離の更新
self._distance = min_dist
if min_dist > margin:
# 干渉なし
collision = False
else:
# 干渉あり
collision = True
return collision
class Robot2DEnv(Base2DEnv):
"""
経路生成時の2次元環境 (ロボット用)
プロパティ
_distance(float): 干渉物との最短距離
_inteferences(dist): 干渉物の情報 (名称(円や長方形など) + 形状(半径，中心点など))
メソッド
public
inteferences(): _inteferencesプロパティのゲッター
distance(): _distanceプロパティのゲッター
is_collision(): 干渉判定
is_collision_dist(): 干渉判定 + 最短距離
"""
# 定数の定義
def __init__(self):
"""
コンストラクタ
"""
objects = []
self._interferences = {}
self._rot = MyRotation()
# 円形の干渉物を定義 (円の位置 (x, y) と半径を定義)
circles = [[1.5, 0.3, 0.3], [0.5, -0.5, 0.3]]
for x, y, radius in circles:
# fclに円が定義されていないため，球とする
circle = fcl.Sphere(radius)
# 円の中心点が原点であるため，中心点を平行移動させる
translation = fcl.Transform(np.array([x, y, self._Z_VALUE]))
obj = fcl.CollisionObject(circle, translation)
# モデルを追加
objects.append(obj)
# 円形の干渉物を保存
self._interferences[INTERFERENCE.CIRCLE] = circles
# 長方形の干渉物を定義
# 長方形の長さ (x, y) と中心点の位置 (x, y)，角度 [deg]
rectangles = [[0.4, 0.6, 1.4, -0.6, 0], [0.5, 0.2, 1.2, 0.7, 0]]
for x, y, center_x, center_y, angle in rectangles:
# 直方体の長さ (x, y, z) を設定
rectangle = fcl.Box(x, y, self._Z_VALUE)
# 回転行列の作成
rotation = self._rot.rot(np.deg2rad(angle), ROTATION_Z_AXIS)
# 長方形の中心点，回転行列を設定
translation = fcl.Transform(rotation, np.array([center_x, center_y, self._Z_VALUE]))
obj = fcl.CollisionObject(rectangle, translation)
# モデルを追加
objects.append(obj)
# 長方形の干渉物を保存
self._interferences[INTERFERENCE.RECTANGLE] = rectangles
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(objects)
self._manager.setup()
# 最短距離を更新
self._distance = 0.0
回転行列 (rotation.py)
回転行列の定義を説明する．
rotation.py
# 回転行列の定義
# 標準ライブラリの読み込み
import numpy as np
# サードパーティーの読み込み
# 自作モジュールの読み込み
from constant import * # 定数
class MyRotation:
"""
回転行列クラス
"""
_PITCH_THRESHOLD = 1e-4 # ピッチ角の閾値
_ZERO_NEAR = 1e-4 # 0近傍の閾値
_EPSILON = 1e-5 # 微小値
_ROT_MAX_VALUE = 1.0 # 回転行列の最大値
_ROT_MIN_VALUE = -1.0 # 回転行列の最小値
def _rot_x(self, theta):
"""
x軸方向にtheta[rad]回転させる回転行列
パラメータ
theta(float): 回転角度 [rad]
戻り値
rotation(numpy.ndarray): 回転行列
"""
rotation = np.array([[1, 0, 0 ],
[0, np.cos(theta), -np.sin(theta)],
[0, np.sin(theta), np.cos(theta)]])
return rotation
def _rot_y(self, theta):
"""
y軸方向にtheta[rad]回転させる回転行列
パラメータ
theta(float): 回転角度 [rad]
戻り値
rotation(numpy.ndarray): 回転行列
"""
rotation = np.array([[ np.cos(theta), 0, np.sin(theta)],
[ 0, 1, 0 ],
[-np.sin(theta), 0, np.cos(theta)]])
return rotation
def _rot_z(self, theta):
"""
z軸方向にtheta[rad]回転させる回転行列
パラメータ
theta(float): 回転角度 [rad]
戻り値
rotation(numpy.ndarray): 回転行列
"""
rotation = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
return rotation
def rot(self, theta, axis):
"""
回転軸に応じた回転行列の取得
パラメータ
theta(float): 回転角度 [rad]
axis(str): 回転軸
戻り値
rotation(numpy.ndarray): 回転行列
"""
if axis == ROTATION_X_AXIS:
# 回転軸がx軸
rotation = self._rot_x(theta)
elif axis == ROTATION_Y_AXIS:
# 回転軸がy軸
rotation = self._rot_y(theta)
elif axis == ROTATION_Z_AXIS:
# 回転軸がz軸
rotation = self._rot_z(theta)
else:
# 異常
raise ValueError(f"axis is abnormal. now is {axis}")
return rotation
2軸ロボットアーム (robot.py)
2軸ロボットアームの処理を説明する．
robot.py
# ロボットアームの運動学を記載
# ライブラリの読み込み
import numpy as np
# サードパーティーの読み込み
import fcl
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import MyRotation # 回転行列
class Robot:
"""
ロボットのベースクラス(抽象クラス)
プロパティ
_links(numpy.ndarray): ロボットのリンク長 [m]
_rot(Rotation): 回転行列クラス
_objects(list): 干渉物オブジェクト
_manager(fcl.DynamicAABBTreeCollisionManager): 干渉判定クラス
_jacov_thetas(list): 微分逆行列で取得した角度を保存
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
update(): 角度を与えて，各リンクの直方体を更新する
differential_inverse_kinematics(): 微分逆運動学
links(): _linksプロパティのゲッター
manager(): _managerプロパティのゲッター
jacov_thetas(): _jacov_thetasプロパティのゲッター
protected
_calc_homogeneou_matrix(): 同時変換行列の計算
_jacovian(): ヤコビ行列
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_NONE # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_NONE # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_NONE # リンク数
_DIMENTION_AXIS = DIMENTION_NONE # 回転軸数
_INITIAL_THETA = 0.0 # 初期回転角度 [rad]
_HOMOGENEOU_MAT_ELEMENT = 4 # 同時変換行列の次元数
_ROTATION_MAT_ELEMENT = 3 # 回転行列の次元数
_DETERMINANT_THRESHOLD = 1e-4 # 行列式の閾値
_BOX_WIDTH = 1e-2 # 各リンクの幅を定義
_JACOV_DELTA_TIME = 0.10 # ヤコビの1時刻
_JACOV_NEAR_POS = 1e-6 # 目標位置との近傍距離 [m]
_JACOV_MAX_COUNT = 100 # ヤコビの最大回数
_LEVENBERG_MARUQUARDT_LAMBDA = 0.1 # Levenberg-Marquardt法で使用するパラメータ
def __init__(self, links):
"""
コンストラクタ
パラメータ
links(numpy.ndarray): ロボットのリンク長 [m]
"""
if np.size(links) != self._DIMENTION_LINK:
# 異常
raise ValueError(f"links's size is abnormal. correct is {self._DIMENTION_Link}")
# プロパティの初期化
self._links = links
self._rot = MyRotation()
self._objects = []
self._manager = None
self._jacov_thetas = []
self._lambda = self._LEVENBERG_MARUQUARDT_LAMBDA
def _reset_jacov_thetas(self):
"""
_jacov_thetasプロパティのリセット
"""
if len(self._jacov_thetas) != 0:
self._jacov_thetas.clear()
@property
def links(self):
"""
_linksプロパティのゲッター
"""
return self._links
@property
def manager(self):
"""
_managerプロパティのゲッター
"""
return self._manager
@property
def jacov_thetas(self):
"""
_jacov_thetasプロパティのゲッター
"""
return self._jacov_thetas
def forward_kinematics(self, thetas):
"""
順運動学 (ロボットの関節角度からロボットの手先位置を算出)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
"""
raise NotImplementedError("forward_kinematics() is necessary override.")
def inverse_kinematics(self, pose):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
戻り値
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
raise NotImplementedError("inverse_kinematics() is necessary override.")
def forward_kinematics_all_pos(self, thetas):
"""
順運動学で全リンクの位置を取得
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
all_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
"""
raise NotImplementedError("forward_kinematics() is necessary override.")
def update(self, thetas):
"""
角度を与えて，各リンクの直方体を更新する
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
raise NotImplementedError("update() is necessary override.")
def differential_inverse_kinematics(self, thetas, target_pos):
"""
2点間の微分逆運動学
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
target_pos(numpy.ndarray): 目標位置 (位置[m]・姿勢[rad])
戻り値
target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
"""
raise NotImplementedError("differential_inverse_kinematics() is necessary override.")
def _jacovian(self, thetas):
"""
ヤコビ行列の計算
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
jacovian(numpy.ndarray): ヤコビ行列
"""
raise NotImplementedError("_jacovian() is necessary override.")
def _calc_homogeneou_matrix(self, thetas):
"""
同時変換行列の計算
パラメータ
thetas(numpy.ndarray): 関節角度 [rad]
戻り値
homogeneou_matix(numpy.ndarray): 全リンクの同時変換行列
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
homogeneou_matix = np.zeros((self._DIMENTION_LINK, self._HOMOGENEOU_MAT_ELEMENT, self._HOMOGENEOU_MAT_ELEMENT))
# 1リンク前の同時変換行列
prev_homogeneou_matrix = np.eye(self._HOMOGENEOU_MAT_ELEMENT)
for i in range(self._DIMENTION_THETA):
# 4行4列の要素を1に更新
homogeneou_matix[i, -1, -1] = 1
# 回転行列の計算
rotation_matrix = self._rot.rot(thetas[i], self._axiss[i])
# リンク間の相対位置を取得
relative_position = self._relative_positions[i].reshape(1, -1)
# 同時変換行列に回転行列を保存
homogeneou_matix[i, :self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT] = rotation_matrix
# 同時変換行列に相対位置を保存
homogeneou_matix[i, :self._ROTATION_MAT_ELEMENT, self._ROTATION_MAT_ELEMENT] = relative_position
# 1リンク前の同時変換行列と組み合わせる
homogeneou_matix[i] = np.dot(prev_homogeneou_matrix, homogeneou_matix[i])
# 1リンク前の同時変換行列の更新
prev_homogeneou_matrix = homogeneou_matix[i]
return homogeneou_matix
class Robot2DoF(Robot):
"""
2軸ロボットクラス
プロパティ
_links(numpy.ndarray): ロボットのリンク長
_rot(Rotation): 回転行列クラス
_axiss(list): 関節の回転軸
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_2D # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_2D # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_2D # リンク数
def __init__(self, links):
"""
コンストラクタ
パラメータ
links(numpy.ndarray): ロボットのリンク長 [m]
"""
# 親クラスの初期化
super().__init__(links)
# ロボットの各リンクを直方体として定義する
self._objects = []
# (全リンクの角度を0とした時の) 各リンク間の相対位置
self._relative_positions = np.zeros((self._DIMENTION_POSE + 1, 3))
self._relative_positions[0] = np.array([0, 0, 0])
self._relative_positions[1] = np.array([self._links[0], 0, 0])
self._relative_positions[2] = np.array([self._links[1], 0, 0])
# リンク1とリンク2は回転軸がz軸である
self._axiss = [ROTATION_Z_AXIS, ROTATION_Z_AXIS]
# 初期角度
initial_thetas = np.zeros(self._DIMENTION_THETA)
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(initial_thetas)
# 1つ前のリンクの回転行列を更新
prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)
# ロボットの各リンクを直方体として定義する
for i in range(self._DIMENTION_THETA):
# 各リンクの回転行列を定義
rotation = self._rot.rot(initial_thetas[i], self._axiss[i])
rotation = np.dot(prev_rotation, rotation)
# 各リンクの中心位置 (x, y, z) を定義
center = np.zeros(DIMENTION_3D)
center[:self._DIMENTION_POSE] = all_link_pose[i + 1] / 2 + all_link_pose[i]
# 直方体の定義 (x, y, zの長さを保存)
box = fcl.Box(self._relative_positions[i + 1, 0], 2 * self._BOX_WIDTH, 2 * self._BOX_WIDTH)
# 直方体の中心を定義 (位置・姿勢)
translation = fcl.Transform(rotation, center)
obj = fcl.CollisionObject(box, translation)
# モデルを追加
self._objects.append(obj)
# 1つ前のリンクの回転行列を更新
prev_rotation = rotation
# 直方体をAABBとして，定義
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(self._objects)
self._manager.setup()
self._jacov_thetas = []
def forward_kinematics(self, thetas):
"""
順運動学 (ロボットの関節角度からロボットの手先位置を算出)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 同時変換行列(2 * 4 * 4)を計算する
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# 最終リンクの同時変換行列(最終リンク座標の位置・姿勢)より，手先位置を計算する
final_link_matrix = homogeneou_matrix[self._DIMENTION_LINK - 1]
# 最終リンクから手先位置までの早退位置(4ベクトル)を定義
relative_pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
relative_pos[:self._HOMOGENEOU_MAT_ELEMENT - 1] = self._relative_positions[-1]
pose = np.dot(final_link_matrix, relative_pos)
# 手先位置(x, y)を取得
pose = pose[:self._DIMENTION_POSE]
return pose
def differential_inverse_kinematics(self, thetas, target_pos):
"""
2点間の微分逆運動学
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
target_pos(numpy.ndarray): 目標位置 (位置[m]・姿勢[rad])
戻り値
target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
"""
# 引数の確認
if np.size(thetas) != self._DIMENTION_THETA:
# 異常
raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")
# 現在の関節角度を保存 (異なるアドレスにデータを保存するためにnp.copy()を採用)
current_thetas = np.copy(thetas)
# 目標値の関節角度が見つかったどうか
success_flg = False
# 微分逆行列で取得したデータを初期化
self._reset_jacov_thetas()
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 目標位置に近づくまでループ
for i in range(self._JACOV_MAX_COUNT):
# ヤコビ行列の取得
jacovian = self._jacovian(current_thetas)
# 現在の手先位置を計算
current_pos = self.forward_kinematics(current_thetas)
# 位置の差分を計算
dP = target_pos - current_pos
# # <修正内容> 旧内容 (特異点非対応) ↓
# # dP(位置の差分) = J(ヤコビ行列) * dTheta(角度の差分)
# # dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
# # 行列式が0近傍であるかの確認(ヤコビ行列の逆行列の存在確認)
# det = np.linalg.det(jacovian)
# if abs(det) <= self._DETERMINANT_THRESHOLD:
# # 0近傍であるため，逆行列が存在しない
# raise ValueError(f"abs(det) is near 0. abs(det) is {abs(det)}")
# # dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
# dTheta = np.dot(np.linalg.inv(jacovian), dP)
# # <修正内容> 旧内容 (特異点非対応) ↑
# # <修正内容> 新内容 (レーベンバーグマルカート法) ↓
# # 逆行列の中身を作成
# new_jacovian = np.dot(jacovian, jacovian.T) + self._lambda * np.eye(jacovian.shape[0])
# dTheta = np.dot(np.dot(jacovian.T, np.linalg.inv(new_jacovian)), dP)
# # <修正内容> 新内容 (レーベンバーグマルカート法) ↑
# # <修正内容> 新内容 (特異値分解) ↓
# # ヤコビ行列を特異値分解する
# U, S, V_T = np.linalg.svd(jacovian)
# # 特異値の閾値を作成 (特異値の最大値を使用する)
# threshold = S[0] * 0.05
# # 特異値を安定化する
# S_inv = np.array([1/s if s > threshold else 0 for s in S])
# # 擬似逆行列を構成する
# J_pinv = np.dot(np.dot(V_T.T, np.diag(S_inv)), U.T)
# dTheta = np.dot(J_pinv, dP)
# # <修正内容> 新内容 (特異値分解) ↑
# <修正内容> 新内容 (レーベンバーグマルカート法 + 可操作度) ↓
# 可操作度を計算
manipulabitity = np.sqrt(np.linalg.det(np.dot(jacovian, jacovian.T)))
print(f"manipulabitity = {manipulabitity}")
if manipulabitity <= 0.1:
inv_data = np.dot(jacovian, jacovian.T) + self._lambda * np.eye(jacovian.shape[0])
dTheta = np.dot(np.dot(jacovian.T, np.linalg.inv(inv_data)), dP)
else:
# 特異点近傍ではないため，逆行列を普通に計算
dTheta = np.dot(np.linalg.inv(jacovian), dP)
# <修正内容> 新内容 (レーベンバーグマルカート法 + 可操作度) ↑
# 関節角度の更新 (+=とするとcurrent_thetasが常に同じアドレスになるため，current_thetasを異なるアドレスとしたい)
# print(f"id(current_thetas) = {id(current_thetas)}") # アドレス確認
# current_thetas = current_thetas + dTheta * self._JACOV_DELTA_TIME
# 位置の差分が大きいほど，角度の更新量を小さくしたい
coefficient = 1 / max(0.5, np.linalg.norm(dP))
# coefficient = 1
current_thetas = current_thetas + dTheta * coefficient * self._JACOV_DELTA_TIME
# print(f"id(current_thetas) = {id(current_thetas)}") # アドレス確認
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 更新後の手先位置が目標位置の近傍であれば，処理終了とする
current_pos = self.forward_kinematics(current_thetas)
distance = np.linalg.norm(target_pos - current_pos)
# print(f"distance = {distance}")
if distance <= self._JACOV_NEAR_POS:
# 近傍のため，処理終了
success_flg = True
break
# if not success_flg:
# # 目標位置の関節角度が見つからない
# raise ValueError(f"target_pos's theta is not found. please change target_pos")
target_thetas = current_thetas
return target_thetas
def _jacovian(self, thetas):
"""
ヤコビ行列の計算
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
jacovian(numpy.ndarray): ヤコビ行列
"""
# 引数の確認
if np.size(thetas) != self._DIMENTION_THETA:
# 異常
raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")
# jacovian = [[-l1 * sin(theta1) - l2 * sin(theta12), -l2 * sin(theta12)], [l1 * cos(theta1) + l2 * cos(theta12), l2 * cos(theta12)]]
# 各リンクの長さをローカル変数に保存
l1 = self._links[0]
l2 = self._links[1]
# 三角関数の計算
sin1 = np.sin(thetas[0])
cos1 = np.cos(thetas[0])
sin12 = np.sin(thetas[0] + thetas[1])
cos12 = np.cos(thetas[0] + thetas[1])
# ヤコビ行列
jacovian = np.array([[-l1 * sin1 - l2 * sin12, -l2 * sin12],
[ l1 * cos1 + l2 * cos12, l2 * cos12]])
return jacovian
def inverse_kinematics(self, pose, upper=False):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
upper(bool): 腕が上向かどうか
戻り値
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
# パラメータの次元数を確認
if np.size(pose) != self._DIMENTION_POSE:
raise ValueError(f"parameter pose's size is abnormal. pose's size is {np.size(pose)}")
# c2 = {(px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)} / (2 * l1 * l2)
px = pose[0]
py = pose[1]
l1 = self._links[0]
l2 = self._links[1]
cos2 = ((px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)) / (2 * l1 * l2)
# cosの範囲は-1以上1以下である
if cos2 < -1 or cos2 > 1:
# 異常
raise ValueError(f"cos2 is abnormal. cos2 is {cos2}")
# sinも求めて，theta2をatan2()より算出する
sin2 = np.sqrt(1 - cos2 ** 2)
theta2 = np.arctan2(sin2, cos2)
if not upper:
# 下向きの角度のため，三角関数も更新
theta2 = -theta2
sin2 = np.sin(theta2)
cos2 = np.cos(theta2)
# 行列計算
# [c1, s1] = [[l1 + l2 * c2, -l2 * s2], [l2 * s2, l1 + l2 * c2]] ** -1 * [px, py]
element1 = l1 + l2 * cos2
element2 = -l2 * sin2
matrix = np.array([[ element1, element2],
[-element2, element1]])
# 行列式を計算
det = np.linalg.det(matrix)
# 0近傍の確認
if det <= self._DETERMINANT_THRESHOLD and det >= -self._DETERMINANT_THRESHOLD:
# 0近傍 (異常)
raise ValueError(f"det is abnormal. det is {det}")
# [c1, s1]の計算
cos1_sin1 = np.dot(np.linalg.inv(matrix), pose)
# theta1をatan2()より算出する
theta1 = np.arctan2(cos1_sin1[1], cos1_sin1[0])
thetas = np.array([theta1, theta2])
return thetas
def forward_kinematics_all_link_pos(self, thetas):
"""
順運動学で全リンクの位置を取得 (グラフの描画で使用する)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
all_link_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 同時変換行列(2 * 4 * 4)を計算する
homogeneou_matix = self._calc_homogeneou_matrix(thetas)
# 全リンクの座標系の原点を取得
all_link_pose = np.zeros((self._DIMENTION_LINK + 1, self._DIMENTION_POSE))
for i, matrix in enumerate(homogeneou_matix):
# 同時変換行列から位置を取得
pos = matrix[:self._DIMENTION_POSE, self._ROTATION_MAT_ELEMENT].reshape(1, -1)
all_link_pose[i] = pos
# 最後のリンクの座標系の原点から，手先の位置を計算する
pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
pos[:DIMENTION_3D] = self._relative_positions[-1]
all_link_pose[-1] = np.dot(homogeneou_matix[-1], pos)[:self._DIMENTION_POSE].reshape(1, -1)
return all_link_pose
def update(self, thetas):
"""
角度を与えて，各リンクの直方体を更新する
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(thetas)
# 1つ前のリンクの回転行列を定義
prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)
# ロボットの各リンクを直方体として定義する
for i in range(self._DIMENTION_THETA):
# 各リンクの回転行列を定義
rotation = self._rot.rot(thetas[i], self._axiss[i])
# 1つ前のリンクの回転も考慮する
rotation = np.dot(prev_rotation, rotation)
# 各リンクの中心位置 (x, y, z) を定義
center = np.zeros(DIMENTION_3D)
center[:self._DIMENTION_POSE] = (all_link_pose[i + 1] - all_link_pose[i]) / 2 + all_link_pose[i]
# 直方体の中心を定義 (位置・姿勢)
translation = fcl.Transform(rotation, center)
# モデルの位置を更新
self._objects[i].setTransform(translation)
# 1つ前のリンクの回転行列を更新
prev_rotation = rotation
# AABBを更新
self._manager.update()
RRT*-Smart のアニメーション (animation.py)
上記でRRT*-Smart の実装をしたが，想定通りに動いているかの確認が必要なため，アニメーションを作成して確認する．
animation.py
# ロボットのアニメーションを実施
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画用
import matplotlib.animation as ani # アニメーション用
import matplotlib.patches as patches # 2次元形状の描画
# 自作モジュールの読み込み
from constant import * # 定数
class RobotAnimation:
"""
ロボットのアニメーション作成
プロパティ
_figure: 描画枠
_axis: 描画内容
publicメソッド (全てのクラスから参照可能)
plot_Animation(): アニメーション作成
protectedメソッド (自クラスまたは子クラスが参照可能)
_reset2D(): 2次元データのリセット
"""
# 定数の定義
_ANIMATION_NAME = "robot_animation.gif"
_PLOT_NAME = "robot_plot.gif"
def __init__(self):
"""
コンストラクタ
"""
pass
def _reset2D(self):
"""
2次元データのリセット
"""
self._figure = plt.Figure()
self._axis = self._figure.add_subplot(111)
# X/Y軸に文字を記載
self._axis.set_xlabel("X")
self._axis.set_ylabel("Y")
self._axis.grid()
self._axis.set_aspect("equal")
def _plot_circle(self, x, y, radius):
"""
円の描画
パラメータ
x(float): 中心点 (x)
y(float): 中心点 (y)
radius(float): 半径
"""
circle = patches.Circle((x, y), radius, color="gray", alpha=0.5)
self._axis.add_patch(circle)
def _plot_rectangle(self, x, y, width, height, angle):
"""
長方形の描画
パラメータ
x(float): 左下隅の座標 (x)
y(float): 左下隅の座標 (y)
width(float): 幅
height(float): 高さ
angle(float): 角度 [deg]
"""
rect = patches.Rectangle((x, y), width, height, angle=angle, color="gray", alpha=0.5)
# 長方形を軸に追加
self._axis.add_patch(rect)
def _plot_environment(self, environment):
"""
アニメーション作成
パラメータ
environment(Robot2DEnv): 経路生成時の環境
"""
if environment is None:
# 環境がないため，何もしない
return
for name, datas in environment.interferences.items():
# 干渉物の名称
if name == INTERFERENCE.CIRCLE:
# 円
for x, y, radius in datas:
self._plot_circle(x, y, radius)
elif name == INTERFERENCE.RECTANGLE:
# 長方形
for x, y, center_x, center_y, angle in datas:
# 中心点から左隅の点に移動させるために，-x/2，-y/2を実施
self._plot_rectangle(center_x - x / 2, center_y - y / 2, x, y, angle)
else:
# 何もしない
pass
def plot_Animation(self, robot, all_link_thetas, environment, anime_file_name=""):
"""
アニメーション作成
パラメータ
robot(Robot2DoF): ロボットクラス
all_link_thetas(numpy.ndarray): 全リンクの回転角度
environment(Robot2DEnv): 経路生成時の環境
anime_file_name(str): アニメーションのファイル名
"""
# 引数の確認
if all_link_thetas.size == 0:
raise ValueError(f"all_link_thetas's size is abnormal. all_link_thetas's size is {all_link_thetas.size}")
# データをリセットする
self._reset2D()
# 全画像を保存する
imgs = []
# 環境の描画
self._plot_environment(environment)
# 手先位置の軌跡を保存
position_trajectory = np.zeros((all_link_thetas.shape[0], DIMENTION_2D))
# 始点と終点をプロット
# 始点位置を取得
start_pos = robot.forward_kinematics(all_link_thetas[0])
start_image = self._axis.scatter(start_pos[0], start_pos[1], color="cyan")
end_pos = robot.forward_kinematics(all_link_thetas[-1])
end_image = self._axis.scatter(end_pos[0], end_pos[1], color="red")
# 軌道生成
for i, thetas in enumerate(all_link_thetas):
path_images = []
# 順運動学により，全リンク (ベースリンク, リンク1，手先位置) の位置を計算
all_link_pos = robot.forward_kinematics_all_link_pos(thetas)
# 線プロット
image = self._axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], color="blue")
path_images.extend(image)
# 点プロット
image = self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], color="black", alpha=0.5)
path_images.extend([image])
# 手先位置を保存
position_trajectory[i] = all_link_pos[-1]
# 手先位置の軌跡をプロット
image = self._axis.plot(position_trajectory[:i + 1, 0], position_trajectory[:i + 1, 1], color="lime")
path_images.extend(image)
# 始点と終点の画像を保存
path_images.extend([start_image])
path_images.extend([end_image])
# 画像を1枚にまとめて保存
imgs.append(path_images)
# アニメーション作成
animation = ani.ArtistAnimation(self._figure, imgs)
if anime_file_name:
# ファイル名が存在する
animation.save(anime_file_name, writer='imagemagick')
else:
# ファイル名が存在しない
animation.save(self._ANIMATION_NAME, writer='imagemagick')
plt.show()
メイン処理 (main.py)
経路生成およびアニメーション作成のメイン処理を説明する．
main.py
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot2DoF # ロボットクラス
from animation import RobotAnimation # ロボットのアニメーション
from rrt import RRTRobot, RRTStarRobot, RRTStarSmartRobot # 経路生成
from environment import Robot2DEnv # 経路生成の環境
CONST_SEED = 1 # 固定したいシード値
def main():
"""
メイン処理
"""
# 2軸ロボットのリンク長
links = np.array([1.0, 1.0])
# 2軸ロボットのインスタンスを作成
robot = Robot2DoF(links)
# 始点
start_pos = np.array([1.0, -1.0])
# 終点
end_pos = np.array([1.0, 1.0])
try:
# 始点と終点の逆運動学
start_theta = robot.inverse_kinematics(start_pos)
end_theta = robot.inverse_kinematics(end_pos)
except Exception as e:
# 逆運動学の解が存在しない (始点または終点が異常な値)
raise ValueError(f"please start_pos or end_pos is change. pos is singularity")
# RRTのデバッグフラグ
debug = True
# 経路の修正方法
# modification = MODIFICATION.PARTIAL_START
modification = None
# 環境
environment = Robot2DEnv()
# 補間の種類 (位置空間/関節空間)
interpolations = [INTERPOLATION.POSITION, INTERPOLATION.JOINT]
# interpolations = [INTERPOLATION.JOINT, ]
# 経路生成手法
path_plans = [[RRTRobot, "rrt"], [RRTStarRobot, "rrt_star"], [RRTStarSmartRobot, "rrt_star_smart"]]
for path_plan, name in path_plans:
rrt = path_plan()
for interpolation in interpolations:
# アルゴリズムで比較したいからシード値は固定
np.random.seed(CONST_SEED)
if interpolation == INTERPOLATION.POSITION:
# 位置空間での経路生成の実行
planning_result = rrt.planning(start_pos, end_pos, environment, robot, interpolation, modification=modification, debug=debug)
else:
# 関節空間での経路生成の実行
planning_result = rrt.planning(start_theta, end_theta, environment, robot, interpolation, modification=modification, debug=debug)
print(f"planning_result = {planning_result}")
if not planning_result:
# 経路生成の失敗
continue
# アニメーション作成
robotAnime = RobotAnimation()
if interpolation == INTERPOLATION.POSITION:
# 位置空間による RRT 経路生成
# 逆運動学による関節を取得
thetas = np.zeros((rrt.pathes.shape[0], DIMENTION_2D))
for i, pos in enumerate(rrt.pathes):
# 逆運動学
theta = robot.inverse_kinematics(pos)
thetas[i] = theta
# ファイル名
file_name = f"{name}_robot_pos_anime.gif"
robotAnime.plot_Animation(robot, thetas, environment, file_name)
else:
# 関節空間による RRT 経路生成
# ファイル名
file_name = f"{name}_robot_joint_anime.gif"
robotAnime.plot_Animation(robot, rrt.pathes, environment, file_name)
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
性能評価およびアニメーション
RRT*-Smart の性能評価およびアニメーションを展開する．
RRT と RRT*-Smart と RRT* の経路比較
RRT*-Smart は RRT* の探索範囲を効率化した手法であるため，経路の比較をする．
アニメーションの見方は以下の通りである．
・水色の星形(左下)データ：経路生成の始点
・緑色の星形(右上)データ：経路生成の終点
・灰色の星形データ：作成されたノード
・赤色の線：新規データと最短コスト(距離)の親ノードを結んだ線
・緑色の線：最終的な始点から終点までの経路
・灰色の円：干渉物
初めに，RRT でロボットの位置空間上で作成した経路を下図に添付した．
次に，RRT* でロボットの位置空間上で作成した経路を下図に添付した．
最後に，RRT*-Smart でロボットの位置空間上で作成した経路を下図に添付した．
RRT* よりも RRT*-Smart の方が干渉物の近傍を通る経路となった．
今回は探索回数を1,000回にしている．そのため，もっと回数を増やせばより最適な経路になったと考えられる．また，パラメータを変えることで，より最短な経路になる可能性が高い．
他の環境でも試してみたいと思った．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・干渉物の存在する2次元空間で，2軸ロボットアームを用いて RRT*-Smart による経路生成 + アニメーション
次記事では，下記内容を実装していきます．
・干渉物の存在する2次元空間で，2軸ロボットアームを用いて RRT*-Connect (RRT* + RRT-Connect) による経路生成 + アニメーション
(https://qiita.com/haruhiro1020/items/253764ced728003dba75)
・RRT*-Smart の探索1回ごとのアニメーション作成
(https://qiita.com/haruhiro1020/items/f6924aebc24d1091641f)