TSR - (2014/08/05 (火) 20:30:08) の１つ前との変更点
追加された行は緑色になります。
削除された行は赤色になります。
* 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) ) $$
** TSR Chain
その名の通りTSRを連結したもの．
* CBiRRT2: Constrained Bidirectional RRT
TSRによる拘束条件を用いて計算可能な拡張型[[RRT]]．
CBiRRT2(q_start, q_goal) {
tree_a.Init(q_init); // 初期値x_initをツリーのルートに登録する
tree_b.Init(q_goal); // 目標値x_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;
}
** 参考文献
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) ) $$
** TSR Chain
その名の通りTSRを連結したもの．
* 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;
}
** 参考文献
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
表示オプション
横に並べて表示：
変化行の前後のみ表示：
