「TSR」(2014/08/06 (水) 20:13:56) の最新版変更点
追加された行は緑色になります。
削除された行は赤色になります。
* TSR: Task Space Region
エンドエフェクタの拘束条件を記述する．
** TSR
3つのパートからなる．
$$ ^o T _w $$ : 世界座標系oからTSR座標系wへの座標変換
$$ ^w T _e $$ : TSR座標系wからエンドエフェクタeへの座標変換
$$ ^w B $$ : TSR座標系における拘束条件
$$ ^w B = \left[ \begin{array}{cc} x_{min} & x_{max} \\ y_{min} & y_{max} \\ z_{min} & z_{max} \\ \psi_{min} & \psi_{max} \\ \theta_{min} & \theta_{max} \\ \phi_{min} & \phi_{max} \\ \end{array} \right] $$
回転はRPYで表される．
(この後の検算の結果，おそらく元論文ではRPY = $$ RotZ(\phi) RotY(\theta) RotX(\psi) $$ と思われる．)
** TSRに対する距離の定義
コンフィグレーション空間 $$ q_s $$ に対応するエンドエフェクタの座標を $$ ^o T _s $$とする．
このときのTSR座標系を $$ ^o T _{s'} $$ とすると，次のように逆算できる．
$$ {^o T _{s'}} = {^o T _s} ( {^w T _e} ) ^ {-1} $$
初期のTSR座標系との差分は以下のように計算される．
$$ {^w T _{s'}} = ( { ^o T _w } )^{-1} { ^o T _{s'} } $$
これを拘束 $$ ^w B $$ に合わせた形になおすと，
$$ ^w d = \left[ \begin{array}{c} ^w t _{s'} \\ \arctan ( {^w R _{s'}}(3, 2) / {^w R _{s'}}(3, 3) ) \\ -\arcsin {^w R _{s'}}(3, 1) \\ \arctan ( {^w R _{s'}}(2, 1) / {^w R _{s'}}(1, 1) ) \\ \end{array} \right] $$
拘束に対する距離に直すと，各要素は以下のようになる．
$$ \Delta x _i = \left\{ \begin{array}{cc} {^w d _i} - {^w B}(i, 1) & \mbox{if}({^w d _i} < {^w B}(i, 1)) \\ {^w d _i} - {^w B}(i, 2) & \mbox{if}({^w d _i} > {^w B}(i, 2)) \\ 0 & \mbox{otherwise} \end{array} \right. $$
すなわちTSRの距離は $$ || \Delta x || $$ となる．
*** 回転角度の検算
$$ RotZ(\phi) RotY(\theta) RotX(\psi) = \left[ \begin{array}{ccc} C_{\phi} C_{\theta} & -S_{\phi} C_{\psi} + C_{\phi} S_{\theta} S_{\psi} & S_{\phi} S_{\psi} + C_{\phi} S_{\theta} C_{\psi} \\ S_{\phi} C_{\theta} & C_{\phi} C_{\psi} + S_{\phi} S_{\theta} S_{\psi} & -C_{\phi} S_{\psi} + S_{\phi} S_{\theta} C_{\psi} \\ -S_{\theta} & C_{\theta} S_{\psi} & C_{\theta} C_{\psi}\end{array} \right] $$
ここから，
$$ \psi = \arctan (C_{\theta} S_{\psi}) / (C_{\theta} C_{\psi}) = \arctan ( {^w R _{s'}}(3, 2) / {^w R _{s'}}(3, 3) ) $$
$$ \theta = -\arcsin (-S_{\theta}) = -\arcsin {^w R _{s'}}(3, 1) $$
$$ \phi = \arctan (S_{\phi} C_{\theta}) / (C_{\phi} C_{\theta}) = \arctan ( {^w R _{s'}}(2, 1) / {^w R _{s'}}(1, 1) ) $$
** 拘束多様体への投影
距離を定義できたので，距離を零とするような$$q$$の修正を考える．
ヤコビアンの擬似逆行列を用いて反復的に求めることができる．
$$ \Delta q _{error} = J^T (J J^t) ^{-1} \Delta x $$
$$ q \leftarrow (q - \Delta q_{error}) $$
** TSR Chain
その名の通りTSRを連結したもの．
TSRにおけるエンドエフェクタ座標が次のTSR座標に，
二番目以降のTSR座標がそれ以前のTSRを連結した結果になる．
ここで，仮想関節$$ ^w T _{sample} $$をTSRに付加することを考える．
$$ C_i = [{^o T _w}, {^w T _e}, {^w B}, {^w T _{sample}}] $$
という4つの構成要素から成り立つものをi番目のTSRとすると，
$$ {^o T _w} $$はそれ以前のすべてのTSRの$$ ^w T _{sample} $$の影響を受ける．
つまり，
$$ C_i . {^o T _w} = (C_{i-1} . {^o T _w}) (C_{i-1} . {^w T _{sample}}) (C_{i-1} . {^w T _e}) $$
各TSRの仮想関節$$ ^w T _{sample} $$を決定するような仮想関節角度列cをTSR Chainに持たせる．
** TSR Chain に対する距離の定義
コンフィグレーション空間 $$ q_s $$ に対応するエンドエフェクタの座標を $$ ^o T _s $$とする．
TSR Chain自体をひとつの仮想マニピュレータとして捉え，終端の座標を$$ ^o T _s $$を目標としてIKを使って解く．
その時の両座標の差分が距離として定義される．
* CBiRRT2: Constrained Bidirectional RRT
TSRによる拘束条件を用いて計算可能な拡張型[[RRT]]．
CBiRRT2(q_start, q_goal) {
tree_a.Init(q_init); // 初期値q_initをツリーのルートに登録する
tree_b.Init(q_goal); // 目標値q_goalを別のツリーのルートに登録する
while (TimeRemaining()) { // 規定の時間内に計算が終わらなければ失敗
tree_goal = GetBackwardTree(tree_a, tree_b); // ゴールから伸びている方の木を選ぶ
if (tree_goal.empty() || // ゴールから伸びている方の木が空か，
random(0, 1) < P_SAMPLE) { // 確率P_SAMPLEでTSRから直接サンプリング
AddRoot(tree_goal);
} else { // 通常の探索を行う
q_rand = RandomConfig(); // ランダムに目標値を入れる
// ツリーAの探索
q_a_near = NearestNeighbor(tree_a, q_rand);
q_a_rearch = ConstrainedExtend(tree_a, q_a_near, q_rand);
// ツリーBの探索
q_b_near = NearestNeighbor(tree_b, q_rand);
q_b_rearch = ConstrainedExtend(tree_b, q_b_near, q_rand);
if (q_a_reach == q_b_reach) { // 十分近いところに到達したならば
path = ExtractPath(tree_a, q_a_reach, tree_b, q_b_reach); // 2つの木を接続
return ShortenPath(path); // パスを最適化したものが解
} else {
Swap(tree_a, tree_b); // 2つのツリーを入れ替える
}
}
}
return NULL;
}
ConstrainedExtend(tree, q_near, q_target) {
q_s = q_near;
q_s_old = q_near;
while (true) {
if (q_target == q_s) { // 目標に到達している場合
return q_s;
} else if (norm(q_target - q_s) > norm(q_s_old - q_target)) { // 発散する方向に動いている場合
return q_s_old;
}
q_s_old = q_s;
q_s = q_s +
min(DELTA_Q, norm(q_target - q_s) * (q_target - q_s) / norm(q_target - q_s) // ターゲット方向に動く
c = GetConstraintValues(tree, q_s_old); // q_s_oldのときの仮想関節角度列c．同時に格納されているので取り出すのみで良い．
(q_s, c) = ConstrainConfig(q_s_old, q_s, c, NULL); // コンフィグ空間から拘束多様体への投影を行う
if (!q_s.empty()) {
tree.AddVertex(q_s, c);
tree.AddEdge(q_s_old, q_s);
} else {
return q_s_old;
}
}
}
ここまでは細かい差異はあるが基本的にBiRRTと同じ．
直接サンプリングを行う場合
AddRoot(tree) {
for (i = 1 to m) { // mはTSR Chainの数
cc = GetTSRChainsForManipulator(i); // ccはTSR Chain
(t_o_to_targ, c) = SampleFromTSRChains(cc); // 直接サンプリング
targets.AddTarget(t_o_to_targ, i);
}
(q_s, c) = GetInitialGuess(); // 初期値，元論文ではロボットの初期姿勢と零ベクトルと記述されている
(q_s, c) = ConstrainConfig(NULL, q_s, c, targets); // コンフィグ空間から拘束多様体への投影を行う
if (!q_s.empty()) {
tree.AddVertex(q_s, c);
}
}
コンフィグレーションが拘束条件を満たすように毎度修正を行う．
// @brief コンフィグレーションが拘束条件を満たすように，物理拘束を考慮し拘束多様体へ投影する
// @param q_s_old 一手前の関節角度列
// @param q_s 関節角度列
// @param c TSR Chainの仮想関節角度列
// @param targets 目標座標のリスト
ConstrainConfig(q_s_old, q_s, c, targets) {
check_dist = false;
if (targets.empty()) { // ConstrainedExtendから呼ばれた場合
check_dist = true;
for (i = 1 to m) { // mはTSR Chainの数
cc = GetTSRChainsForManipulator(i); // ccはTSR Chain
t_o_to_s = GetEndEffectorTransform(q_s, i); // q_sのときのエンドエフェクタの姿勢
(t_o_to_targ, c) = GetClosestTransform(cc, t_o_to_s, c); // 目標座標をTSR Chainから修正する
targets.AddTarget(t_o_to_targ, i);
}
}
q_s = UpdatePhysicalConstraintDOF(q_s, c); // 物理拘束によりq_sを修正する
q_s = ProjectConfig(q_s, targets); // 多様体への投影を行う
if (q_s.empty() || // 拘束多様体への投影が出来なかった場合か，
(check_dist && // ConstrainedExtendから呼ばれ，
norm(q_s - q_s_old) > 2 * DELTA_Q) { // かつ一手前の値から2*DELTA_Q以上離れた時
return NULL; // 失敗とする．
}
return (q_s, c);
}
うまくつながったら最後はパスを最適化して返す．
ShortenPath(path) {
while (TimeRemaining()) { // 規定の時間内粘る
tree_shortcut = {};
i = (int)(random(1, path.size() - 1));
j = (int)(random(i, path.size()));
q_reach = ConstrainedExtend(tree_shortcut, path[i], path[j]); // iからjまで直通可能か調べる
if (q_reach == path[j] && // jまで到達できて
tree_shortcut.size() < (j - i)) { // かつ元々のパスよりも短ければ
path = {path[0 ... i], tree_shortcut, path[j + 1 ...]}; // i-j間を直通したものに置き換える
}
}
return path;
}
** 参考文献
Dmitry Berenson, Siddhartha Srinivasa, James Kuffner.
"Task Space Regions: A Framework for Pose-Constrained Manipulation Planning",
International Journal of Robotics Research (IJRR), Vol. 30, No. 12, October, 2011, pp. 1435 - 1460.
http://www.ri.cmu.edu/pub_files/2011/10/dmitry_ijrr10-1.pdf
* TSR: Task Space Region
エンドエフェクタの拘束条件を記述する．
** TSR
3つのパートからなる．
$$ ^o T _w $$ : 世界座標系oからTSR座標系wへの座標変換
$$ ^w T _e $$ : TSR座標系wからエンドエフェクタeへの座標変換
$$ ^w B $$ : TSR座標系における拘束条件
$$ ^w B = \left[ \begin{array}{cc} x_{min} & x_{max} \\ y_{min} & y_{max} \\ z_{min} & z_{max} \\ \psi_{min} & \psi_{max} \\ \theta_{min} & \theta_{max} \\ \phi_{min} & \phi_{max} \\ \end{array} \right] $$
回転はRPYで表される．
(この後の検算の結果，おそらく元論文ではRPY = $$ RotZ(\phi) RotY(\theta) RotX(\psi) $$ と思われる．)
** TSRに対する距離の定義
コンフィグレーション空間 $$ q_s $$ に対応するエンドエフェクタの座標を $$ ^o T _s $$とする．
このときのTSR座標系を $$ ^o T _{s'} $$ とすると，次のように逆算できる．
$$ {^o T _{s'}} = {^o T _s} ( {^w T _e} ) ^ {-1} $$
初期のTSR座標系との差分は以下のように計算される．
$$ {^w T _{s'}} = ( { ^o T _w } )^{-1} { ^o T _{s'} } $$
これを拘束 $$ ^w B $$ に合わせた形になおすと，
$$ ^w d = \left[ \begin{array}{c} ^w t _{s'} \\ \arctan ( {^w R _{s'}}(3, 2) / {^w R _{s'}}(3, 3) ) \\ -\arcsin {^w R _{s'}}(3, 1) \\ \arctan ( {^w R _{s'}}(2, 1) / {^w R _{s'}}(1, 1) ) \\ \end{array} \right] $$
拘束に対する距離に直すと，各要素は以下のようになる．
$$ \Delta x _i = \left\{ \begin{array}{cc} {^w d _i} - {^w B}(i, 1) & \mbox{if}({^w d _i} < {^w B}(i, 1)) \\ {^w d _i} - {^w B}(i, 2) & \mbox{if}({^w d _i} > {^w B}(i, 2)) \\ 0 & \mbox{otherwise} \end{array} \right. $$
すなわちTSRの距離は $$ || \Delta x || $$ となる．
*** 回転角度の検算
$$ RotZ(\phi) RotY(\theta) RotX(\psi) = \left[ \begin{array}{ccc} C_{\phi} C_{\theta} & -S_{\phi} C_{\psi} + C_{\phi} S_{\theta} S_{\psi} & S_{\phi} S_{\psi} + C_{\phi} S_{\theta} C_{\psi} \\ S_{\phi} C_{\theta} & C_{\phi} C_{\psi} + S_{\phi} S_{\theta} S_{\psi} & -C_{\phi} S_{\psi} + S_{\phi} S_{\theta} C_{\psi} \\ -S_{\theta} & C_{\theta} S_{\psi} & C_{\theta} C_{\psi}\end{array} \right] $$
ここから，
$$ \psi = \arctan (C_{\theta} S_{\psi}) / (C_{\theta} C_{\psi}) = \arctan ( {^w R _{s'}}(3, 2) / {^w R _{s'}}(3, 3) ) $$
$$ \theta = -\arcsin (-S_{\theta}) = -\arcsin {^w R _{s'}}(3, 1) $$
$$ \phi = \arctan (S_{\phi} C_{\theta}) / (C_{\phi} C_{\theta}) = \arctan ( {^w R _{s'}}(2, 1) / {^w R _{s'}}(1, 1) ) $$
** 拘束多様体への投影
距離を定義できたので，距離を零とするような$$q$$の修正を考える．
ヤコビアンの擬似逆行列を用いて反復的に求めることができる．
$$ \Delta q _{error} = J^T (J J^t) ^{-1} \Delta x $$
$$ q \leftarrow (q - \Delta q_{error}) $$
** TSR Chain
その名の通りTSRを連結したもの．
TSRにおけるエンドエフェクタ座標が次のTSR座標に，
二番目以降のTSR座標がそれ以前のTSRを連結した結果になる．
ここで，仮想関節$$ ^w T _{sample} $$をTSRに付加することを考える．
$$ C_i = [{^o T _w}, {^w T _e}, {^w B}, {^w T _{sample}}] $$
という4つの構成要素から成り立つものをi番目のTSRとすると，
$$ {^o T _w} $$はそれ以前のすべてのTSRの$$ ^w T _{sample} $$の影響を受ける．
つまり，
$$ C_i . {^o T _w} = (C_{i-1} . {^o T _w}) (C_{i-1} . {^w T _{sample}}) (C_{i-1} . {^w T _e}) $$
各TSRの仮想関節$$ ^w T _{sample} $$を決定するような仮想関節角度列cをTSR Chainに持たせる．
** TSR Chain に対する距離の定義
コンフィグレーション空間 $$ q_s $$ に対応するエンドエフェクタの座標を $$ ^o T _s $$とする．
TSR Chain自体をひとつの仮想マニピュレータとして捉え，終端の座標を$$ ^o T _s $$を目標としてIKを使って解く．
その時の両座標の差分が距離として定義される．
* CBiRRT2: Constrained Bidirectional RRT
TSRによる拘束条件を用いて計算可能な拡張型[[RRT]]．
CBiRRT2(q_start, q_goal) {
tree_a.Init(q_init); // 初期値q_initをツリーのルートに登録する
tree_b.Init(q_goal); // 目標値q_goalを別のツリーのルートに登録する
while (TimeRemaining()) { // 規定の時間内に計算が終わらなければ失敗
tree_goal = GetBackwardTree(tree_a, tree_b); // ゴールから伸びている方の木を選ぶ
if (tree_goal.empty() || // ゴールから伸びている方の木が空か，
random(0, 1) < P_SAMPLE) { // 確率P_SAMPLEでTSRから直接サンプリング
AddRoot(tree_goal);
} else { // 通常の探索を行う
q_rand = RandomConfig(); // ランダムに目標値を入れる
// ツリーAの探索
q_a_near = NearestNeighbor(tree_a, q_rand);
q_a_rearch = ConstrainedExtend(tree_a, q_a_near, q_rand);
// ツリーBの探索
q_b_near = NearestNeighbor(tree_b, q_rand);
q_b_rearch = ConstrainedExtend(tree_b, q_b_near, q_rand);
if (q_a_reach == q_b_reach) { // 十分近いところに到達したならば
path = ExtractPath(tree_a, q_a_reach, tree_b, q_b_reach); // 2つの木を接続
return ShortenPath(path); // パスを最適化したものが解
} else {
Swap(tree_a, tree_b); // 2つのツリーを入れ替える
}
}
}
return NULL;
}
ConstrainedExtend(tree, q_near, q_target) {
q_s = q_near;
q_s_old = q_near;
while (true) {
if (q_target == q_s) { // 目標に到達している場合
return q_s;
} else if (norm(q_target - q_s) > norm(q_s_old - q_target)) { // 発散する方向に動いている場合
return q_s_old;
}
q_s_old = q_s;
q_s = q_s +
min(DELTA_Q, norm(q_target - q_s) * (q_target - q_s) / norm(q_target - q_s) // ターゲット方向に動く
c = GetConstraintValues(tree, q_s_old); // q_s_oldのときの仮想関節角度列c．同時に格納されているので取り出すのみで良い．
(q_s, c) = ConstrainConfig(q_s_old, q_s, c, NULL); // コンフィグ空間から拘束多様体への投影を行う
if (!q_s.empty()) {
tree.AddVertex(q_s, c);
tree.AddEdge(q_s_old, q_s);
} else {
return q_s_old;
}
}
}
ここまでは細かい差異はあるが基本的にBiRRTと同じ．
直接サンプリングを行う場合
AddRoot(tree) {
for (i = 1 to m) { // mはTSR Chainの数
cc = GetTSRChainsForManipulator(i); // ccはTSR Chain
(t_o_to_targ, c) = SampleFromTSRChains(cc); // 直接サンプリング
targets.AddTarget(t_o_to_targ, i);
}
(q_s, c) = GetInitialGuess(); // 初期値，元論文ではロボットの初期姿勢と零ベクトルと記述されている
(q_s, c) = ConstrainConfig(NULL, q_s, c, targets); // コンフィグ空間から拘束多様体への投影を行う
if (!q_s.empty()) {
tree.AddVertex(q_s, c);
}
}
コンフィグレーションが拘束条件を満たすように毎度修正を行う．
// @brief コンフィグレーションが拘束条件を満たすように，物理拘束を考慮し拘束多様体へ投影する
// @param q_s_old 一手前の関節角度列
// @param q_s 関節角度列
// @param c TSR Chainの仮想関節角度列
// @param targets 目標座標のリスト
ConstrainConfig(q_s_old, q_s, c, targets) {
check_dist = false;
if (targets.empty()) { // ConstrainedExtendから呼ばれた場合
check_dist = true;
for (i = 1 to m) { // mはTSR Chainの数
cc = GetTSRChainsForManipulator(i); // ccはTSR Chain
t_o_to_s = GetEndEffectorTransform(q_s, i); // q_sのときのエンドエフェクタの姿勢
(t_o_to_targ, c) = GetClosestTransform(cc, t_o_to_s, c); // 目標座標をTSR Chainから修正する
targets.AddTarget(t_o_to_targ, i);
}
}
q_s = UpdatePhysicalConstraintDOF(q_s, c); // 物理拘束によりq_sを修正する
q_s = ProjectConfig(q_s, targets); // 多様体への投影を行う
if (q_s.empty() || // 拘束多様体への投影が出来なかった場合か，
(check_dist && // ConstrainedExtendから呼ばれ，
norm(q_s - q_s_old) > 2 * DELTA_Q) { // かつ一手前の値から2*DELTA_Q以上離れた時
return NULL; // 失敗とする．
}
return (q_s, c);
}
うまくつながったら最後はパスを最適化して返す．
ShortenPath(path) {
while (TimeRemaining()) { // 規定の時間内粘る
tree_shortcut = {};
i = (int)(random(1, path.size() - 1));
j = (int)(random(i, path.size()));
q_reach = ConstrainedExtend(tree_shortcut, path[i], path[j]); // iからjまで直通可能か調べる
if (q_reach == path[j] && // jまで到達できて
tree_shortcut.size() < (j - i)) { // かつ元々のパスよりも短ければ
path = {path[0 ... i], tree_shortcut, path[j + 1 ...]}; // i-j間を直通したものに置き換える
}
}
return path;
}
** 参考文献
Dmitry Berenson, Siddhartha Srinivasa, James Kuffner.
"Task Space Regions: A Framework for Pose-Constrained Manipulation Planning",
International Journal of Robotics Research (IJRR), Vol. 30, No. 12, October, 2011, pp. 1435 - 1460.
http://www.ri.cmu.edu/pub_files/2011/10/dmitry_ijrr10-1.pdf
表示オプション
横に並べて表示：
変化行の前後のみ表示：
