はじめに
私がロボットに関して興味を持っており，ロボットの経路を作成したいと思ったところ，RRT (Rapidly-exploring Random Tree) と呼ばれる経路生成の手法に辿り着いた．
本記事では，経路生成の古典的手法である RRT (Rapidly-exploring Random Tree) を改良した RRT*-Smart をPythonより，実装してみる．
前記事では，球と直方体の干渉物が存在する3次元空間を探索する RRT*-Smart を実装した．
前記事の内容を下記に記します．
・https://qiita.com/haruhiro1020/items/f6924aebc24d1091641f
本記事では，球と直方体の干渉物が存在する3次元空間を探索する6軸ロボットアームでの 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 の違いを説明したのち，ソースコードおよび性能評価を実施する．
本記事で経路生成する環境は下図の通りである．
青色の点が6軸ロボットアームの初期位置，赤色の点が6軸ロボットアームの目標位置である．
下図環境下で，干渉しない経路を生成するのが，本記事の目標である．
6軸ロボットアームに関しては，下記リンクに記載．
・https://qiita.com/haruhiro1020/items/e543294c6912e56e704f
python-fclに関しては，下記リンクに記載．
・https://qiita.com/haruhiro1020/items/d364e4c56b1570e1423a
本記事で実装すること
・球と直方体の干渉物が存在する3次元空間で，6軸ロボットアームを使用してRRT*-Smart による経路生成
・RRT* とRRT*-Smart の経路生成結果をアニメーションで表現
本記事では実装できないこと(将来実装したい内容)
・球と直方体の干渉物が存在する3次元空間で，7軸ロボットアーム (冗長ロボットアーム) を使用して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 アルゴリズムは前記事に記載したため，本記事では割愛する．
(https://qiita.com/haruhiro1020/items/9d9d8f6e8242c7148727)
6軸ロボットアームに関して
6軸ロボットアームを説明する．
ロボットアームの順運動学(関節角度から手先位置を算出)は以下記事にて説明した．
・https://qiita.com/haruhiro1020/items/a240f8c8e538e75473a3
ロボットアームの逆運動学(手先位置から関節角度を算出)は以下記事にて説明した．
・https://qiita.com/haruhiro1020/items/af1d404b518aa6e13468
ソースコードの説明
Pythonにて，RRT*-Smart のソースコードを作成する．
定数の定義 (constant.py)
複数ファイルで使用する定数を定義したファイルは下記の通りです．
constant.py
# 複数ファイルで使用する定数の定義
from enum import Enum
# 次元数を定義
DIMENTION_NONE = -1 # 未定義
DIMENTION_2D = 2 # 2次元
DIMENTION_3D = 3 # 3次元
DIMENTION_6D = 6 # 6次元
# シード値の最小値と最大値
MIN_SEED = 0
MAX_SEED = 2 ** 32 - 1 # 4バイト (uint) の最大値
# ノードの最短ノード要素とコスト要素を定義
# RRT
RRT_NEAR_NODE_IDX = -1 # RRTでの最短ノード要素
# RRT-Connect
RRT_CONNECT_NEAR_NODE_IDX = -1 # RRT-Connectでの最短ノード要素
# RRT*
RRT_STAR_NEAR_NODE_IDX = -3 # RRT*での最短ノード勝訴
RRT_STAR_COST_IDX = -2 # RRT*でのコスト要素
RRT_STAR_RADIUS_IDX = -1 # RRT*での半径要素
# Informed 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*-Smart
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での半径要素
# RRT*-Connect
RRT_STAR_CONNECT_NEAR_NODE_IDX = -3 # RRT*-Smartでの最短ノード勝訴
RRT_STAR_CONNECT_COST_IDX = -2 # RRT*-Smartでのコスト要素
RRT_STAR_CONNECT_RADIUS_IDX = -1 # RRT*-Smartでの半径要素
# RRT-Connectの状態を定義
STATE_START_TREE_RANDOM = 0 # 始点ツリーにランダム点を追加
STATE_START_TREE_END_TREE = 1 # 始点ツリーから終点ツリーへノードを伸ばす
STATE_END_TREE_RANDOM = 2 # 終点ツリーにランダム点を追加
STATE_END_TREE_START_TREE = 3 # 終点ツリーから始点ツリーへノードを伸ばす
# 回転軸
ROTATION_X_AXIS = "rot_x" # x軸周りに回転
ROTATION_Y_AXIS = "rot_y" # y軸周りに回転
ROTATION_Z_AXIS = "rot_z" # z軸周りに回転
ROTATION_X_NEGATIVE_AXIS = "rot_neg_x" # x軸周りに逆回転
ROTATION_Y_NEGATIVE_AXIS = "rot_neg_y" # y軸周りに逆回転
ROTATION_Z_NEGATIVE_AXIS = "rot_neg_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 = "graph" # グラフ
PLOT_ANIMATION = "anime" # アニメーション
# 補間方法の定義
class INTERPOLATION(Enum):
"""
補間方法
"""
JOINT = "joint" # 関節補間
POSITION = "pos" # 位置補間
LINEAR = "linear" # 直線補間
CUBIC = "cubic" # 3次補間
QUINTIC = "quintic" # 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による経路生成手法の定義
class PATHPLAN(Enum):
"""
経路生成手法
"""
RRT = "rrt" # RRTによる経路生成
# 双方向RRT
RRT_CONNECT = "rrt_connect" # RRT-Connectによる経路生成
# RRT*系列
RRT_STAR = "rrt_star" # RRT*による経路生成
RRT_STAR_SMART = "rrt_star_smart" # RRT*-Smartによる経路生成
RRT_STAR_CONNECT = "rrt_star_connect" # RRT*-Connectによる経路生成
INFORMED_RRT_STAR = "informed_rrt_star" # Informed RRT*による経路生成
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プロパティのゲッター
reset(): データの初期化
add_node(): ノードの追加
get_near_node(): 最短距離のノードを取得
get_near_node_list(): ノードと近傍ノードをリストで取得
protected
_chk_node_exist(): ノードが存在するかの確認
"""
# 定数の定義
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にロボットを追加したクラス
プロパティ
_name(str): 経路生成手法名
_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
プロパティのゲッター関連
name(): _nameプロパティのゲッター
start_tree(): _treeプロパティのゲッター
start_random_tree(): _random_treeプロパティのゲッター
pathes(): _pathesプロパティのゲッター
strict_min_pos(): _strict_min_posプロパティのゲッター
strict_max_pos(): _strict_max_posプロパティのゲッター
debug(): _debugプロパティのゲッター
robot(): _robotプロパティのゲッター
environment(): _environmentプロパティのゲッター
interpolation(): _interpolationプロパティのゲッター
dim(): _dimプロパティのゲッター
メイン処理関連
planning(): 経路生成の実装
protected
メイン処理関連
_add_node_start_tree(): 始点ツリーにノードを追加
_chk_end_pos_dist(): 終点との距離が一定範囲内であるかの確認
_fin_planning(): 経路生成の終了処理
_calc_new_pos(): 最短ノードからランダムな値方向へ新しいノード(位置)を作成
_get_random_pos(): ランダムな位置を取得
準備処理関連
_preparation_planning(): 経路生成の準備
_reset(): データの初期化
_set_robot(): 経路生成したいロボット情報を設定
_strict_planning_pos(): 探索範囲を制限する
干渉判定の処理関連
_line_interference(): 2点間の干渉チェック
_interference_pos(): 干渉判定
ファイル関連
_make_folder(): フォルダーの作成
_reset_folder(): フォルダー内のファイルを全削除
_get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
_make_path_plan_folder(): 経路生成結果を保存するフォルダー作成
_save(): 生成した経路をファイル保存
_save_numpy_data_to_txt(): Numpyデータをテキストファイルに保存
経路生成後の後処理関連
_modify_path(): 始点から終点までの経路を修正
_get_shortcut_two_nodes(): Shortcutで使用する2つのノードを取得
_path_partial_shortcut(): Partial Shortcut (経路上の2点をランダムで選択して, 2点内の全ノード内の1パラメータを直線補間して, 干渉なければ2点内のノード内の1パラメータを修正)
_path_shortcut(): Shortcut (経路上の2点をランダムで選択して, 干渉なければ2点内のノードを全削除)
_path_pruning_random(): Pruning (経路の経由点をランダム点の削除)
_path_pruning_from_end(): Pruning (経路の経由点を終点方向から順番に削除)
_path_pruning_from_start(): Pruning (経路の経由点を始点方向から順番に削除)
"""
# 定数の定義
# ファイル名の定義
# 各経路生成手法で絶対に定義するべきファイル名 ↓
# _pathesプロパティを保存するファイル名
_FILE_NAME_PATHES = f"{PATHPLAN.RRT.value}_pathes.csv"
# _start_treeプロパティを保存するファイル名
_FILE_NAME_START_TREE = f"{PATHPLAN.RRT.value}_start_tree.csv"
# _start_random_treeプロパティを保存するファイル名
_FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT.value}_start_random_tree.csv"
# _before_modify_pathesプロパティを保存するファイル名
_FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT.value}_before_modify_pathes.csv"
# _pruning_pathesプロパティを保存するファイル名
_FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT.value}_pruning_pathes.csv"
# _shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT.value}_shortcut_pathes.csv"
# _partial_shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT.value}_partial_shortcut_pathes.csv"
# 毎処理保存するファイル名の定義
# Pruning実施毎の経路を保存
_FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT.value}_pruning_each_pathes"
# Shortcut実施毎の経路を保存
_FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT.value}_shortcut_each_pathes"
# Partial Shortcut実施毎の経路を保存
_FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT.value}_partial_shortcut_each_pathes"
# 各経路生成手法で絶対に定義するべきファイル名 ↑
# フォルダー名の定義
_FOLODER_NAME_PRUNING = "Pruning" # Pruning結果を保存するフォルダ
# Shortcut結果を保存するフォルダ
_FOLODER_NAME_SHORTCUT = "Shortcut"
# PartialShortcut結果を保存するフォルダ
_FOLODER_NAME_PARTIAL_SHORTCUT = "PartialShortcut"
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX # ノード内の最短ノード要素
# 干渉判定の定義
_DEVIDED_DISTANCE_POS = 0.01 # 2点間を分割する際の基準距離 [m]
_DEVIDED_DISTANCE_JOINT = 0.005 # 2点間を分割する際の基準距離 [rad]
# 干渉物とのマージン (最短距離がマージンよりも小さかったら干渉ありとする) [m]
_INTERFERENCE_MARGIN = 0.05
# 探索に関する定義
_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._name = PATHPLAN.RRT.value
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.POSITION.value
self._moving_value = self._MOVING_VALUE_JOINT
# PruningやShortcut関連のプロパティの初期化
self._before_modify_pathes = []
self._pruning_pathes = []
self._shortcut_pathes = []
self._partial_shortcut_pathes = []
# プロパティのゲッターメソッド ↓
@property
def name(self):
"""
_nameプロパティのゲッター
"""
return self._name
@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
@property
def debug(self):
"""
_debugプロパティのゲッター
"""
return self._debug
@property
def robot(self):
"""
_robotプロパティのゲッター
"""
return self._robot
@property
def environment(self):
"""
_environmentプロパティのゲッター
"""
return self._environment
@property
def interpolation(self):
"""
_interpolationプロパティのゲッター
"""
return self._interpolation
@property
def dim(self):
"""
_dimプロパティのゲッター
"""
return self._dim
# プロパティのゲッターメソッド ↑
# メイン処理メソッド ↓
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._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)
# 始点ノードを設定
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()
# ファイルに保存する
self._save()
return result
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 _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 _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 _preparation_planning(self, start_pos, end_pos, environment, robot, interpolation, modification, debug):
"""
経路生成の準備
パラメータ
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
environment(Base2DEnv): 環境
robot(Robot): ロボットクラス
interpolation(int): 補間方法 (関節空間/位置空間)
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
debug(bool): デバッグフラグ
"""
# デバッグフラグの更新
self._debug = debug
# 修正方法の更新
self._modification = modification
# データの初期化
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}")
# 2，3軸のロボット以外は，関節空間での探索しか認めない
# (6軸ロボットの姿勢の探索が未実装だから)
if not (start_pos_dim == DIMENTION_2D or start_pos_dim == DIMENTION_3D):
# 関節空間以外はエラー
if interpolation != INTERPOLATION.JOINT.value:
raise ValueError(f"interpolation is abnormal. interpolation is allowed only {INTERPOLATION.JOINT.value}")
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._make_path_plan_folder(interpolation)
# フォルダー内のファイルを全部削除
self._reset_folder(interpolation)
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.POSITION.value
def _set_robot(self, robot, interpolation):
"""
経路生成したいロボット情報を設定
パラメータ
robot(Robot): ロボットクラス
interpolation(int): 補間の種類 (関節補間/位置補間)
"""
self._robot = robot
if interpolation == INTERPOLATION.POSITION.value:
# 位置空間
self._moving_value = self._MOVING_VALUE_POS
elif interpolation == INTERPOLATION.JOINT.value:
# 関節空間
self._moving_value = self._MOVING_VALUE_JOINT
else:
# 異常値
raise ValueError(f"interpolation is abnormal. interpolation is {interpolation}")
# 補間種類の更新
self._interpolation = interpolation
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 _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.value:
# 位置空間の場合は，逆運動学を計算
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 _make_folder(self, folder_name):
"""
フォルダーの作成
パラメータ
folder_name(str): 作成したいフォルダー名
"""
# フォルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
os.makedirs(folder_name, exist_ok=True)
def _reset_folder(self, interpolation):
"""
フォルダー内のファイルを全削除
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
"""
# フォルダー名を取得
folder_names = self._get_path_plan_folders(interpolation)
for folder_name in folder_names:
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) # 通常ファイル・シンボリックリンクを削除
def _get_path_plan_folders(self, interpolation):
"""
経路生成結果を保存する複数のフォルダー名を取得
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
戻り値
folder_names(list): 複数のフォルダー名
"""
folder_names = []
folder_name = self._name
# サブフォルダー
sub_folder_names = [self._FOLODER_NAME_PRUNING, self._FOLODER_NAME_SHORTCUT, self._FOLODER_NAME_PARTIAL_SHORTCUT]
for sub_folder_name in sub_folder_names:
folder_names.append(os.path.join(folder_name, interpolation, sub_folder_name))
return folder_names
def _make_path_plan_folder(self, interpolation):
"""
経路生成結果を保存するフォルダー作成
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
"""
# フォルダー名を取得
folder_names = self._get_path_plan_folders(interpolation)
for folder_name in folder_names:
self._make_folder(folder_name)
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)
if self._modification is not None:
# 修正前の始点から終点までの経路を保存
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)
# Shortcut後の経路を保存
self._save_numpy_data_to_txt(self._shortcut_pathes, self._FILE_NAME_SHORTCUT_PATHES)
# Partial Shortcut後の経路を保存
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
# ファイル名にフォルダ名を追加 (各経路生成手法で異なるフォルダにデータを保存)
full_path = f"{self._name}/{self._interpolation}/{file_name}"
np.savetxt(full_path, data)
# ファイル関連メソッド ↑
# 経路生成後の後処理メソッド ↓
def _modify_path(self):
"""
始点から終点までの経路を修正
パラメータ
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
"""
if self._modification is None:
# 経路を修正しない
return
# 修正前の経路を保存
self._before_modify_pathes = np.copy(self._pathes)
if self._modification == MODIFICATION.PRUNING_START:
# 始点からPruning
self._path_pruning_from_start()
elif self._modification == MODIFICATION.PRUNING_END:
# 終点からPruning
self._path_pruning_from_end()
elif self._modification == MODIFICATION.PRUNING_RANDOM:
# ランダム点のPruning
self._path_pruning_random()
elif self._modification == MODIFICATION.SHORTCUT_NORMAL:
# Shortcut
self._path_shortcut()
elif self._modification == MODIFICATION.SHORTCUT_PARTIAL:
# Partial Shortcut
self._path_partial_shortcut()
elif self._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 {self._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):
"""
Partial Shortcut (経路上の2点をランダムで選択して, 2点内の全ノード内の1パラメータを直線補間して, 干渉なければ2点内のノード内の1パラメータを修正)
self._pathesプロパティにPruning後の経路が保存される
"""
# 要素番号を保存
n_success = 0
continueous_fail = 0 # 連続失敗回数
# 位置を構成する要素数を取得
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 # 連続失敗回数
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 # 連続失敗回数
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
# 経由点数分ループする (始点と終点を含まないために，-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
# 経由点数分ループする (始点と終点を含まないために，-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)
# 経路生成後の後処理メソッド ↑
class CostedTree(Tree):
"""
コスト付きツリークラス
プロパティ
_nodes(numpy.ndarray): ノード
_near_node_idx(int): 最短ノード列番号 (_nodesの中に最短ノードも保存している)
_cost_idx(int): コスト列番号 (_nodesの中にコストも保存している)
_radius_idx(int): 探索半径の列番号 (_nodesの中に探索半径も保存している)
メソッド
public
calc_cost(): コストの計算
chg_node_info(): ノード情報を変更
"""
# 定数の定義
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 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*クラス
プロパティ
_name(str): 経路生成手法名
_max_iterate(int): 最大探索回数
_count_success(int): ノード追加の成功回数
_near_node_radius(float): 近傍ノードの探索半径
メソッド
public
プロパティの ゲッター/セッター 関連
max_iterate(): _max_iterateプロパティの ゲッター/セッター
count_success(): _count_successプロパティのゲッター
file_name_current_tree(): _FILE_NAME_CURRENT_TREE定数のゲッター
file_name_random_tree(): _FILE_NAME_RANDOM_TREE定数のゲッター
folder_name_current_tree(): _FOLDER_NAME_CURRENT_TREE定数のゲッター
メイン処理関連
planning(): 経路生成の実装
protected
メイン処理関連
_add_node_start_tree(): 始点ツリーにノードを追加
_calc_cost(): ノードのコストを計算
_get_near_node_list(): ノードと近傍ノードをリストで取得
_get_min_cost_node(): コストが最小となる近傍ノードを取得
_update_near_node(): 近傍ノード内の親ノードを更新
_fin_planning(): 経路生成の終了処理
_get_unit_ball(): 単位球内の位置を取得
ファイル関連
_get_path_plan_folders(): 経路生成結果を保存する複数のフォルダー名を取得
_save_current_tree(): 現在のツリーをファイル化
"""
# 定数の定義
# ファイル名の定義
# 各経路生成手法で絶対に定義するべきファイル名 ↓
# _pathesプロパティを保存するファイル名
_FILE_NAME_PATHES = f"{PATHPLAN.RRT_STAR.value}_pathes.csv"
# _start_treeプロパティを保存するファイル名
_FILE_NAME_START_TREE = f"{PATHPLAN.RRT_STAR.value}_start_tree.csv"
# _start_random_treeプロパティを保存するファイル名
_FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT_STAR.value}_start_random_tree.csv"
# _before_modify_pathesプロパティを保存するファイル名
_FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT_STAR.value}_before_modify_pathes.csv"
# _pruning_pathesプロパティを保存するファイル名
_FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT_STAR.value}_pruning_pathes.csv"
# _shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR.value}_shortcut_pathes.csv"
# _partial_shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR.value}_partial_shortcut_pathes.csv"
# 毎処理保存するファイル名の定義
# Pruning実施毎の経路を保存
_FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT_STAR.value}_pruning_each_pathes"
# Shortcut実施毎の経路を保存
_FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR.value}_shortcut_each_pathes"
# Partial Shortcut実施毎の経路を保存
_FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR.value}_partial_shortcut_each_pathes"
# 各経路生成手法で絶対に定義するべきファイル名 ↑
# フォルダー名の定義
_FOLDER_NAME_CURRENT_TREE = "result" # ノード追加時の現在のツリーを保存するフォルダ名
# 毎処理保存するファイル名の定義
# ノード追加時の現在のツリーを保存するファイル名
_FILE_NAME_CURRENT_TREE = f"{PATHPLAN.RRT_STAR.value}_current_tree"
# ノード追加時の現在のランダムツリーを保存するファイル名
_FILE_NAME_RANDOM_TREE = f"{PATHPLAN.RRT_STAR.value}_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 = 2000 # 最大探索回数
_TIMEOUT_VALUE = 1000 # タイムアウト時間 [second]
_NEAR_NODE_RADIUS_COEF = 20 # 近傍ノードとする探索球の半径の係数
def __init__(self):
"""
コンストラクタ
"""
self._name = PATHPLAN.RRT_STAR.value
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._debug = False
self._max_iterate = self._MAX_ITERATE
self._environment = None
self._robot = None
self._near_node_radius = 0
# プロパティの ゲッター/セッター メソッド ↓
@property
def max_iterate(self):
"""
_max_iterateプロパティのゲッター
"""
return self._max_iterate
@max_iterate.setter
def max_iterate(self, value):
"""
_max_iterateプロパティのセッター
パラメータ
value(int): 設定したい値
"""
# 0以下では何もしない
if value > 0:
self._max_iterate = 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 folder_name_current_tree(self):
"""
_FOLDER_NAME_CURRENT_TREE定数のゲッター
"""
return self._FOLDER_NAME_CURRENT_TREE
# プロパティの ゲッター/セッター メソッド ↑
# メイン処理メソッド ↓
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._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)
# 始点ノードを設定
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 * self._NEAR_NODE_RADIUS_COEF
# 経路生成は一定時間内に終了させる
start_time = time.time()
self._count_success = 0 # ノード追加に成功した回数
# 全探索回数ループ
for _ 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()
# ファイル保存
self._save()
return result
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 _calc_cost(self, pos, near_node):
"""
ノードのコストを計算
パラメータ
pos(numpy.ndarray): 新規ノード位置
near_node(int): 新規ノードとの最短ノード
戻り値
cost(float): コスト
"""
# 最短ノードの情報を保存
node = self._start_tree.nodes[near_node]
node_pos = 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._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._start_tree.nodes.shape[0]
for node_idx in near_node_list:
# 近傍ノードに関する情報を保存
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 + cost
if new_cost < node_cost:
# 小さくなるため，最短ノードを修正
self._start_tree.chg_node_info(
node_idx, new_node_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(end_pos, end_pos, min_cost_node, 0.0, 0.0)
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 _get_path_plan_folders(self, interpolation):
"""
経路生成結果を保存する複数のフォルダー名を取得
パラメータ
interpolation(str): 探索方法 (位置空間/関節空間)
戻り値
folder_names(list): 複数のフォルダー名
"""
folder_names = []
folder_name = self._name
# サブフォルダー
sub_folder_names = [self._FOLODER_NAME_PRUNING, self._FOLODER_NAME_SHORTCUT,
self._FOLODER_NAME_PARTIAL_SHORTCUT, self._FOLDER_NAME_CURRENT_TREE]
for sub_folder_name in sub_folder_names:
folder_names.append(os.path.join(
folder_name, interpolation, sub_folder_name))
return folder_names
def _save_current_tree(self):
"""
現在のツリーをファイル化
"""
if self._debug:
# 新規ノードが追加できたら，self._count_successはインクリメントされていく
# 始点のツリーを保存
file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_TREE}_{self._count_success}.csv")
self._save_numpy_data_to_txt(self._start_tree.nodes, file_name)
# 始点のランダムツリーを保存
file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_RANDOM_TREE}_{self._count_success}.csv")
self._save_numpy_data_to_txt(self._start_random_tree.nodes, file_name)
# ファイル関連メソッド ↑
class RRTStarSmartRobot(RRTStarRobot):
"""
RRT*-Smart (RRT*の改良版 (サンプリングの改良 + 経路の最適化処理を追加))
RRT*ではランダムな値をサンプルしているため，効率的ではない
RRT*-Smartは始点から終点までの経路生成が完了したら確率で利用または探索を実施
制限時は，ビーコンを採用する．
プロパティ
_beacons(list): ビーコンの位置・半径
メソッド
public
プロパティのゲッター関連
beacons(): _beaconsプロパティのゲッター
file_name_beacons(): _FILE_NAME_BEACONS定数のゲッター
メイン処理関連
planning(): 経路生成の実装
protected
メイン処理関連
_get_random_pos(): ランダムな位置を取得
_path_optimization(): 経路の最適化 (親ノードの最適化)
_intelligent_sampling(): Intelligent Samplingによる位置を計算
_set_beacon(): ビーコン情報の設定
準備処理関連
_reset(): データの初期化
ファイル関連
_save_current_tree(): 現在のツリーをファイル化
"""
# 定数の定義
# ファイル名の定義
# 各経路生成手法で絶対に定義するべきファイル名 ↓
# _pathesプロパティを保存するファイル名
_FILE_NAME_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_pathes.csv"
# _start_treeプロパティを保存するファイル名
_FILE_NAME_START_TREE = f"{PATHPLAN.RRT_STAR_SMART.value}_start_tree.csv"
# _start_random_treeプロパティを保存するファイル名
_FILE_NAME_START_RANDOM_TREE = f"{PATHPLAN.RRT_STAR_SMART.value}_start_random_tree.csv"
# _before_modify_pathesプロパティを保存するファイル名
_FILE_NAME_BEFORE_MODIFY_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_before_modify_pathes.csv"
# _pruning_pathesプロパティを保存するファイル名
_FILE_NAME_PRUNING_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_pruning_pathes.csv"
# _shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_shortcut_pathes.csv"
# _partial_shortcut_pathesプロパティを保存するファイル名
_FILE_NAME_PARTIAL_SHORTCUT_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_partial_shortcut_pathes.csv"
# 毎処理保存するファイル名の定義
# Pruning実施毎の経路を保存
_FILE_NAME_PRUING_EACH_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_pruning_each_pathes"
# Shortcut実施毎の経路を保存
_FILE_NAME_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_shortcut_each_pathes"
# Partial Shortcut実施毎の経路を保存
_FILE_NAME_PARTIAL_SHORTCUT_EACH_PATHES = f"{PATHPLAN.RRT_STAR_SMART.value}_partial_shortcut_each_pathes"
# 各経路生成手法で絶対に定義するべきファイル名 ↑
# 毎処理保存するファイル名の定義
# ノード追加時の現在のツリーを保存するファイル名
_FILE_NAME_CURRENT_TREE = f"{PATHPLAN.RRT_STAR_SMART.value}_current_tree"
# ノード追加時の現在のランダムツリーを保存するファイル名
_FILE_NAME_RANDOM_TREE = f"{PATHPLAN.RRT_STAR_SMART.value}_random_tree"
# Intelligent Samplingで使用するビーコンを保存するファイル名
_FILE_NAME_BEACONS = f"{PATHPLAN.RRT_STAR_SMART.value}_beacons"
# ツリーの要素番号を定義
_NODE_NEAR_NODE_IDX = RRT_STAR_SMART_NEAR_NODE_IDX # ノード内の最短ノード要素
_NODE_COST_IDX = RRT_STAR_SMART_COST_IDX # ノード内のコスト要素
_NODE_RADIUS_IDX = RRT_STAR_SMART_RADIUS_IDX # ノード内の半径要素
# その他
_MAX_ITERATE = 2000 # 最大探索回数
_TIMEOUT_VALUE = 1000 # タイムアウト時間 [second]
_MAX_COST = 1e10 # 最大コスト
# RRT*-Smart特有
# Intelligent Samplingに関する定数
_INTERFERENCE_PROB_MAX = 0.8 # 干渉物に近い点を選択する最大確率
_INTERFERENCE_PROB_ITER_COEF = 0.002 # 干渉物に近い点を選択時の探索回数の係数
# 探索の10回に1回の頻度でIntelligent Sampling
_INTELLIGENT_SAMPLING_FREQ = _MAX_ITERATE // 10
# 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._name = PATHPLAN.RRT_STAR_SMART.value
self._beacons = []
# プロパティのゲッターメソッド ↓
@property
def beacons(self):
"""
_beaconsプロパティのゲッター
"""
return self._beacons
@property
def file_name_beacons(self):
"""
_FILE_NAME_BEACONS定数のゲッター
"""
return self._FILE_NAME_BEACONS
# プロパティのゲッターメソッド ↑
# メイン処理メソッド ↓
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._preparation_planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)
# 始点ノードを設定
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_beacon(np.zeros(self._dim).reshape(1, -1), 0.0)
# 近傍ノードを定義する半径
self._near_node_radius = self._moving_value * self._NEAR_NODE_RADIUS_COEF
# 経路生成は一定時間内に終了させる
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):
print(f"first_success_count = {first_success_count}")
# RRT*との差異 ↓
random_pos, beacon_radius, beacon_pos = self._get_random_pos(i, result, first_success_count, end_pos, beacon_nodes, prev_cost)
# RRT*との差異 ↑
# ランダムな値と最短ノードを計算
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_beacon(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
# RRT*との差異 ↓
# 経路の最適化
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
# RRT*との差異 ↑
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()
# ファイル保存
self._save()
return result
def _get_random_pos(self, i, result, first_success_count, target_pos, beacon_nodes, prev_cost):
"""
ランダムな位置を取得
パラメータ
i(int): 探索回数
result(bool): 始点から終点までの経路生成に成功したかどうか
first_succes__count(int): 始点から終点までの経路生成に成功した際の探索回数
target_pos(numpy.ndarray): 目標点
beacon_nodes(list): Intelligent Samplingで使用したいビーコン一覧
prev_cost(float): 1探索前の最短コスト
戻り値
random_pos(numpy.ndarray): ランダムな位置
beacon_radius(float): ビーコンの半径
beacon_pos(numpy.ndarray): ビーコンの位置
"""
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 = super()._get_random_pos(target_pos)
else:
# RRT*と同様でランダムサンプリング
random_pos = super()._get_random_pos(target_pos)
else:
# RRT*と同様でランダムサンプリング
random_pos = super()._get_random_pos(target_pos)
return random_pos, beacon_radius, beacon_pos
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 _set_beacon(self, center, radius):
"""
ビーコン情報の設定
パラメータ
center(numpy.ndarray): 球の中心点
radius(float): 球の半径
"""
beacon = np.append(center.reshape(1, -1), radius).reshape(1, -1)
if len(self._beacons) == 0: # 初回だけ実施
self._beacons = beacon
else:
self._beacons = np.append(self._beacons, beacon, axis=0)
# メイン処理メソッド ↑
# 準備処理メソッド ↓
def _reset(self):
"""
データの初期化
"""
# 親クラスの初期化処理
super()._reset()
# ビーコンの初期化
if len(self._beacons) != 0:
# 何かしらのデータが保存
del self._beacons
self._beacons = []
# 準備処理メソッド ↑
# ファイル関連メソッド ↓
def _save_current_tree(self):
"""
現在のツリーをファイル化
"""
if self._debug:
# 新規ノードが追加できたら，self._count_successはインクリメントされていく
# 始点のツリーを保存
file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_CURRENT_TREE}_{self._count_success}.csv")
self._save_numpy_data_to_txt(self._start_tree.nodes, file_name)
# 始点のランダムツリーを保存
file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_RANDOM_TREE}_{self._count_success}.csv")
self._save_numpy_data_to_txt(self._start_random_tree.nodes, file_name)
# ビーコン情報を保存
file_name = os.path.join(self._FOLDER_NAME_CURRENT_TREE, f"{self._FILE_NAME_BEACONS}_{self._count_success}.csv")
self._save_numpy_data_to_txt(self._beacons, file_name)
# ファイル関連メソッド ↑
class MainRRTRobot:
"""
ロボットの経路生成のメインクラス
プロパティ
_rrt(rrt.py内のクラス): 経路生成手法クラス
メソッド
public
rrt(): _rrtプロパティのゲッター
planning(): 経路生成
protected
_reset(): データの初期化
_get_each_path_plan_instance(): 経路生成手法に応じたクラスのインスタンスを取得
"""
# 定数の定義
def __init__(self):
"""
コンストラクタ
"""
# プロパティの初期化
self._rrt = None
# プロパティのゲッターメソッド ↓
@property
def rrt(self):
"""
_rrtプロパティのゲッター
"""
return self._rrt
# プロパティのゲッターメソッド ↑
def _reset(self):
"""
データの初期化
"""
if self._rrt is not None:
# インスタンス作成済みの場合
del self._rrt
self._rrt = None
def _get_each_path_plan_instance(self, path_plan):
"""
経路生成手法に応じたクラスのインスタンスを取得
パラメータ
path_plan(str): 経路生成手法名
戻り値
rrt: 各経路生成手法のクラス
"""
# 経路生成手法によって，アニメーションの分岐を分ける
if path_plan == PATHPLAN.RRT.value:
# RRT
rrt = RRTRobot()
elif path_plan == PATHPLAN.RRT_STAR.value:
# RRT*
rrt = RRTStarRobot()
elif path_plan == PATHPLAN.RRT_STAR_SMART.value:
# RRT*-Smart
rrt = RRTStarSmartRobot()
else:
# 異常
raise ValueError(f"path_plan is abnormal. path_plan is {path_plan}")
return rrt
def planning(self, path_plan, start_pos, end_pos, environment, robot, interpolation, modification=MODIFICATION.PRUNING_START, debug=False):
"""
経路生成
パラメータ
path_plan(str): 経路生成手法名
start_pos(list): 経路生成の始点
end_pos(list): 経路生成の終点
environment(Base2DEnv): 環境
robot(Robot): ロボットクラス
interpolation(int): 補間方法 (関節空間/位置空間)
modification(str): 経路生成後の修正方法 (Pruning/Shortcutなど)
debug(bool): デバッグフラグ
戻り値
result(bool): True / False = 成功 / 失敗
"""
# データのリセット
self._reset()
# 経路生成手法クラスのインスタンスを取得
self._rrt = self._get_each_path_plan_instance(path_plan)
# 経路生成
result = self._rrt.planning(start_pos, end_pos, environment, robot, interpolation, modification, debug)
return result
経路生成の環境 (environment.py)
本記事で使用する環境に関するクラスを作成した．
Robot3DEnv()クラスを使用する．
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軸方向の値
_PLOT_MIN_X = -0.5 # 描画時のX座標の最小値 (アニメーションを見て，手動で設定した)
_PLOT_MAX_X = 2.0 # 描画時のX座標の最大値 (アニメーションを見て，手動で設定した)
_PLOT_MIN_Y = -1.5 # 描画時のY座標の最小値 (アニメーションを見て，手動で設定した)
_PLOT_MAX_Y = 2.0 # 描画時のY座標の最大値 (アニメーションを見て，手動で設定した)
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.value] = circles
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(objects)
self._manager.setup()
# 最短距離を更新
self._distance = 0.0
@property
def plot_min_pos(self):
"""
描画時の最小値(x, y)を取得
"""
min_pos = np.array([self._PLOT_MIN_X, self._PLOT_MIN_Y])
return min_pos
@property
def plot_max_pos(self):
"""
描画時の最大値(x, y)を取得
"""
max_pos = np.array([self._PLOT_MAX_X, self._PLOT_MAX_Y])
return max_pos
@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.value] = 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.value] = rectangles
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(objects)
self._manager.setup()
# 最短距離を更新
self._distance = 0.0
class Base3DEnv:
"""
経路生成時の3次元環境
プロパティ
_distance(float): 干渉物との最短距離
_inteferences(dist): 干渉物の情報 (名称(球や直方体など) + 形状(半径，中心点など))
メソッド
public
inteferences(): _inteferencesプロパティのゲッター
distance(): _distanceプロパティのゲッター
is_collision(): 干渉判定
is_collision_dist(): 干渉判定 + 最短距離
"""
# 定数の定義
_INITIAL_MARGIN = 0.0 # 干渉物とのマージン [m]
_PLOT_MIN_X = -1 # 描画時のX座標の最小値
_PLOT_MAX_X = 2 # 描画時のX座標の最大値
_PLOT_MIN_Y = -2 # 描画時のY座標の最小値
_PLOT_MAX_Y = 2 # 描画時のY座標の最大値
_PLOT_MIN_Z = -1 # 描画時のZ座標の最小値
_PLOT_MAX_Z = 4 # 描画時のZ座標の最大値
def __init__(self):
"""
コンストラクタ
"""
objects = []
self._interferences = {}
# 球の干渉物を定義 (球の位置 (x, y, z) と半径を定義)
balls = [[0.8, 0.8, 0.8, 0.3], [1.2, 0.8, 0.8, 0.3], [1.2, 1.2, 1.2, 0.3], [0.8, 1.2, 1.2, 0.3]]
for x, y, z, radius in balls:
# 球の半径を設定
ball = fcl.Sphere(radius)
# 球の中心点を平行移動
translation = fcl.Transform(np.array([x, y, z]))
obj = fcl.CollisionObject(ball, translation)
# モデルを追加
objects.append(obj)
# 球の干渉物を保存
self._interferences[INTERFERENCE.BALL.value] = balls
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(objects)
self._manager.setup()
# 最短距離を更新
self._distance = 0.0
@property
def plot_min_pos(self):
"""
描画時の最小値(x, y, z)を取得
"""
min_pos = np.array([self._PLOT_MIN_X, self._PLOT_MIN_Y, self._PLOT_MIN_Z])
return min_pos
@property
def plot_max_pos(self):
"""
描画時の最大値(x, y, z)を取得
"""
max_pos = np.array([self._PLOT_MAX_X, self._PLOT_MAX_Y, self._PLOT_MAX_Z])
return max_pos
@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=_INITIAL_MARGIN):
"""
干渉判定 + 最短距離
パラメータ
obj(fcl.CollisionObject): 干渉物
margin(float): 干渉判定のマージン [m]
戻り値
collision(bool): True / False = 干渉あり / 干渉なし
"""
# 引数確認
if margin < 0:
margin = self._INITIAL_MARGIN
dist_data = fcl.DistanceData()
# 最短距離の結果を保存する
self._manager.distance(obj, dist_data, fcl.defaultDistanceCallback)
min_dist = dist_data.result.min_distance
# 最短距離の更新
self._distance = min_dist
# print(f"min_dist = {min_dist}")
if min_dist > margin:
# 干渉なし
collision = False
else:
# 干渉あり
collision = True
return collision
class Robot3DEnv(Base3DEnv):
"""
経路生成時の3次元環境 (ロボット用)
プロパティ
_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, z) と半径を定義)
balls = [[0.5, 1.5, 1.5, 0.5], [1.5, 0.3, 1.5, 0.3], [1.5, 0.5, 1.0, 0.5]]
for x, y, z, radius in balls:
# fclに円が定義されていないため，球とする
ball = fcl.Sphere(radius)
# 円の中心点が原点であるため，中心点を平行移動させる
translation = fcl.Transform(np.array([x, y, z]))
obj = fcl.CollisionObject(ball, translation)
# モデルを追加
objects.append(obj)
# 円形の干渉物を保存
self._interferences[INTERFERENCE.BALL.value] = balls
# 直方体の干渉物を定義
# 直方体の長さ (x, y, z) と中心点の位置 (x, y, z)，x,y,z軸の回転角度 [deg]
cuboids = [[0.4, 0.3, 0.8, 1.4, -0.6, 1.5, 0, 0, 0], [0.6, 0.8, 1.0, 1.2, 0.7, 2.0, 0, 0, 0]]
for x, y, z, center_x, center_y, center_z, angle_x, angle_y, angle_z in cuboids:
# 直方体の長さ (x, y, z) を設定
cuboid = fcl.Box(x, y, z)
# 回転行列の作成
rotation_x = self._rot.rot(np.deg2rad(angle_x), ROTATION_X_AXIS)
rotation_y = self._rot.rot(np.deg2rad(angle_y), ROTATION_Y_AXIS)
rotation_z = self._rot.rot(np.deg2rad(angle_z), ROTATION_Z_AXIS)
rotation = np.dot(rotation_z, np.dot(rotation_y, rotation_x))
# 長方形の中心点，回転行列を設定
translation = fcl.Transform(rotation, np.array([center_x, center_y, center_z]))
obj = fcl.CollisionObject(cuboid, translation)
# モデルを追加
objects.append(obj)
# 長方形の干渉物を保存
self._interferences[INTERFERENCE.CUBOID.value] = cuboids
# 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:
"""
回転行列クラス
プロパティ
メソッド
public
rot(): 回転軸に応じた回転行列の取得
rot_from_zyx_eluer(): Z-Y-Xオイラー角から回転行列を計算
rot_to_zyx_euler(): 回転行列からZ-Y-Xオイラー角を計算
rot_from_zyz_euler(): Z-Y-Zオイラー角から回転行列を計算
rot_to_zyz_euler(): 回転行列からZ-Y-Zオイラー角を計算
difference_rot_matrix(): 回転行列(rot1から見たrot2)の差分を計算
get_single_axis(): 一軸回転法 (single axis rotation method) による回転角度と回転軸を計算
intermediate_rot(): 初期姿勢と目標姿勢の中間姿勢を計算
protected
_rot_x(): x軸方向にtheta[rad]回転させた時の回転行列
_rot_y(): y軸方向にtheta[rad]回転させた時の回転行列
_rot_z(): z軸方向にtheta[rad]回転させた時の回転行列
_rot_from_single_axis(): 一軸回転法から回転行列を算出
"""
# 定数の定義
_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)
elif axis == ROTATION_X_NEGATIVE_AXIS:
# 回転軸がx軸だが，逆回転
rotation = self._rot_x(-theta)
elif axis == ROTATION_Y_NEGATIVE_AXIS:
# 回転軸がy軸だが，逆回転
rotation = self._rot_y(-theta)
elif axis == ROTATION_Z_NEGATIVE_AXIS:
# 回転軸がz軸だが，逆回転
rotation = self._rot_z(-theta)
else:
# 異常
raise ValueError(f"axis is abnormal. now is {axis}")
return rotation
def rot_from_zyx_euler(self, euler):
"""
Z(theta1)-Y(theta2)-X(theta3)オイラー角から回転行列を計算
パラメータ
euler(numpy.ndarray): Z(theta1)-Y(theta2)-X(theta3)オイラー角 [rad]
戻り値
rotation(numpy.ndarray): 回転行列
"""
# 引数を分解する
theta1 = euler[0]
theta2 = euler[1]
theta3 = euler[2]
# 回転行列を計算
rot_z = self._rot_z(theta1)
rot_y = self._rot_y(theta2)
rot_x = self._rot_x(theta3)
# Z-Y-X軸に順番に回転させた時の回転行列を計算
rotation = np.dot(rot_z, rot_y)
rotation = np.dot(rotation, rot_x)
return rotation
def rot_to_zyx_euler(self, rotation, y_cos_plus=True):
"""
回転行列からZ(theta1)-Y(theta2)-X(theta3)オイラー角を計算
パラメータ
rotation(numpy.ndarray): 回転行列
y_cos_plus(bool): Y(theta2)を+にするか
戻り値
euler(numpy.ndarray): Z(theta1)-Y(theta2)-X(theta3)オイラー角 [rad] の順番にデータを保存
"""
# 引数の確認
if rotation.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"rotation's shape is abnormal. rotaton'shape is {rotation.shape}")
# 回転行列の要素を一度，ローカル変数に保存
# r11 = cos(theta1) * cos(theta2)
# r12 = -sin(theta1) * cos(theta3) + cos(theta1) * sin(theta2) * sin(theta3)
# r13 = sin(theta1) * sin(theta3) + cos(theta1) * sin(theta2) * cos(theta3)
r11, r12, r13 = rotation[0, :]
# r21 = sin(theta1) * cos(theta2)
# r22 = cos(theta1) * cos(theta3) + sin(theta1) * sin(theta2) * sin(theta3)
# r23 = -cos(theta1) * sin(theta3) + sin(theta1) * sin(theta2) * cos(theta3)
r21, r22, r23 = rotation[1, :]
# r31 = -sin(theta2)
# r32 = cos(theta2) * sin(theta3)
# r33 = cos(theta2) * cos(theta3)
r31, r32, r33 = rotation[2, :]
# はじめにtheta2から求める
# r32 ** 2 + r33 * 2 = cos^2(theta2)
# r31 = -sin(theta2)
# theta2 = np.atan2(sin(theta2), cos(theta2)) = np.atan2(-r31, root(r32 ** 2 + r33 ** 2))
theta2_sin = -r31
if y_cos_plus:
theta2_cos = np.sqrt(r32 ** 2 + r33 ** 2)
else:
theta2_cos = -np.sqrt(r32 ** 2 + r33 ** 2)
theta2 = np.arctan2(theta2_sin, theta2_cos)
if abs(theta2 - np.pi / 2) <= self._PITCH_THRESHOLD:
# theta2がpi/2付近では特異点となる
# cos(theta2) = 0でsin(theta2) = 1となる
# r13 = sin(theta1) * sin(theta3) + cos(theta1) * sin(theta2) * cos(theta3) = sin(theta1) * sin(theta3) + cos(theta1) * cos(theta3) = cos(theta1 - theta3)
# r23 = -cos(theta1) * sin(theta3) + sin(theta1) * sin(theta2) * cos(theta3) = -cos(theta1) * sin(theta3) + sin(theta1) * cos(theta3) = sin(theta1 - theta3)
# theta1 - theta3 = np.atan2(sin(theta1 - theta3), cos(theta1 - theta3)) = np.atan2(r23, r13)
# theta1 = 0とすると theta3 = -np.atan2(r23, r13)となる
theta1 = 0.0
theta3 = -np.arctan2(r23, r13)
elif abs(theta2 + np.pi / 2) <= self._PITCH_THRESHOLD:
# theta2が-pi/2付近では特異点となる
# cos(theta2) = 0でsin(theta2) = -1となる
# r13 = sin(theta1) * sin(theta3) + cos(theta1) * sin(theta2) * cos(theta3) = sin(theta1) * sin(theta3) - cos(theta1) * cos(theta3) = -cos(theta1 + theta3)
# r23 = -cos(theta1) * sin(theta3) + sin(theta1) * sin(theta2) * cos(theta3) = -cos(theta1) * sin(theta3) - sin(theta1) * cos(theta3) = -sin(theta1 + theta3)
# theta1 + theta3 = np.atan2(sin(theta1 + theta3), cos(theta1 + theta3)) = np.atan2(-r23, -r13)
# theta1 = 0とすると theta3 = np.atan2(-r23, -r13)となる
theta1 = 0.0
theta3 = np.arctan2(-r23, -r13)
else:
# 特異点付近ではない場合
# r21 = sin(theta1) * cos(theta2)
# r11 = cos(theta1) * cos(theta2)
# theta1 = np.atan2(sin(theta1), cos(theta1)) = np.atan2(r21/cos(theta2), r11/cos(theta2)) = np.atan2(r21, r11)
theta1 = np.arctan2(r21, r11)
# r32 = cos(theta2) * sin(theta3)
# r33 = cos(theta2) * cos(theta3)
# theta3 = np.atan2(sin(theta3), cos(theta3)) = np.atan2(r32/cos(theta2), r33/cos(theta2)) = np.atan2(r32, r33)
theta3 = np.arctan2(r32, r33)
# Z(theta1)-Y(theta2)-X(theta3)オイラー角の順番に保存
rpy = np.array([theta1, theta2, theta3])
return rpy
def rot_from_zyz_euler(self, euler):
"""
Z(theta1)-Y(theta2)-Z(theta3)オイラー角による回転行列の計算
パラメータ
euler(numpy.ndarray): Z(theta1)-Y(theta2)-Z(theta3)オイラー角 [rad]
戻り値
rotation(numpy.ndarray): 回転行列
"""
# 引数を分解する
theta1 = euler[0]
theta2 = euler[1]
theta3 = euler[2]
# 回転行列を計算
rot_z1 = self._rot_z(theta1)
rot_y = self._rot_y(theta2)
rot_z2 = self._rot_z(theta3)
# Z-Y-X軸に順番に回転させた時の回転行列を計算
rotation = np.dot(rot_z1, rot_y)
rotation = np.dot(rotation, rot_z2)
return rotation
def rot_to_zyz_euler(self, rotation, y_plus=True):
"""
回転行列からZ(theta1)-Y(theta2)-Z(theta3)オイラー角を取得
パラメータ
rotation(numpy.ndarray): 回転行列
y_plus(bool): Y(theta2)を+にするか
戻り値
euler(numpy.ndarray): Z(theta1)-Y(theta2)-Z(theta3) [rad] の順番にデータを保存
"""
# 引数の確認
if rotation.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"rotation's shape is abnormal. rotaton'shape is {rotation.shape}")
# 回転行列の要素を一度，ローカル変数に保存
# r11 = c1 * c2 * c3 - s1 * s3
# r12 = -c1 * c2 * s3 - s1 * c3
# r13 = c1 * s2
r11, r12, r13 = rotation[0, :]
# r21 = s1 * c2 * c3 + c1 * s3
# r22 = -s1 * c2 * s3 + c1 * c3
# r23 = s1 * s2
r21, r22, r23 = rotation[1, :]
# r31 = -s2 * c3
# r32 = s2 * s3
# r33 = c2
r31, r32, r33 = rotation[2, :]
# Y(theta2)の値から求める
# r33 = c2よりtheta2 = arccos(r33)
# cos()は-1 ~ 1の範囲のため，制限しておく
y_cos = r33
if y_plus:
y_sin = np.sqrt(r31 ** 2 + r32 ** 2)
else:
y_sin = -np.sqrt(r31 ** 2 + r32 ** 2)
# r33 = np.clip(r33, self._ROT_MIN_VALUE, self._ROT_MAX_VALUE)
# arccos()には2通りの解が存在する cos(theta) = cos(-theta) だから
# theta2 = np.arccos(r33)
# if not y_plus:
# # 角度を負にする
# theta2 *= -1
theta2 = np.arctan2(y_sin, y_cos)
if abs(r33 - 1) <= self._ZERO_NEAR:
# cos(theta2)が1近傍の時は，特異点
# r11 = c1 * c2 * c3 - s1 * s3 で c2 = 1 より，r11 = c1 * c3 - s1 * s3 = c13
# r21 = s1 * c2 * c3 + c1 * s3 で c2 = 1 より，r21 = s1 * c3 + c1 * s3 = s13
# よって，theta1 + theta3 = np.atan2(r21, r11) で，theta1 = 0 とする
theta1 = 0.0
theta3 = np.arctan2(r21, r11)
elif abs(r33 + 1) <= self._ZERO_NEAR:
# cos(theta2)が-1近傍の時も特異点
# r11 = c1 * c2 * c3 - s1 * s3 で c2 = -1 より，r11 = -c1 * c3 - s1 * s3 = -c(1-3)
# r21 = s1 * c2 * c3 + c1 * s3 で c2 = -1 より，r21 = -s1 * c3 + c1 * s3 = -s(1-3)
# よって，theta1 - theta3 = np.atan2(-r21, -r11) で，theta1 = 0 とする
theta1 = 0.0
theta3 = -np.arctan2(-r21, -r11)
else:
# 特異点以外の場合
# r32 = s2 * s3よりs3 = r32 / s2
# r31 = -s2 * c3よりc3 = r31 / -s2
# よって，theta3 = np.atan2(r32, -r31)
theta3 = np.arctan2(r32 / y_sin, -r31 / y_sin)
# r23 = s1 * s2よりs1 = r23 / s2
# r13 = c1 * s2よりc1 = r13 / s2
# よって，theta1 = np.atan2(r23, r13)
theta1 = np.arctan2(r23 / y_sin, r13 / y_sin)
# Z(theta1)-Y(theta2)-Z(theta3)の順番に保存
euler = np.array([theta1, theta2, theta3])
return euler
def difference_rot_matrix(self, rot1, rot2):
"""
回転行列(rot1から見たrot2)の差分を計算
パラメータ
rot1(numpy.ndarray): 回転行列1 (3 * 3行列)
rot2(numpy.ndarray): 回転行列2 (3 * 3行列)
戻り値
diff_rot(numpy.ndarray): 回転行列の差分
"""
# 引数の確認
if rot1.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"rot1.shape is not (3, 3). rot1.shape is {rot1.shape}")
if rot2.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"rot2.shape is not (3, 3). rot2.shape is {rot2.shape}")
# rot1から見た4ot2の回転行列を計算
diff_rot = np.dot(rot1.T, rot2)
return diff_rot
def get_single_axis(self, init_rot, target_rot, t):
"""
一軸回転法 (single axis rotation method) による回転角度と回転軸を計算
パラメータ
init_rot(numpy.ndarray): 初期姿勢
target_rot(numpy.ndarray): 目標姿勢
t(float): 目標姿勢への割合 (0 ~ 1)
戻り値
sigle_axis(numpy.ndarray): 一軸回転(回転角度(スカラー)，回転軸(3ベクトル))
"""
single_axis = np.zeros(4)
# 引数の範囲確認
if t < 0 or t > 1:
raise ValueError(f"t is abnormal. t's range is 0 to 1. t is {t}")
# 2姿勢の差分を計算
diff_rot = np.dot(init_rot.T, target_rot)
# 回転角度のcos()を計算
# cos(theta) = (trace(diff_rot) - 1) / 2
# cos(theta)の値を最小値が-1，最大値が+1となるようにnp.clip()を採用
cos_rot_angle = np.clip((np.trace(diff_rot) - 1) / 2, self._ROT_MIN_VALUE, self._ROT_MAX_VALUE)
# 回転軸関連のデータを計算
# u = [r32 - r23, r13 - r31, r21 - r12] = 2 * sin(theta) * lambda
# 両辺の大きさをとり，|lambda| = 1の性質を使用し，sin(theta)について解くと
# sin(theta) = 1 / 2 * |u|
u = np.array([diff_rot[2, 1] - diff_rot[1, 2], diff_rot[0, 2] - diff_rot[2, 0], diff_rot[1, 0] - diff_rot[0, 1]])
sin_rot_angle = np.linalg.norm(u) / 2
# sin()，cos()からthetaを算出
rot_angle = np.arctan2(sin_rot_angle, cos_rot_angle)
# rot_angle = np.arccos(cos_rot_angle)
# 回転が0 or pi付近では，何もしない
# print(f"rot_angle = {rot_angle}")
if np.isclose(rot_angle, 0.0, atol=1e-3) or np.isclose(abs(rot_angle), np.pi, atol=1e-3):
rot_angle = 0
rot_axis = np.array([1, 0, 0])
else:
# 回転軸を算出する
# lambda = u / (2 * sin(theta))
rot_axis = u / (2 * np.sin(rot_angle) + self._EPSILON)
# rot_axis = rot_angle /
# 正規化する
rot_axis = rot_axis / (np.linalg.norm(rot_axis) + self._EPSILON)
single_axis[0] = rot_angle * t
single_axis[1:] = rot_axis
return single_axis
def _rot_from_single_axis(self, single_axis):
"""
一軸回転法から回転行列を算出
パラメータ
single_axis(numpy.ndarray): 一軸回転法で算出した一軸(回転角度(スカラー)と回転軸(3ベクトル)
戻り値
rotation(numpy.ndarray): 回転行列
"""
# パラメータを分解する
theta = single_axis[0]
lambda_x = single_axis[1]
lambda_y = single_axis[2]
lambda_z = single_axis[3]
# 三角関数を算出する
sin = np.sin(theta)
cos = np.cos(theta)
one_minus_cos = 1 - cos
# r11 = (1 - cos(theta)) * lambda_x ** 2 + cos(theta)
# r12 = (1 - cos(theta)) * lambda_x * lambda_y - lambda_z * sin(theta)
# r13 = (1 - cos(theta)) * lambda_z * lambda_x + lambda_y * sin(theta)
# r21 = (1 - cos(theta)) * lambda_x * lambda_y + lambda_z * sin(theta)
# r22 = (1 - cos(theta)) * lambda_y ** 2 + cos(theta)
# r23 = (1 - cos(theta)) * lambda_y * lambda_z - lambda_x * sin(theta)
# r31 = (1 - cos(theta)) * lambda_z * lambda_x - lambda_y * sin(theta)
# r32 = (1 - cos(theta)) * lambda_y * lambda_z + lambda_x * sin(theta)
# r33 = (1 - cos(theta)) * lambda_z ** 2 + cos(theta)
r11 = one_minus_cos * lambda_x ** 2 + cos
r12 = one_minus_cos * lambda_x * lambda_y - lambda_z * sin
r13 = one_minus_cos * lambda_z * lambda_x + lambda_y * sin
r21 = one_minus_cos * lambda_x * lambda_y + lambda_z * sin
r22 = one_minus_cos * lambda_y ** 2 + cos
r23 = one_minus_cos * lambda_y * lambda_z - lambda_x * sin
r31 = one_minus_cos * lambda_z * lambda_x - lambda_y * sin
r32 = one_minus_cos * lambda_y * lambda_z + lambda_x * sin
r33 = one_minus_cos * lambda_z ** 2 + cos
rotation = np.array([[r11, r12, r13],
[r21, r22, r23],
[r31, r32, r33]])
return rotation
def intermediate_rot(self, init_rot, target_rot, t):
"""
初期姿勢と目標姿勢の中間姿勢を計算
パラメータ
init_rot(numpy.ndarray): 初期姿勢
target_rot(numpy.ndarray): 目標姿勢
t(float): 目標姿勢への割合 (0 ~ 1)
戻り値
rotation(numpy.ndarray): 中間姿勢
"""
# 引数の範囲確認
if init_rot.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"init_rot' shape is abnormal. init_rot.shape is {init_rot.shape}")
if target_rot.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"target_rot' shape is abnormal. target_rot.shape is {target_rot.shape}")
if t < 0 or t > 1:
raise ValueError(f"t is abnormal. t's range is 0 to 1. t is {t}")
# 1軸回転法により，回転角度と回転軸を算出
single_axis = self.get_single_axis(init_rot, target_rot, t)
# 回転角度と回転軸から，回転行列に変換
sub_rotation = self._rot_from_single_axis(single_axis)
# 目標姿勢 = 初期姿勢 * 差分姿勢
rotation = np.dot(init_rot, sub_rotation)
return rotation
6軸ロボットアーム (robot.py)
6軸ロボットアームの処理を説明する．
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)))
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()
class Robot3DoF(Robot):
"""
3軸ロボットクラス
プロパティ
_links(numpy.ndarray): ロボットのリンク長
_rot(Rotation): 回転行列クラス
_axiss(list): 関節の回転軸
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
update(): 角度を与えて，各リンクの直方体を更新する
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_3D # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_3D # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_3D # リンク数
_THETA1_XY_THRESHOLD = 1e-4 # theta1算出時のx, y閾値
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([0, 0, self._links[0]])
self._relative_positions[2] = np.array([self._links[1], 0, 0 ])
self._relative_positions[3] = np.array([self._links[2], 0, 0 ])
# リンク1は回転軸がz軸，リンク2とリンク3は回転軸がy軸である
self._axiss = [ROTATION_Z_AXIS, ROTATION_Y_NEGATIVE_AXIS, ROTATION_Y_NEGATIVE_AXIS]
# 初期角度
initial_thetas = np.zeros(self._DIMENTION_THETA)
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2，リンク3)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(initial_thetas)
# 1つ前のリンクの回転行列を更新
prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)
# 各リンクの角度が全部0の時のx, y, zを定義
box_xyz = [[self._BOX_WIDTH * 2, self._BOX_WIDTH * 2, self._links[0] ],
[self._links[1], self._BOX_WIDTH * 2, self._BOX_WIDTH * 2],
[self._links[2], self._BOX_WIDTH * 2, self._BOX_WIDTH * 2]]
# ロボットの各リンクを直方体として定義する
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 = (all_link_pose[i + 1] - all_link_pose[i]) / 2 + all_link_pose[i]
# 直方体の定義 (x, y, zの長さを保存)
x, y, z = box_xyz[i]
box = fcl.Box(x, y, z)
# 直方体の中心を定義 (位置・姿勢)
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()
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)}")
# 同時変換行列(3 * 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)
# 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)}")
# 現在の手先位置を計算
current_pos = self.forward_kinematics(current_thetas)
# 位置の差分を計算
dP = target_pos - current_pos
# dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
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.1, np.linalg.norm(dP))
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]
l3 = self._links[2]
# 三角関数の計算
sin1 = np.sin(thetas[0])
cos1 = np.cos(thetas[0])
sin2 = np.sin(thetas[1])
cos2 = np.cos(thetas[1])
sin23 = np.sin(thetas[1] + thetas[2])
cos23 = np.cos(thetas[1] + thetas[2])
# ヤコビ行列
jacovian = np.array([[-sin1 * (l2 * cos2 + l3 * cos23), -cos1 * (l2 * sin2 + l3 * sin23), -cos1 * l3 * sin23],
[ cos1 * (l2 * cos2 + l3 * cos23), -sin1 * (l2 * sin2 + l3 * sin23), -sin1 * l3 * sin23],
[ 0 , l2 * cos2 + l3 * cos23 , l3 * cos23]])
return jacovian
def inverse_kinematics(self, pose, upper=False, front=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
upper(bool): 腕が上向かどうか
front(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)}")
# はじめにtheta3を算出する
# c3 = {(px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)} / (2 * l2 * l3)
px = pose[0]
py = pose[1]
pz = pose[2]
l1 = self._links[0]
l2 = self._links[1]
l3 = self._links[2]
cos3 = ((px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)) / (2 * l2 * l3)
# cosの範囲は-1以上1以下である
if abs(cos3) > 1:
# 異常
raise ValueError(f"cos3 is abnormal. cos3 is {cos3}")
# sinも求めて，theta3をatan2(sin2, cos2)より算出する
sin3 = np.sqrt(1 - cos3 ** 2)
theta3 = np.arctan2(sin3, cos3)
if not upper:
# 下向きの角度のため，三角関数も更新
theta3 = -theta3
sin3 = np.sin(theta3)
cos3 = np.cos(theta3)
# 次にtheta1を算出する
# theta1 = atan2(py, px)
# py, pxが0近傍なら失敗
if abs(py) <= self._THETA1_XY_THRESHOLD and abs(px) <= self._THETA1_XY_THRESHOLD:
# 近傍のため，エラー
raise ValueError(f"abs(py) and abs(px) is abnormal. py is {py}, px is {px}")
if front:
# 位置(pose)に対して正面向き
theta1 = np.arctan2( py, px)
else:
# 位置(pose)に対して後ろ向き
theta1 = np.arctan2(-py, -px)
# 最後にtheta2を算出する
# 行列計算
# [c2, s2] = [[l2 + l3 * c3, -l3 * s3], [l3 * s3, l2 + l3 * c3]] ** -1 * [root(px ** 2 + py ** 2), pz - l1]
element1 = l2 + l3 * cos3
element2 = -l3 * sin3
matrix = np.array([[ element1, element2],
[-element2, element1]])
# 行列式を計算
det = np.linalg.det(matrix)
# 0近傍の確認
if abs(det) <= self._DETERMINANT_THRESHOLD:
# 0近傍 (異常)
raise ValueError(f"det is abnormal. det is {det}")
# 位置を保存 ([root(px ** 2 + py ** 2), pz - l1])
position = np.array([np.sqrt(px ** 2 + py ** 2), pz - l1])
# [c2, s2]の計算
cos2_sin2 = np.dot(np.linalg.inv(matrix), position)
# theta2をatan2()より算出する
theta2 = np.arctan2(cos2_sin2[1], cos2_sin2[0])
thetas = np.array([theta1, theta2, theta3])
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)}")
# 同時変換行列(3 * 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，リンク3)の位置を計算
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 = (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()
class Robot6DoF(Robot):
"""
6軸ロボットクラス
プロパティ
_links(numpy.ndarray): ロボットのリンク長
_rot(Rotation): 回転行列クラス
_axiss(list): 関節の回転軸
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
update(): 角度を与えて，各リンクの直方体を更新する
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_6D # 手先位置・姿勢の次元数
_DIMENTION_THETA = DIMENTION_6D # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_6D # リンク数
_DETERMINANT_THRESHOLD = 1e-4 # 行列式の閾値
_THETA1_XY_THRESHOLD = 1e-4 # theta1算出時のx, y閾値
_BOX_WIDTH = 1e-2 # 各リンクの幅を定義
def __init__(self, links):
"""
コンストラクタ
パラメータ
links(numpy.ndarray): ロボットのリンク長 [m]
"""
# 親クラスの初期化
super().__init__(links)
# ロボットの各リンクを直方体として定義する
self._objects = []
# (全リンクの角度を0とした時の) 各リンク間の相対位置
self._relative_positions = np.zeros((self._DIMENTION_LINK + 1, 3))
self._relative_positions[0] = np.array([0, 0, 0])
self._relative_positions[1] = np.array([0, 0, self._links[0]])
self._relative_positions[2] = np.array([0, 0, self._links[1]])
self._relative_positions[3] = np.array([0, 0, self._links[2]])
self._relative_positions[4] = np.array([0, 0, self._links[3]])
self._relative_positions[5] = np.array([0, 0, self._links[4]])
self._relative_positions[6] = np.array([0, 0, self._links[5]])
# リンク1，リンク4，リンク6は回転軸がz軸，リンク2，リンク3，リンク5は回転軸がy軸である
self._axiss = [ROTATION_Z_AXIS, ROTATION_Y_AXIS, ROTATION_Y_AXIS, ROTATION_Z_AXIS, ROTATION_Y_AXIS, ROTATION_Z_AXIS]
# 初期角度
initial_thetas = np.zeros(self._DIMENTION_THETA)
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2，リンク3，リンク4，リンク5，リンク6)の位置を計算
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 = (all_link_pose[i + 1, :DIMENTION_3D] - all_link_pose[i, :DIMENTION_3D]) / 2 + all_link_pose[i, :DIMENTION_3D]
# 直方体の定義 (x, y, zの長さを保存)
x, y, z = [self._BOX_WIDTH * 2, self._BOX_WIDTH * 2, self._links[i]]
box = fcl.Box(x, y, z)
# 直方体の中心を定義 (位置・姿勢)
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()
def forward_kinematics(self, thetas):
"""
順運動学 (ロボットの関節角度からロボットの手先位置を算出)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
pose(numpy.ndarray): ロボットの手先位置・姿勢(Z-Y-Xオイラー角) [m] / [rad]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 同時変換行列(6 * 4 * 4)を計算する
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# 最終リンクの同時変換行列(最終リンク座標の位置・姿勢)より，手先位置を計算する
final_link_matrix = homogeneou_matrix[-1]
# 最終リンクから手先位置までの相対位置(4ベクトル)を定義
relative_pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
relative_pos[:self._HOMOGENEOU_MAT_ELEMENT - 1] = self._relative_positions[-1]
# 手先位置(x, y, z)・姿勢(theta_z, theta_y, theta_x)を保存する
pose = np.zeros(self._DIMENTION_POSE)
# 手先位置(x, y, z)を保存
pose[:DIMENTION_3D] = np.dot(final_link_matrix, relative_pos)[:DIMENTION_3D]
# 最終リンクの回転行列からZ-Y-Xオイラー角を計算
zyx_euler = self._rot.rot_to_zyx_euler(final_link_matrix[:self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT])
# 手先位置姿勢を保存(手先姿勢は最終リンクと同じ姿勢と仮定する)
pose[DIMENTION_3D:] = zyx_euler
return pose
def inverse_kinematics(self, pose, right=False, front=True, above=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置([m])・姿勢([rad])
right(bool): 腕が右向きかどうか
front(bool): 正面かどうか
above(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)}")
# 引数から位置・姿勢を取得
pos = pose[:DIMENTION_3D]
zyx_euler = pose[DIMENTION_3D:]
thetas = np.zeros(self._DIMENTION_THETA)
# 関節1, 2, 3の角度を計算
thetas[:DIMENTION_3D] = self._inverse_kinematics_123(pos, zyx_euler, right=right, front=front)
# 関節4, 5, 6の角度を計算
thetas[DIMENTION_3D:] = self._inverse_kinematics_456(zyx_euler, thetas[:DIMENTION_3D], above=above)
return thetas
def _inverse_kinematics_123(self, pos, zyx_euler, right=False, front=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出) で関節1, 2, 3を計算
パラメータ
pos(numpy.ndarray): ロボットの手先位置 [m]
zyx_euler(numpy.ndarray): ロボットの手先姿勢(Z-Y-Xオイラー角) [rad]
right(bool): 肘が右向きどうか
front(bool): 正面かどうか
戻り値
theta123(numpy.ndarray): ロボットの関節角度(関節1, 2, 3) [rad]
"""
# リンクをローカル変数に保存
l1 = self._links[0]
l2 = self._links[1]
l34 = self._links[2] + self._links[3]
l56 = self._links[4] + self._links[5]
# Z-Y-Xオイラー角から回転行列を計算
rotation = self._rot.rot_from_zyx_euler(zyx_euler)
# 回転行列のz方向を取得
rotation_z = rotation[:, 2]
# 関節5までの絶対位置を算出
pos5 = pos - rotation_z * l56
px = pos5[0]
py = pos5[1]
pz = pos5[2]
# theta1 ~ theta3までの関節角度を算出する
# はじめにtheta3を算出する
cos3 = ((px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l34 ** 2)) / (2 * l2 * l34)
# cos()は-1以上かつ1以下の範囲である
if abs(cos3) > 1:
# 異常
raise ValueError(f"cos3 is abnormal. cos3 is {cos3}")
# sinを算出して，theta3を計算する
if right:
# 右向き
sin3 = np.sqrt(1 - cos3 ** 2)
else:
# 左向き
sin3 = -np.sqrt(1 - cos3 ** 2)
theta3 = np.arctan2(sin3, cos3)
# 次にtheta1を求める
# theta1 = atan2(py, px)
if abs(px) <= self._THETA1_XY_THRESHOLD and abs(py) <= self._THETA1_XY_THRESHOLD:
# 近傍のため，theta1=0固定とする
theta1 = 0.0
else:
if front:
# 正面向き
theta1 = np.arctan2( py, px)
else:
# 背面向き
theta1 = np.arctan2(-py, -px)
# 最後にtheta2を算出する
# [s2, c2] = [[l2 + l34 * c3, l34 * s3], [-l34 * s3, l2 + l34 * c3]]^(-1) * [root(px ** 2 + py ** 2), pz - l1]
element1 = l2 + l34 * cos3
element2 = l34 * sin3
matrix = np.array([[ element1, element2],
[-element2, element1]])
# 行列式を計算
det = np.linalg.det(matrix)
# 0近傍の確認
if abs(det) <= self._DETERMINANT_THRESHOLD:
# 0近傍 (異常)
raise ValueError(f"det is abnormal. det is {det}")
# 位置を保存 ([root(px ** 2 + py ** 2), pz - l1])
position = np.array([np.sqrt(px ** 2 + py ** 2), pz - l1])
# [s2, c2]の計算
sin2_cos2 = np.dot(np.linalg.inv(matrix), position)
# theta2をatan2()より算出する
theta2 = np.arctan2(sin2_cos2[0], sin2_cos2[1])
# 関節1, 2, 3の順番に保存
theta123 = np.array([theta1, theta2, theta3])
return theta123
def _inverse_kinematics_456(self, zyx_euler, theta123, above=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出) で関節4, 5, 6を計算
パラメータ
zyx_euler(numpy.ndarray): ロボットの手先姿勢(Z-Y-Xオイラー角) [rad]
theta123(numpy.ndarray): 関節1, 2, 3の角度 [rad]
above(bool): 手首が上かどうか
戻り値
theta456(numpy.ndarray): ロボットの関節角度(関節4, 5, 6) [rad]
"""
thetas = np.zeros(self._DIMENTION_THETA)
thetas[:3] = theta123
# wR6 = wR3 * 3R6
# 3R6 = wR3.T * wR6
# ロボットの手先姿勢(Z-Y-Xオイラー角)から回転行列を算出
rot_we = self._rot.rot_from_zyx_euler(zyx_euler)
# 同時変換行列(6 * 4 * 4)を計算する (関節4, 5, 6は0で適当な値を入れる)
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# ワールド座標系から見た関節3の回転行列を計算
rot_w3 = homogeneou_matrix[2, :3, :3]
# 関節3から見た関節6の回転行列を計算する
rot_36 = np.dot(rot_w3.T, rot_we)
# Z(関節4)-Y(関節5)-Z(関節6)オイラー角を算出する
theta456 = self._rot.rot_to_zyz_euler(rot_36, above)
return theta456
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)}")
# 同時変換行列(6 * 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):
# 同時変換行列から位置を取得
all_link_pose[i, :DIMENTION_3D] = matrix[:self._ROTATION_MAT_ELEMENT, self._ROTATION_MAT_ELEMENT].reshape(1, -1)
# 同時変換行列から回転行列を取得して，Z-Y-Xオイラー角に変換する
zyx_euler = self._rot.rot_to_zyx_euler(matrix[:self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT])
all_link_pose[i, DIMENTION_3D:] = zyx_euler
# 最後のリンクの座標
# 最後のリンクの座標系の原点から，手先の位置を計算する
pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
pos[:DIMENTION_3D] = self._relative_positions[-1]
all_link_pose[-1, :DIMENTION_3D] = np.dot(homogeneou_matix[-1], pos)[:DIMENTION_3D].reshape(1, -1)
# 手先姿勢は最終リンクの姿勢と一致していると考え，最終リンクの回転行列をZ-Y-Xオイラー角に変換する
zyx_euler = self._rot.rot_to_zyx_euler(homogeneou_matix[-1, :self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT])
all_link_pose[-1, DIMENTION_3D:] = zyx_euler
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，リンク3)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(thetas)
# ロボットの各リンクを直方体として定義する
for i in range(self._DIMENTION_THETA):
# 各リンクの回転行列を定義
rotation = self._rot.rot_from_zyx_euler(all_link_pose[i, DIMENTION_3D:])
# 各リンクの中心位置 (x, y, z) を定義
center = (all_link_pose[i + 1, :DIMENTION_3D] - all_link_pose[i, :DIMENTION_3D]) / 2 + all_link_pose[i, :DIMENTION_3D]
# 直方体の中心を定義 (位置・姿勢)
translation = fcl.Transform(rotation, center)
# モデルの位置を更新
self._objects[i].setTransform(translation)
# 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 mpl_toolkits.mplot3d.art3d import Poly3DCollection # 3次元形状の描画
import logging # ログ用
import os # フォルダ作成用
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import MyRotation # 回転行列
LOG_FLG = True # ログ出力フラグ
logging.basicConfig(level=logging.INFO) # デバッグ情報をログ出力できるように修正
class RRTAnimation:
"""
RRTのアニメーション作成
プロパティ
_figure: 描画枠
_axis: 描画内容
_path_plan: アニメーションできる経路生成手法名
_rot: 回転行列
_log_flg: ログ出力フラグ
メソッド
public
plot(): グラフの描画
plot_Animation(): アニメーションの描画
protected
ログ出力関連
_log(): ログ出力
設定関連
_reset_2d(): 2次元データの描画内容をリセット
_set_2d_axis(): 2次元データのラベルなどを設定
_reset_3d(): 3次元データの描画内容をリセット
_set_3d_axis(): 3次元データのラベルなどを設定
環境関連
_plot_environment(): 環境の描画
_plot_circle(): 円の描画
_plot_rectangle(): 長方形の描画
_plot_ball(): 球の描画
_plot_cuboid(): 直方体の描画
_plot_2d_ellipse(): 2次元の楕円をプロット
_plot_3d_ellipse(): 3次元の楕円をプロット
ロボット関連
_plot_robot(): ロボットの描画 (グラフ用)
_plot_robot_animation(): ロボットの描画 (アニメーション用)
_plot_robot_1data(): ロボットを1データ分描画
_convert_pos_to_theta(): データを位置空間から関節空間に変換
_convert_theta_to_pos(): データを関節空間から位置空間に変換
_convert_multi_theta_to_pos(): 複数データを位置空間から関節空間に変換
_convert_multi_pos_to_theta(): 複数データを位置空間から関節空間に変換
グラフ描画関連
_get_file_name(): ファイル名の取得
_plot_2d(): グラフの描画 (2次元)
_plot_3d(): グラフの描画 (3次元)
_plot_all_node_parent(): ツリー内の全ノードを親ノードと一緒に描画
_plot_start_end_node(): 始点ノードと終点ノードをプロット
_plot_2d_tree(): ツリーの描画 (2次元)
_plot_3d_tree(): ツリーの描画 (3次元)
_plot_2d_pathes(): 始点から終点までの最終的な経路をプロット (2次元)
_plot_3d_pathes(): 始点から終点までの最終的な経路をプロット (3次元)
アニメーション描画関連
_plot_2d_animation(): アニメーションの描画 (2次元)
_plot_3d_animation(): アニメーションの描画 (3次元)
_update_2d_data(): 1フレーム分のデータを描画 (2次元)
_update_3d_data(): 1フレーム分のデータを描画 (2次元)
_plot_2d_path_animation(): 始点から終点までの経路を描画 (2次元アニメーション用)
_plot_3d_path_animation(): 始点から終点までの経路を描画 (3次元アニメーション用)
_plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
_plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
"""
# 定数の定義
_NEAR_NODE_IDX = RRT_NEAR_NODE_IDX # ノード内の最短ノード要素
# 干渉物に関する定数
_INTERFERENCE_COLOR = "gray" # 干渉物の色
_INTERFERENCE_ALPHA = 0.5 # 干渉物の色の濃さ (0 ~ 1)
_RADIUS_TO_DIAMETER = 2 # 半径から直径への変換
# 描画時の色
_START_POS_COLOR = "cyan" # 始点の色
_END_POS_COLOR = "lime" # 終点の色
# ロボットに関する定数
_ROBOT_LINK_COLOR = "black" # ロボットのリンクに関する色
_ROBOT_JOINT_COLOR = "black" # ロボットの関節に関する色
# ファイルの拡張子に関する定数
_FILE_NAME_GRAPH_EXTENTION = "png" # グラフファイルの拡張子
_FILE_NAME_ANIME_EXTENSION = "gif" # アニメーションファイルの拡張子
# アニメーション
_ANIMATION_MARGIN = 5 # アニメーションのマージン
_ANIMATION_INTERBAL = 500 # アニメーションのインターバル [msec]
_ANIMATION_OVER_PERIOD_TIME = 20 # 全時刻をアニメーション化すると容量が大きくなるため，一定時刻ごとにアニメーション化するための一定時刻を定義
def __init__(self):
"""
コンストラクタ
"""
# プロパティの初期化
self._path_plan = PATHPLAN.RRT.value
self._rot = MyRotation()
self._log_flg = LOG_FLG
# ログ出力関連のメソッド ↓
def _log(self, data):
"""
ログ出力
パラメータ
data(): ログ出力したいデータ
"""
if self._log_flg:
# ログ出力する
logging.info(data)
else:
# ログ出力しない
pass
# ログ出力関連のメソッド ↑
# 設定関連のメソッド ↓
def _reset_2d(self):
"""
2次元データの描画内容をリセット
"""
self._figure = plt.Figure()
self._axis = self._figure.add_subplot(111)
# X/Y軸に文字を記載
self._axis.set_xlabel("X [m]")
self._axis.set_ylabel("Y [m]")
def _set_2d_axis(self, min_pos, max_pos):
"""
2次元データのラベルなどを設定
パラメータ
min_pos(numpy.ndarray): 描画範囲の最小値 (x, y)
max_pos(numpy.ndarray): 描画範囲の最大値 (x, y)
"""
# X/Y軸に文字を記載
self._axis.set_xlabel("X [m]")
self._axis.set_ylabel("Y [m]")
# X/Y軸の最小値・最大値を設定
self._axis.set_xlim(min_pos[0], max_pos[0])
self._axis.set_ylim(min_pos[1], max_pos[1])
self._axis.grid()
self._axis.set_aspect("equal")
def _reset_3d(self):
"""
3次元データの描画内容をリセット
"""
self._figure = plt.figure()
self._axis = self._figure.add_subplot(111, projection="3d")
# 0 ~ 2piまでの範囲とする
theta_1_0 = np.linspace(0, np.pi * 2, 20)
theta_2_0 = np.linspace(0, np.pi * 2, 20)
theta_1, theta_2 = np.meshgrid(theta_1_0, theta_2_0)
# x, y, zの曲座標表示 (中心点が原点である半径1の球)
self._x = np.cos(theta_2) * np.sin(theta_1)
self._y = np.sin(theta_2) * np.sin(theta_1)
self._z = np.cos(theta_1)
def _set_3d_axis(self, min_pos, max_pos):
"""
3次元データのラベルなどを設定
パラメータ
min_pos(numpy.ndarray): 描画範囲の最小値 (x, y, z)
max_pos(numpy.ndarray): 描画範囲の最大値 (x, y, z)
"""
# X/Y/Z軸に文字を記載
self._axis.set_xlabel("X [m]")
self._axis.set_ylabel("Y [m]")
self._axis.set_zlabel("Z [m]")
# X/Y/Z軸の最小値・最大値を設定
self._axis.set_xlim(min_pos[0], max_pos[0])
self._axis.set_ylim(min_pos[1], max_pos[1])
self._axis.set_zlim(min_pos[2], max_pos[2])
self._axis.grid()
self._axis.set_aspect("equal")
# 設定関連のメソッド ↑
# 環境関連のメソッド ↓
def _plot_environment(self, environment):
"""
環境の描画
パラメータ
environment(envirionment.py内のクラス): 環境
"""
if environment is None:
# 環境がないため，何もしない
return
for name, datas in environment.interferences.items():
# 干渉物の名称
if name == INTERFERENCE.CIRCLE.value:
# 円
for x, y, radius in datas:
self._plot_circle(x, y, radius)
elif name == INTERFERENCE.RECTANGLE.value:
# 長方形
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)
elif name == INTERFERENCE.BALL.value:
# 球
for x, y, z, radius in datas:
self._plot_ball(np.array([x, y, z]), radius)
elif name == INTERFERENCE.CUBOID.value:
# 直方体
for x, y, z, center_x, center_y, center_z, angle_x, angle_y, angle_z in datas:
# 回転行列の作成
rotation_x = self._rot.rot(np.deg2rad(angle_x), ROTATION_X_AXIS)
rotation_y = self._rot.rot(np.deg2rad(angle_y), ROTATION_Y_AXIS)
rotation_z = self._rot.rot(np.deg2rad(angle_z), ROTATION_Z_AXIS)
rotation = np.dot(rotation_z, np.dot(rotation_y, rotation_x))
# 描画
self._plot_cuboid(np.array([center_x, center_y, center_z]), x, y, z)
else:
# 何もしない
pass
def _plot_circle(self, x, y, radius, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA, fill=True):
"""
円の描画
パラメータ
x(float): 中心点 (x)
y(float): 中心点 (y)
radius(float): 半径
fill(bool): 円の内部を塗りつぶすかどうか
color(str): 色
alpha(float): 色の濃さ (0 ~ 1)
"""
# 半径が0以下では描画しない
if radius <= 0:
return
circle = patches.Circle((x, y), radius, color=color, alpha=alpha, fill=fill)
self._axis.add_patch(circle)
def _plot_rectangle(self, x, y, width, height, angle, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
"""
長方形の描画
パラメータ
x(float): 左下隅のx座標
y(float): 左下隅のy座標
width(float): 幅
height(float): 高さ
angle(float): 角度 [deg]
color(str): 色
alpha(float): 色の濃さ (0 ~ 1)
"""
# 幅と高さが0以下では描画しない
if width <= 0 or height <= 0:
return
rect = patches.Rectangle((x, y), width, height, angle=angle, color=color, alpha=alpha)
self._axis.add_patch(rect)
def _plot_ball(self, center, radius, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
"""
球の描画
パラメータ
center(np.ndarray): 中心位置 (x, y, z)
radius(float): 半径
color(str): 色
alpha(float): 色の濃さ (0 ~ 1)
"""
# 半径が0以下では描画しない
if radius <= 0:
return
self._axis.plot_wireframe(self._x * radius + center[0], self._y * radius + center[1], self._z * radius + center[2], color=color, alpha=alpha)
def _plot_cuboid(self, center, x, y, z, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
"""
直方体の描画
パラメータ
center(numpy.ndarray): 中心位置 (x, y, z)
x(float): 直方体のx軸の長さ
y(float): 直方体のy軸の長さ
z(float): 直方体のz軸の長さ
rotation(numpy.ndarray): 回転行列
"""
# 直方体の長さが0以下では描画しない
if x <= 0 or y <= 0 or z <= 0:
return
# 直方体の頂点を算出する
points = np.array([[center[0] - x / 2, center[1] - y / 2, center[2] - z / 2],
[center[0] + x / 2, center[1] - y / 2, center[2] - z / 2],
[center[0] + x / 2, center[1] + y / 2, center[2] - z / 2],
[center[0] - x / 2, center[1] + y / 2, center[2] - z / 2],
[center[0] - x / 2, center[1] - y / 2, center[2] + z / 2],
[center[0] + x / 2, center[1] - y / 2, center[2] + z / 2],
[center[0] + x / 2, center[1] + y / 2, center[2] + z / 2],
[center[0] - x / 2, center[1] + y / 2, center[2] + z / 2]])
# 頂点の4点から面を算出する
verts = [[points[0], points[1], points[2], points[3]],
[points[4], points[5], points[6], points[7]],
[points[0], points[1], points[5], points[4]],
[points[2], points[3], points[7], points[6]],
[points[1], points[2], points[6], points[5]],
[points[4], points[7], points[3], points[0]]]
# 直方体の描画
self._axis.add_collection3d(Poly3DCollection(verts, facecolors=color, edgecolors=color, alpha=alpha))
def _plot_2d_ellipse(self, center, radius, theta, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA, fill=True):
"""
2次元の楕円をプロット
パラメータ
center(numpy.ndarray): 楕円の中心点
radius(numpy.ndarray): 楕円の半径
theta(float): 楕円の回転角度 [rad]
color(str): 色
alpha(float): 色の濃さ (0.0 ~ 1.0)
"""
# 半径が0以下では描画しない
if radius.any() <= 0:
return
# 楕円の中身は塗りつぶさない (描画時に半径から直径にすると整合性が取れる)
ellipse = patches.Ellipse(center, radius[0] * self._RADIUS_TO_DIAMETER, radius[1] * self._RADIUS_TO_DIAMETER, theta, color=color, alpha=alpha, fill=fill)
self._axis.add_patch(ellipse)
def _plot_3d_ellipse(self, center, radius, rotation, color=_INTERFERENCE_COLOR, alpha=_INTERFERENCE_ALPHA):
"""
3次元の楕円をプロット
パラメータ
center(numpy.ndarray): 楕円の中心点 (x, y, z)
radius(numpy.ndarray): 楕円の半径 (x, y, z)
rotation(numpy.ndarray): 楕円の回転行列 (3 * 3行列)
color(str): 色
alpha(float): 色の濃さ (0.0 ~ 1.0)
"""
# 半径が0以下では描画しない
if radius.any() <= 0:
return
x = self._x * radius[0]
y = self._y * radius[1]
z = self._z * radius[2]
# 座標をベクトル形式に変換
# shape: (3, N)
points = np.stack([x.flatten(), y.flatten(), z.flatten()], axis=0)
rotated_points = np.dot(rotation, points)
# 元の形に戻す（メッシュグリッドと同じ形状）
x_rot = rotated_points[0, :].reshape(x.shape)
y_rot = rotated_points[1, :].reshape(y.shape)
z_rot = rotated_points[2, :].reshape(z.shape)
self._axis.plot_wireframe(x_rot + center[0], y_rot + center[1], z_rot + center[2], color=color, alpha=alpha)
# 環境関連のメソッド ↑
# ロボット関連のメソッド ↓
def _plot_robot(self, rrt):
"""
ロボットの描画 (グラフ用)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# パラメータrrtから，ロボットクラスをローカル変数に保存
robot = rrt.robot
if robot is None:
# 処理終了
return
# 始点から終点までの経路を位置空間から関節空間に変換
pathes = self._convert_multi_pos_to_theta(robot, rrt.pathes, rrt.interpolation)
# 始点から終点までの経路を全部プロット
for path in pathes:
# ロボットのプロット
self._plot_robot_1data(robot, path, rrt.dim)
def _plot_robot_animation(self, i, rrt):
"""
ロボットの描画 (アニメーション用)
パラメータ
i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# パラメータrrtから，ロボットクラスをローカル変数に保存
robot = rrt.robot
if robot is None:
# 処理終了
return
if i < 0:
# 異常
return
if rrt.debug:
# デバッグありのため，始点から終点に関係しないノードも使用
if self._ANIMATION_OVER_PERIOD_TIME > 1:
# アニメーションの容量削減
n_tree_nodes = rrt.start_tree.nodes.shape[0] // self._ANIMATION_OVER_PERIOD_TIME
now_node = i * self._ANIMATION_OVER_PERIOD_TIME
else:
n_tree_nodes = rrt.start_tree.nodes.shape[0]
now_node = i
if i < n_tree_nodes:
# ツリー内のノードをプロット
theta = self._convert_pos_to_theta(robot, rrt.start_tree.nodes[now_node, :self._NEAR_NODE_IDX], rrt.interpolation)
else:
# 始点から終点までの経由点を使用
n_waypoint = i - n_tree_nodes
if n_waypoint < rrt.pathes.shape[0]:
# n_waypoint番目の経由点をプロット
theta = self._convert_pos_to_theta(robot, rrt.pathes[n_waypoint], rrt.interpolation)
else:
# 終点をプロット
theta = self._convert_pos_to_theta(robot, rrt.pathes[-1], rrt.interpolation)
else:
# デバッグなしのため，始点から終点までの経由点を使用
if i < rrt.pathes.shape[0]:
# i番目の経由点をプロット
theta = self._convert_pos_to_theta(robot, rrt.pathes[i], rrt.interpolation)
else:
# 終点をプロット
theta = self._convert_pos_to_theta(robot, rrt.pathes[-1], rrt.interpolation)
# ロボットのプロット
self._plot_robot_1data(robot, theta, rrt.dim)
def _plot_robot_1data(self, robot, theta, dim, color="blue", alpha=0.2):
"""
ロボットを1データ分描画
パラメータ
robot(robot.py内のクラス): ロボットクラス
theta(numpy.ndarray): 1点分のデータ
"""
# 関節角度から，全リンク位置を計算
all_link_pos = robot.forward_kinematics_all_link_pos(theta)
if dim == DIMENTION_2D: # 2次元
# 線をプロット
self._axis.plot( all_link_pos[:, 0], all_link_pos[:, 1], color=color, alpha=alpha)
# 点をプロット
self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], color=color, alpha=alpha)
else: # 3次元
# 線をプロット
self._axis.plot( all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], color=color, alpha=alpha)
# 点をプロット
self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], color=color, alpha=alpha)
def _convert_pos_to_theta(self, robot, path, interpolation):
"""
データを位置空間から関節空間に変換
パラメータ
robot(robot.py内のクラス): ロボットクラス
path(numpy.ndarray): 経路上の経由点
interpolation(int): 補間方法
戻り値
theta(numpy.ndarray): 関節
"""
if robot is None:
# ロボットクラスが存在しないため，そのままpathを返す
theta = np.copy(path)
else:
if interpolation == INTERPOLATION.POSITION.value:
# 位置空間のため，逆運動学より関節空間に変換
theta = robot.inverse_kinematics(path)
else:
# 関節空間のため，成形せずそのままpathを返す
theta = np.copy(path)
return theta
def _convert_theta_to_pos(self, robot, path, interpolation):
"""
データを関節空間から位置空間に変換
パラメータ
robot(robot.py内のクラス): ロボットクラス
path(numpy.ndarray): 経路上の経由点
interpolation(int): 補間方法
戻り値
pos(numpy.ndarray): 位置
"""
if robot is None:
# ロボットクラスが存在しないため，そのままpathを返す
pos = np.copy(path)
else:
if interpolation == INTERPOLATION.JOINT.value:
# 関節空間のため，順運動学より位置空間に変換
pos = robot.forward_kinematics(path)
else:
# 位置空間のため，成形せずそのままpathを返す
pos = np.copy(path)
return pos
def _convert_multi_theta_to_pos(self, robot, pathes, interpolation):
"""
複数データを関節空間から位置空間に変換
パラメータ
robot(robot.py内のクラス): ロボットクラス
pathes(numpy.ndarray): 変換したい複数データ
interpolation(int): 補間手法 (位置空間/関節空間)
戻り値
pos_pathes(numpy.ndarray): 位置空間
"""
if robot is None:
# ロボットが存在しないため，変換せずそのまま返す
pos_pathes = np.copy(pathes)
else:
if interpolation == INTERPOLATION.JOINT.value:
# リスト内包表記より，複数データを位置空間へ変換
pos_pathes = [self._convert_theta_to_pos(robot, path, interpolation) for path in pathes]
# リスト型からnumpy型に型変換
pos_pathes = np.array(pos_pathes)
else:
# 位置空間であるから，変換せずそのまま返す
pos_pathes = np.copy(pathes)
return pos_pathes
def _convert_multi_pos_to_theta(self, robot, pathes, interpolation):
"""
複数データを位置空間から関節空間に変換
パラメータ
robot(robot.py内のクラス): ロボットクラス
pathes(numpy.ndarray): 変換したい複数データ
interpolation(int): 補間手法 (位置空間/関節空間)
戻り値
joint_pathes(numpy.ndarray): 関節空間
"""
if robot is None:
# ロボットが存在しないため，変換せずそのまま返す
joint_pathes = np.copy(pathes)
else:
if interpolation == INTERPOLATION.POSITION.value:
# リスト内包表記より，複数データを関節空間へ変換
joint_pathes = [self._convert_pos_to_theta(robot, path, interpolation) for path in pathes]
# リスト型からnumpy型に型変換
joint_pathes = np.array(joint_pathes)
else:
# 関節空間であるから，変換せずそのまま返す
joint_pathes = np.copy(pathes)
return joint_pathes
# ロボット関連のメソッド ↑
# グラフ描画関連のメソッド ↓
def plot(self, rrt):
"""
グラフの描画
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# 念の為，経路生成手法が正しいかの確認
if rrt.name != self._path_plan:
# 想定外の経路生成手法であるため，異常
raise ValueError(f"rrt.name is not {self._path_plan}. rrt.name is {rrt.name}")
if rrt.dim == DIMENTION_2D:
# 2次元データ
self._plot_2d(rrt)
elif rrt.dim == DIMENTION_3D or rrt.dim == DIMENTION_6D:
# 3次元データ
self._plot_3d(rrt)
else:
# 異常
raise ValueError(f"rrt.dim is abnormal. rrt.dim is {rrt.dim}")
def _get_file_name(self, rrt, animation=True):
"""
ファイル名の取得
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
animation(bool): True / False = アニメーション / グラフ
戻り値
file_name(str): ファイル名
"""
# ファイル名を作成するための要素をローカル変数に保存
if rrt.dim == DIMENTION_2D:
# 2次元空間の探索
dim = "2d"
elif rrt.dim == DIMENTION_3D:
# 3次元空間の探索
dim = "3d"
else:
# 6次元空間の探索
dim = "6d"
if animation:
# アニメーションの描画
extension = self._FILE_NAME_ANIME_EXTENSION
plot = PLOT_ANIMATION
else:
# グラフの描画
extension = self._FILE_NAME_GRAPH_EXTENTION
plot = PLOT_GRAPH
# 上記の要素を使用して，ファイル名を作成
if rrt.debug:
# デバッグあり
file_name = f"{self._path_plan}/{self._path_plan}_{dim}_{rrt.interpolation}_debug_{plot}.{extension}"
else:
# デバッグなし
file_name = f"{self._path_plan}/{self._path_plan}_{dim}_{rrt.interpolation}_{plot}.{extension}"
self._log(f"file_name = {file_name}")
return file_name
def _plot_2d(self, rrt):
"""
グラフの描画 (2次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# データをリセットする
self._reset_2d()
self._set_2d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)
# 環境をプロット
self._plot_environment(rrt.environment)
# ロボットをプロット
self._plot_robot(rrt)
# 全ノードを親ノード(最短ノード)と一緒にプロット
if rrt.debug:
# デバッグ中であれば，全ノード + 最終的な経路をプロット
self._plot_2d_tree(rrt)
self._plot_2d_pathes(rrt)
else:
# デバッグ中でなければ，最終的な経路だけをプロットする
self._plot_2d_pathes(rrt)
# ファイル名の取得
file_name = self._get_file_name(rrt, animation=False)
# ファイル保存
self._figure.savefig(file_name)
# 描画
plt.show()
def _plot_3d(self, rrt):
"""
グラフの描画 (3次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# データをリセットする
self._reset_3d()
self._set_3d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)
# 環境をプロット
self._plot_environment(rrt.environment)
# ロボットをプロット
self._plot_robot(rrt)
# 全ノードを親ノード(最短ノード)と一緒にプロット
if rrt.debug:
# デバッグ中であれば，全ノード + 最終的な経路をプロット
self._plot_3d_tree(rrt)
self._plot_3d_pathes(rrt)
else:
# デバッグ中でなければ，最終的な経路だけをプロットする
self._plot_3d_pathes(rrt)
# ファイル名の取得
file_name = self._get_file_name(rrt, animation=False)
# ファイル保存
self._figure.savefig(file_name)
# 描画
plt.show()
def _plot_all_node_parent(self, tree_pos, tree_parent, robot, interpolation, dim, color="red", alpha=0.5):
"""
ツリー内の全ノードを親ノードと一緒に描画
パラメータ
tree_pos(numpy.ndarray): ツリー内のノード
tree_parent(numpy.ndarray): ツリー内の親ノード
robot(robot.py内のクラス): ロボットクラス
interpolation(int): 補間方法 (位置空間/関節空間)
dim(int): 次元数
color(str): 色
alpha(float): 色の濃さ (0 ~ 1)
"""
# パラメータの確認
if tree_pos.shape[0] != tree_parent.shape[0]:
# データが異常
raise ValueError(f"tree_pos.shape[0] and tree_parent.shape[0] are not match. tree_pos.shape[0] is {tree_pos.shape[0]}. tree_parent.shape[0] is {tree_parent.shape[0]}")
for i in reversed(range(1, tree_pos.shape[0])):
# 追加の遅いノードから順番にプロットしていく
now_position = tree_pos[i]
near_node_idx = int(tree_parent[i])
while True: # 始点をプロットするまでループ
parent_position = tree_pos[near_node_idx]
plot_pos = np.append(now_position.reshape(1, -1), parent_position.reshape(1, -1), axis=0)
# 描画データを関節空間から位置空間へ変換する
plot_pos = self._convert_multi_theta_to_pos(robot, plot_pos, interpolation)
# 親ノードとの線をプロット
if dim == DIMENTION_2D:
# 2次元データ (plot()メソッドで値は確認済み)
self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], color=color, alpha=alpha)
else:
# 3次元データ
# 6軸ロボットアームは (位置, 姿勢) = (x, y, z, z_euler, y_euler, x_euler) の順番に保存されている
self._axis.plot(plot_pos[:, 0], plot_pos[:, 1], plot_pos[:, 2], color=color, alpha=alpha)
# ノードおよび親ノードの更新
now_position = parent_position
near_node_idx = int(tree_parent[near_node_idx])
if near_node_idx == INITIAL_NODE_NEAR_NODE:
# 始点には，親ノードが存在しないから，while処理を抜ける
break
def _plot_start_end_node(self, rrt):
"""
始点ノードと終点ノードをプロット
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# 始点から終点までの最終的な経路をローカル変数に保存
pathes = rrt.pathes
# 始点/終点を関節空間から位置空間に変換
start_pos = self._convert_theta_to_pos(rrt.robot, pathes[0] , rrt.interpolation)
end_pos = self._convert_theta_to_pos(rrt.robot, pathes[-1], rrt.interpolation)
# 始点/終点をプロット
if rrt.dim == DIMENTION_2D:
# 2次元データ
self._axis.scatter(start_pos[0], start_pos[1], color=self._START_POS_COLOR, marker="*")
self._axis.scatter( end_pos[0], end_pos[1], color=self._END_POS_COLOR, marker="*")
else:
# 3次元データ
# 6軸ロボットアームは (位置, 姿勢) = (x, y, z, z_euler, y_euler, x_euler) の順番に保存されている
self._axis.scatter(start_pos[0], start_pos[1], start_pos[2], color=self._START_POS_COLOR, marker="*")
self._axis.scatter( end_pos[0], end_pos[1], end_pos[2], color=self._END_POS_COLOR, marker="*")
def _plot_2d_tree(self, rrt):
"""
ツリーの描画 (2次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes # 始点ツリー
# ツリーから位置と親ノードを取得
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# 全ノードを親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
def _plot_3d_tree(self, rrt):
"""
ツリーの描画 (3次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes # 始点ツリー
# ツリーから位置と親ノードを取得
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# 全ノードを親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
def _plot_2d_pathes(self, rrt, color="green", alpha=0.7):
"""
始点から終点までの最終的な経路をプロット (2次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# 始点から終点までの経路を位置空間で取得
pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)
self._axis.plot(pathes[:, 0], pathes[:, 1], color=color, alpha=alpha)
def _plot_3d_pathes(self, rrt, color="green", alpha=0.7):
"""
始点から終点までの最終的な経路をプロット (3次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# 始点から終点までの経路を位置空間で取得
pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)
# 6軸ロボットアームは (位置, 姿勢) = (x, y, z, z_euler, y_euler, x_euler) の順番に保存されている
self._axis.plot(pathes[:, 0], pathes[:, 1], pathes[:, 2], color=color, alpha=alpha)
# グラフ描画関連のメソッド ↑
# アニメーション描画関連のメソッド ↓
def plot_Animation(self, rrt):
"""
アニメーションの描画
パラメータ
rrt(rrt.py内のクラス): 経路生成手法
"""
# 念の為，経路生成手法が正しいかの確認
if rrt.name != self._path_plan:
# 想定外の経路生成手法であるため，異常
raise ValueError(f"rrt.name is not {self._path_plan}. rrt.name is {rrt.name}")
if rrt.dim == DIMENTION_2D:
# 2次元データ
self._plot_2d_animation(rrt)
elif rrt.dim == DIMENTION_3D or rrt.dim == DIMENTION_6D:
# 3次元データ
self._plot_3d_animation(rrt)
else:
# 異常
raise ValueError(f"rrt.dim is abnormal. rrt.dim is {rrt.dim}")
def _plot_2d_animation(self, rrt):
"""
アニメーションの描画 (2次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# データをリセット
self._reset_2d()
# アニメーションのフレーム数
if rrt.debug:
# デバッグあり (フレーム数 = ツリーのノード数 + 始点から終点までの経由点数 + マージン)
n_frame = rrt.start_tree.nodes.shape[0] + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
else:
# デバッグなし (フレーム数 = 始点から終点までの経由点数 + マージン)
n_frame = rrt.pathes.shape[0] + self._ANIMATION_MARGIN
self._log(f"n_frame = {n_frame}")
# アニメーション作成
animation = ani.FuncAnimation(self._figure, self._update_2d_data, fargs=(rrt, ), interval=self._ANIMATION_INTERBAL, frames=n_frame)
# ファイル名の取得
file_name = self._get_file_name(rrt, animation=True)
# アニメーションの保存
animation.save(file_name, writer="imagemagick")
plt.show()
def _plot_3d_animation(self, rrt):
"""
アニメーションの描画 (3次元)
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# データをリセット
self._reset_3d()
# アニメーションのフレーム数
if rrt.debug:
# デバッグあり (フレーム数 = ツリーのノード数 + 始点から終点までの経由点数 + マージン)
if self._ANIMATION_OVER_PERIOD_TIME > 1:
# アニメーションの容量削減処理
n_frame = rrt.start_tree.nodes.shape[0] // self._ANIMATION_OVER_PERIOD_TIME + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
else:
n_frame = rrt.start_tree.nodes.shape[0] + rrt.pathes.shape[0] + self._ANIMATION_MARGIN
else:
# デバッグなし (フレーム数 = 始点から終点までの経由点数)
n_frame = rrt.pathes.shape[0] + self._ANIMATION_MARGIN
self._log(f"n_frame = {n_frame}")
# アニメーション作成
animation = ani.FuncAnimation(self._figure, self._update_3d_data, fargs=(rrt, ), interval=self._ANIMATION_INTERBAL, frames=n_frame)
# ファイル名の取得
file_name = self._get_file_name(rrt, animation=True)
# アニメーションの保存
animation.save(file_name, writer="imagemagick")
plt.show()
def _update_2d_data(self, i, rrt):
"""
1フレーム分のデータを描画 (2次元)
パラメータ
i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# 以前のプロットをクリアする
self._axis.clear()
self._set_2d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)
# 環境のプロット
self._plot_environment(rrt.environment)
# ロボットのプロット
self._plot_robot_animation(i, rrt)
if rrt.debug:
# デバッグありのため，親ノードも全プロット
self._plot_2d_tree_animation(i, rrt)
# 始点から終点までの経由点数を計算
n_waypoint = i - rrt.start_tree.nodes.shape[0]
self._plot_2d_path_animation(n_waypoint, rrt)
else:
# デバッグなしのため，始点から終点までの経路をプロット
self._plot_2d_path_animation(i, rrt)
# 始点と終点をプロット
self._plot_start_end_node(rrt)
def _update_3d_data(self, i, rrt):
"""
1フレーム分のデータを描画 (3次元)
パラメータ
i(int): フレーム番号 (ツリーのノード数 + 始点から終点までの経由点数)
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
# 以前のプロットをクリアする
self._axis.clear()
self._set_3d_axis(rrt.environment.plot_min_pos, rrt.environment.plot_max_pos)
# 環境のプロット
self._plot_environment(rrt.environment)
# ロボットのプロット
self._plot_robot_animation(i, rrt)
if rrt.debug:
# 始点から終点までの経由点数を計算
if self._ANIMATION_OVER_PERIOD_TIME > 1:
# アニメーションの容量削減
now_idx = i * self._ANIMATION_OVER_PERIOD_TIME
n_waypoint = now_idx - rrt.start_tree.nodes.shape[0]
else:
now_idx = i
n_waypoint = i - rrt.start_tree.nodes.shape[0]
# デバッグありのため，親ノードも全プロット
self._plot_3d_tree_animation(now_idx, rrt)
self._plot_3d_path_animation(n_waypoint, rrt)
else:
# デバッグなしのため，始点から終点までの経路をプロット
self._plot_3d_path_animation(i, rrt)
# 始点と終点をプロット
self._plot_start_end_node(rrt)
def _plot_2d_path_animation(self, i, rrt, color="green", alpha=0.7):
"""
始点から終点までの経路を描画 (2次元アニメーション用)
パラメータ
i(int): プロットしたい経路番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
if i < rrt.pathes.shape[0]:
# iまでの経由点を全部，位置空間で取得
if i == 0:
pathes = self._convert_theta_to_pos(rrt.robot, rrt.pathes[0], rrt.interpolation)
# 1次元データだから，2次元データに変換
pathes = pathes.reshape(1, -1)
else:
pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes[:i + 1], rrt.interpolation)
else:
# 始点から終点までの全経由点を位置空間で取得
pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)
# プロット
self._axis.plot(pathes[:i + 1, 0], pathes[:i + 1, 1], color=color, alpha=alpha)
def _plot_3d_path_animation(self, i, rrt, color="green", alpha=0.7):
"""
始点から終点までの経路を描画 (3次元アニメーション用)
パラメータ
i(int): プロットしたい経路番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
if i < rrt.pathes.shape[0]:
# iまでの経由点を全部，位置空間で取得
if i == 0:
pathes = self._convert_theta_to_pos(rrt.robot, rrt.pathes[0], rrt.interpolation)
# 1次元データだから，2次元データに変換
pathes = pathes.reshape(1, -1)
else:
pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes[:i + 1], rrt.interpolation)
else:
# 始点から終点までの全経由点を位置空間で取得
pathes = self._convert_multi_theta_to_pos(rrt.robot, rrt.pathes, rrt.interpolation)
# プロット
# 6軸ロボットアームは (位置, 姿勢) = (x, y, z, z_euler, y_euler, x_euler) の順番に保存されている
self._axis.plot(pathes[:i + 1, 0], pathes[:i + 1, 1], pathes[:i + 1, 2], color=color, alpha=alpha)
def _plot_2d_tree_animation(self, i, rrt):
"""
ツリーの描画 (2次元アニメーション用)
パラメータ
i(int): プロットしたい経由点番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes # 始点ツリー
# ツリーから位置と親ノードを取得
if i < start_tree_nodes.shape[0]:
# i番目の経由点までを全取得
start_tree_pos = start_tree_nodes[:i + 1, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:i + 1, self._NEAR_NODE_IDX]
else:
# 経由点を全取得
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# 親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
def _plot_3d_tree_animation(self, i, rrt):
"""
ツリーの描画 (3次元アニメーション用)
パラメータ
i(int): プロットしたい経由点番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes # 始点ツリー
# ツリーから位置と親ノードを取得
if i < start_tree_nodes.shape[0]:
# i番目の経由点までを全取得
start_tree_pos = start_tree_nodes[:i + 1, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:i + 1, self._NEAR_NODE_IDX]
else:
# 経由点を全取得
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# 親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
# アニメーション描画関連のメソッド ↑
class RRTStarAnimation(RRTAnimation):
"""
RRT*のアニメーション作成
プロパティ
_path_plan(str): 経路生成手法名
メソッド
protected
アニメーション描画関連
_plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
_plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
"""
# 定数の定義
_NEAR_NODE_IDX = RRT_STAR_NEAR_NODE_IDX
_RADIUS_NODE_IDX = RRT_STAR_RADIUS_IDX
_EXPLOTION_CIRCLE_COLOR = "blue"
_EXPLOTION_CIRCLE_ALPHA = 0.2
_PLOT_OVER_PERIOD_TIME = 10 # 全時刻をアニメーション化すると容量が大きくなるため，一定時刻ごとにアニメーション化するための一定時刻を定義
def __init__(self):
"""
コンストラクタ
"""
# 親クラスの初期化処理
super().__init__()
# 経路生成手法名をRRT*とする
self._path_plan = PATHPLAN.RRT_STAR.value
# アニメーション描画関連のメソッド ↓
def _plot_2d_tree_animation(self, i, rrt):
"""
ツリーの描画 (2次元アニメーション用)
パラメータ
i(int): プロットしたい経由点番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
# iとRRT*の成功回数を比較
if i == 0:
# 始点だけを取得
start_tree_nodes = rrt.start_tree.nodes
start_tree_pos = start_tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
start_tree_parent = start_tree_nodes[0, self._NEAR_NODE_IDX].reshape(1, -1)
elif i < rrt.count_success:
# i回成功目のツリーをファイルから読み込む
# RRT*はノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
# ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
file_name = f"{rrt.file_name_current_tree}_{i}.csv"
full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
start_tree_nodes = np.loadtxt(full_path)
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
else:
# 最終的な経由点を全部取得
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# 親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
def _plot_3d_tree_animation(self, i, rrt):
"""
ツリーの描画 (3次元アニメーション用)
パラメータ
i(int): プロットしたい経由点番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
print(f"i = {i}")
if i < 0:
# 異常値のため，処理終了
return
# iとRRT*の成功回数を比較
if i == 0:
# 始点だけをプロット
start_tree_nodes = rrt.start_tree.nodes
start_tree_pos = start_tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
start_tree_parent = start_tree_nodes[0, self._NEAR_NODE_IDX].reshape(1, -1)
elif i <= rrt.count_success:
# i回成功目のツリーをファイルから読み込む
# RRT*はノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
# ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
file_name = f"{rrt.file_name_current_tree}_{i}.csv"
full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
start_tree_nodes = np.loadtxt(full_path)
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
else:
# 最終的な経由点を全部取得
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# 親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
# アニメーション描画関連のメソッド ↑
class RRTStarSmartAnimation(RRTStarAnimation):
"""
RRT*-Smartのアニメーション描画
プロパティ
_path_plan(str): 経路生成手法名
メソッド
protected
アニメーション描画関連
_plot_2d_tree_animation(): ツリーの描画 (2次元アニメーション用)
_plot_3d_tree_animation(): ツリーの描画 (3次元アニメーション用)
"""
# 定数の定義
_NEAR_NODE_IDX = RRT_STAR_SMART_NEAR_NODE_IDX
_RADIUS_NODE_IDX = RRT_STAR_SMART_RADIUS_IDX
_BEACON_CIRCLE_COLOR = "blue"
_BEACON_CIRCLE_ALPHA = 0.2
def __init__(self):
"""
コンストラクタ
"""
# 親クラスの初期化処理
super().__init__()
# 経路生成手法名をRRT*-Smartとする
self._path_plan = PATHPLAN.RRT_STAR_SMART.value
# アニメーション描画関連のメソッド ↓
def _plot_2d_tree_animation(self, i, rrt):
"""
ツリーの描画 (2次元アニメーション用)
パラメータ
i(int): プロットしたい経由点番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
# iとRRT*の成功回数を比較
if i == 0:
# 始点だけを取得
start_tree_nodes = rrt.start_tree.nodes
start_tree_pos = start_tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
start_tree_parent = start_tree_nodes[0, self._NEAR_NODE_IDX].reshape(1, -1)
# ビーコンを取得 (list型 → numpy.ndarray型)
beacons = np.array(rrt.beacons)
elif i < rrt.count_success:
# i回成功目のツリーをファイルから読み込む
# RRT*はノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
# ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
file_name = f"{rrt.file_name_current_tree}_{i}.csv"
full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
start_tree_nodes = np.loadtxt(full_path)
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# ビーコンファイルを取得
file_name = f"{rrt.file_name_beacons}_{i}.csv"
full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
beacons = np.loadtxt(full_path)
else:
# 最終的な経由点を全部取得
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# ビーコンを取得 (list型 → numpy.ndarray型)
beacons = np.array(rrt.beacons)
# 親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
# ビーコンをプロット
# ビーコンを位置と半径に分割 (行列をベクトルに変換)
beacon_pos = beacons[-1, :-1]
beacon_radius = beacons[-1, -1]
# 関節空間から位置空間へ変換
beacon_center = self._convert_theta_to_pos(rrt.robot, beacon_pos, rrt.interpolation)
self._plot_circle(beacon_center[0], beacon_center[1], beacon_radius, color=self._EXPLOTION_CIRCLE_COLOR, alpha=self._EXPLOTION_CIRCLE_ALPHA, fill=False)
def _plot_3d_tree_animation(self, i, rrt):
"""
ツリーの描画 (3次元アニメーション用)
パラメータ
i(int): プロットしたい経由点番号
rrt(rrt.py内のクラス): 経路生成手法クラス
"""
if i < 0:
# 異常値のため，処理終了
return
# iとRRT*の成功回数を比較
if i == 0:
# 始点だけを取得
start_tree_nodes = rrt.start_tree.nodes
start_tree_pos = start_tree_nodes[0, :self._NEAR_NODE_IDX].reshape(1, -1)
start_tree_parent = start_tree_nodes[0, self._NEAR_NODE_IDX].reshape(1, -1)
# ビーコンを取得 (list型 → numpy.ndarray型)
beacons = np.array(rrt.beacons)
elif i < rrt.count_success:
# i回成功目のツリーをファイルから読み込む
# RRT*はノード数を増やしていくと，親ノードも変わるため，ノード追加ごとにツリーを毎回ファイル保存している
# ファイルからツリーを読み込む (ファイル名 = 経路生成手法名 / 探索空間名 / result / ツリー名_ノード追加番号.csv)
file_name = f"{rrt.file_name_current_tree}_{i}.csv"
full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
start_tree_nodes = np.loadtxt(full_path)
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# ビーコンファイルを取得
file_name = f"{rrt.file_name_beacons}_{i}.csv"
full_path = os.path.join(rrt.name, rrt.interpolation, rrt.folder_name_current_tree, file_name)
beacons = np.loadtxt(full_path)
else:
# 最終的な経由点を全部取得
# パラメータrrtから使用するデータをローカル変数に保存
start_tree_nodes = rrt.start_tree.nodes
# ノードを位置と親ノードに分割
start_tree_pos = start_tree_nodes[:, :self._NEAR_NODE_IDX]
start_tree_parent = start_tree_nodes[:, self._NEAR_NODE_IDX]
# ビーコンを取得 (list型 → numpy.ndarray型)
beacons = np.array(rrt.beacons)
# 親ノード一緒にプロット
self._plot_all_node_parent(start_tree_pos, start_tree_parent, rrt.robot, rrt.interpolation, rrt.dim)
# 始点ノードと終点ノードをプロット
self._plot_start_end_node(rrt)
# ビーコンをプロット
# ビーコンを位置と半径に分割 (行列をベクトルに変換)
beacon_pos = beacons[-1, :-1]
beacon_radius = beacons[-1, -1]
# 関節空間から位置空間へ変換
beacon_center = self._convert_theta_to_pos(rrt.robot, beacon_pos, rrt.interpolation)
self._plot_ball(beacon_center, beacon_radius, color=self._EXPLOTION_CIRCLE_COLOR, alpha=self._EXPLOTION_CIRCLE_ALPHA)
# アニメーション描画関連のメソッド ↑
class MainRRTRobotAnimation:
"""
ロボットの経路生成アニメーションのメインクラス
プロパティ
なし
メソッド
public
plot(): グラフの描画
plot_Animation(): アニメーションの描画
protected
_get_each_anime_instance(): 経路生成手法に応じたアニメーションクラスのインスタンスを取得
"""
# 定数の定義
def __init__(self):
"""
コンストラクタ
"""
# フォルダ作成
for enum_data in PATHPLAN:
# ファルダーが作成済みでもエラーを出力しないよう，exist_ok=Trueとした．
os.makedirs(enum_data.value, exist_ok=True)
def _get_each_anime_instance(self, rrt):
"""
経路生成手法に応じたアニメーションクラスのインスタンスを取得
パラメータ
rrt(rrt.py内のクラス): 経路生成手法クラス
戻り値
rrt_anime: 各経路生成手法のアニメーションクラス
"""
# 経路生成手法によって，アニメーションの分岐を分ける
if rrt.name == PATHPLAN.RRT.value:
# RRTアニメーション
rrt_anime = RRTAnimation()
elif rrt.name == PATHPLAN.RRT_STAR.value:
# RRT*アニメーション
rrt_anime = RRTStarAnimation()
elif rrt.name == PATHPLAN.RRT_STAR_SMART.value:
# RRT*-Smartアニメーション
rrt_anime = RRTStarSmartAnimation()
else:
# 異常
raise ValueError(f"rrt.name is abnormal. rrt.name is {rrt.name}")
return rrt_anime
def plot(self, rrt):
"""
グラフの描画
パラメータ
rrt(rrt.py内のクラス): 経路生成手法
"""
# アニメーションクラスのインスタンスを取得
rrt_anime = self._get_each_anime_instance(rrt)
# アニメーション描画
rrt_anime.plot(rrt)
def plot_Animation(self, rrt):
"""
アニメーションの描画
パラメータ
rrt(rrt.py内のクラス): 経路生成手法
"""
# アニメーションクラスのインスタンスを取得
rrt_anime = self._get_each_anime_instance(rrt)
# アニメーション描画
rrt_anime.plot_Animation(rrt)
メイン処理 (main.py)
経路生成およびアニメーション作成のメイン処理を説明する．
main.py
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot2DoF, Robot3DoF, Robot6DoF # ロボットクラス
from animation import MainRRTRobotAnimation # 経路生成手法のアニメーション
from rrt import MainRRTRobot # 経路生成
from environment import Robot2DEnv, Robot3DEnv # 経路生成の環境
CONST_SEED = 1 # 固定したいシード値
# CONST_SEED = np.random.randint(MIN_SEED, MAX_SEED) # 固定したいシード値
# print(f"CONST_SEED = {CONST_SEED}")
# N_ROBOT_AXIS = DIMENTION_2D # ロボットの軸数
# N_ROBOT_AXIS = DIMENTION_3D # ロボットの軸数
N_ROBOT_AXIS = DIMENTION_6D # ロボットの軸数
def get_prepare_infos():
"""
経路生成で使用する情報を取得
戻り値
robot(robot.py内のクラス): ロボットクラス
start_pos(numpy.ndarray): 始点
end_pos(numpy.ndarray): 終点
environment(environment.py内のクラス): 環境クラス
interpolations(list): 探索空間
"""
if N_ROBOT_AXIS == DIMENTION_2D: # 2軸ロボットアームの場合
# 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])
# 環境
environment = Robot2DEnv()
# 探索空間
interpolations = [INTERPOLATION.POSITION.value, INTERPOLATION.JOINT.value]
elif N_ROBOT_AXIS == DIMENTION_3D: # 3軸ロボットアームの場合
# 3軸ロボットのリンク長
links = np.array([1.0, 1.0, 1.0])
# 3軸ロボットのインスタンスを作成
robot = Robot3DoF(links)
# 始点
start_pos = np.array([1.0, -1.0, 1.0])
# 終点
end_pos = np.array([-1.0, 1.0, 2.0])
# 環境
environment = Robot3DEnv()
# 探索空間
interpolations = [INTERPOLATION.POSITION.value, INTERPOLATION.JOINT.value]
elif N_ROBOT_AXIS == DIMENTION_6D: # 6軸ロボットアームの場合
# 6軸ロボットのリンク長
links = np.array([1.0, 1.0, 1.0, 0.1, 0.1, 0.1])
# 6軸ロボットのインスタンスを作成
robot = Robot6DoF(links)
# 始点 (位置(x, y, z), 姿勢(z, y, x))
start_pos = np.array([ 1.0, -1.0, 1.0, 0, 0 , 0])
# 終点
end_pos = np.array([-1.0, 1.0, 2.0, 0, np.pi / 2, 0])
# 環境
environment = Robot3DEnv()
# 探索空間
interpolations = [INTERPOLATION.JOINT.value, ]
else:
# 異常
raise ValueError(f"N_ROBOT_AXIS is abnormal. N_ROBOT_AXIS is {N_ROBOT_AXIS}")
return robot, start_pos, end_pos, environment, interpolations
def main():
"""
メイン処理
"""
robot, start_pos, end_pos, environment, interpolations = get_prepare_infos()
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のデバッグフラグ
debugs = [True, False]
debugs = [True, ]
# debugs = [False, ]
# 経路の修正方法
# modification = MODIFICATION.PARTIAL_START
modification = None
# 経路生成手法
path_plans = [PATHPLAN.RRT_STAR.value, PATHPLAN.RRT_STAR_SMART.value]
# path_plans = [PATHPLAN.RRT_STAR_SMART.value, ]
# 描画方法
plot_modes = [PLOT_GRAPH, PLOT_ANIMATION]
plot_modes = [PLOT_ANIMATION, ]
# アニメーション作成クラスのインスタンス生成
robot_anime = MainRRTRobotAnimation()
# 経路生成手法クラスのインスタンスを作成
rrt_robot = MainRRTRobot()
for path_plan in path_plans:
for debug in debugs:
for interpolation in interpolations:
# 各々のアルゴリズムで比較したいからシード値は固定
np.random.seed(CONST_SEED)
if interpolation == INTERPOLATION.POSITION.value:
# 位置空間での経路生成の実行
planning_result = rrt_robot.planning(path_plan, start_pos, end_pos, environment, robot, interpolation, modification=modification, debug=debug)
else:
# 関節空間での経路生成の実行
planning_result = rrt_robot.planning(path_plan, start_theta, end_theta, environment, robot, interpolation, modification=modification, debug=debug)
print(f"planning_result = {planning_result}")
if not planning_result:
# 経路生成の失敗
continue
# 描画
for plot_mode in plot_modes:
if plot_mode == PLOT_GRAPH:
# グラフ描画
robot_anime.plot(rrt_robot.rrt)
elif plot_mode == PLOT_ANIMATION:
# アニメーション描画
robot_anime.plot_Animation(rrt_robot.rrt)
else:
# 何もしない
pass
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
性能評価およびアニメーション
RRT*-Smart の性能評価およびアニメーションを展開する．
RRT*-Smart と RRT* の経路比較
RRT*-Smart は RRT* の探索範囲を効率化した手法であるため，経路の比較をする．
アニメーションの見方は以下の通りである．
・水色の星形(左下)データ：経路生成の始点
・緑色の星形(右上)データ：経路生成の終点
・灰色の星形データ：作成されたノード
・赤色の線：新規データと最短コスト(距離)の親ノードを結んだ線
・緑色の線：最終的な始点から終点までの経路
・灰色の円：干渉物
初めに，RRT* でロボットの位置空間上で作成した経路を下図に添付した．
最後に，RRT*-Smart でロボットの位置空間上で作成した経路を下図に添付した．
RRT* と RRT*-Smart で各々2,000回の探索をした．
RRT*-Smart の方が，最適な経路になったことがわかる．今回は，関節空間を探索したが，直行空間でも探索できるようにしたら，生成される経路が変わる．
RRT* と RRT*-Smart のアルゴリズム比較
上記では，各アルゴリズムでの始点から終点までの経路を比較した．
本節では，各アルゴリズムで作成した経路ができるまでの過程を比較する．
添付できるアニメーション容量が最大で10 [MB] なため，容量の関係上，20探索ごとに描画していくアニメーションを作成した．
アニメーションの見方は以下の通りである．
・水色の星形(中下)データ：経路生成の始点
・緑色の星形(中上)データ：経路生成の終点
・青色の球：Intelligent Sampling での探索範囲
・赤色の線：新規データと最短コスト(距離)の親ノードを結んだ線 (始点ツリー)
・オレンジ色の線：新規データと最短コスト(距離)の親ノードを結んだ線 (終点ツリー)
・緑色の線：最終的な始点から終点までの経路
・灰色の球/直方体：干渉物
初めに，RRT* でロボットの位置空間上で作成した経路ができるまでの過程を下図に添付した．
最後に，RRT*-Smart でロボットの位置空間上で作成した経路ができるまでの過程を下図に添付した．
アニメーションの容量上，20回探索ごとのアニメーションのため，RRT*-Smart のビーコンをプロットできなかった．
ただ，最終的な経路としては，RRT*-Smart の方が最適な経路となった．
6軸ロボットアームや複雑(干渉物が多数)な環境では，RRT*-Smart の方が有利となった．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・干渉物の存在する3次元空間で，6軸ロボットアームを用いて RRT*-Smart による経路生成 + アニメーション
次記事では，下記内容を実装していきます．
・干渉物の存在する3次元空間で，6軸ロボットアームを用いて RRT*-Connect による経路生成 + アニメーション