空白以外の文字列を拾うには？ 出力：
std::regexのオブジェクトを作る際（10行目）、raw string literalを使えばエスケープを行う必要はない。
数字を拾うには？ 出力：
検索文字の先頭と末尾に特定の文字を挿入するには？ 出力： 参照
2013年8月23日金曜日
C++メモ 〜 std::regex 〜
大文字小文字を区別せずに検索するには？
出力：
raw string literalを使えばエスケープせずにダブルクオーテーションを利用できる（７行目）。
空白以外の文字列を拾うには？ 出力：
数字を拾うには？ 出力：
検索文字の先頭と末尾に特定の文字を挿入するには？ 出力： 参照
空白以外の文字列を拾うには？ 出力：
数字を拾うには？ 出力：
検索文字の先頭と末尾に特定の文字を挿入するには？ 出力： 参照
2013年8月6日火曜日
C++メモ 〜 boost::coroutine 〜
Macではlibboost_coroutine.aとlibboost_context.dylib(.a)をリンクする必要があります。
出力：
出力：
出力：
おまけ。 出力：
追記：2014/1/12 ファイルa.txtの内容は以下の通り。 出力： 関数
Routine1のコンストラクタが呼ばれた時点でloop1の中の
yieldが一度呼ばれます。
出力：
loop2の先頭にあるyield(1)が出力の先頭にある1を生成します。
yield(1)がないと、15行目で落ちます。
Routine2のコンストラクタが呼ばれた時点ではAのオブジェクトが設定されていないからです。
出力：
おまけ。 出力：
追記：2014/1/12 ファイルa.txtの内容は以下の通り。 出力： 関数
read_fileのプロトタイプがないと警告がでるようになった(Apple LLVM 5.0, boost-1.55.0)。
2013年8月4日日曜日
運動学
in English
ロボット工学で使われる順運動学と逆運動学についてまとめる。 実装編はこちら。
３次元空間右手系の同次座標で議論を行う。各軸周りの回転行列は以下の通りである。
$x$軸周りの回転行列 $R_{x}(\theta)$:
\begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $y$軸周りの回転行列 $R_{y}(\theta)$: \begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $z$軸周りの回転行列 $R_{z}(\theta)$: \begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} また、方向 $\vec{l}$ への並進移動行列 $L(\vec{l})$ は次式で与えられる。 \begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} いま各関節 $n_i$ の初期状態が図のように与えられたとする。
関節 $n_1$ は$x$軸周りに $\theta_1$、関節 $n_2$ は$y$軸周りに $\theta_2$、関節 $n_3$ は$x$軸周りに $\theta_3$ だけ回転するとき、端点$n_4$の位置ベクトルは次式で計算される。
\begin{equation}
\vec{p}_{4} = R_x(\theta_1) L(\vec{l}_1) R_y(\theta_2) L(\vec{l}_2) R_x(\theta_3) L(\vec{l}_3)
\left(
\begin{array}{c}
0 \\
0 \\
0 \\
1
\end{array}
\right)
\end{equation}
各関節での局所座標は、その関節に到るまでの変換行列の影響を受ける。回転行列、並進移動行列のパラメータは各関節の局所座標系で定義される量である。一般に関節が$n$個ある場合の端点(end-effector)の位置座標 $\vec{p}$ は
\begin{equation}
\vec{p} = R_{a_{1}}(\theta_1) L(\vec{l}_1) R_{a_{2}}(\theta_2) L(\vec{l}_2) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n)
\left(
\begin{array}{c}
0 \\
0 \\
0 \\
1
\end{array}
\right)
\end{equation}
となる。ここで$a_{i}\in \{x,y,z\}$である。
腕（リンク）の長さが定数であるとき位置ベクトル $\vec{p}$ は $n$ 個の角度の関数である。 \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n) \end{equation} 変分をとると \begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots \end{eqnarray} 2次以降の項を無視して書き下ろすと \begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \end{eqnarray} となる。ここで行列 $J$ はヤコビアン（Jacobian）である。いまの場合 $3 \times n$ の行列となる。要素に現れる偏微分は次式で計算できる。 \begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right) \end{equation} 逆行列 $J^{-1}$を使えば位置の変位量 $\delta \vec{p}$ から角度の変位量 $\delta \vec{\theta}$ を求めることができる。 \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p} \end{equation} ${\rm rank}(J)>3$のとき $J$ の逆行列は無数に存在する。 すなわち、関節の数が3より大きい場合、1つの端点（end effector）の位置に対応する腕の姿勢は無数に存在する。したがって、任意の拘束条件を導入して $\delta \vec{\theta}$ を一意に決定する必要がある。その代表的な方法が、擬似逆行列（pseudoinverse）$J^{\#}$ を用いる方法である。 \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p} \end{equation} ここで、 \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1} \end{equation} である。 逆運動学問題を解く手順は以下の通りである。
はじめに
ロボット工学で使われる順運動学と逆運動学についてまとめる。 実装編はこちら。
順運動学
３次元空間右手系の同次座標で議論を行う。各軸周りの回転行列は以下の通りである。
$x$軸周りの回転行列 $R_{x}(\theta)$:
\begin{equation} R_{x}(\theta) = \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} & 0 \\ 0 & \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $y$軸周りの回転行列 $R_{y}(\theta)$: \begin{equation} R_{y}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} $z$軸周りの回転行列 $R_{z}(\theta)$: \begin{equation} R_{z}(\theta) = \left( \begin{array}{cccc} \cos{\theta} & -\sin{\theta} & 0 & 0 \\ \sin{\theta} & \cos{\theta} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} また、方向 $\vec{l}$ への並進移動行列 $L(\vec{l})$ は次式で与えられる。 \begin{equation} L(\vec{l}) = \left( \begin{array}{cccc} 1 & 0 & 0 & l_x \\ 0 & 1 & 0 & l_y \\ 0 & 0 & 1 & l_z \\ 0 & 0 & 0 & 1 \end{array} \right) \end{equation} いま各関節 $n_i$ の初期状態が図のように与えられたとする。
逆運動学
腕（リンク）の長さが定数であるとき位置ベクトル $\vec{p}$ は $n$ 個の角度の関数である。 \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n) \end{equation} 変分をとると \begin{eqnarray} \delta\vec{p} &=& \vec{f}(\theta_1+\delta\theta_1,\theta_2+\delta\theta_2,\cdots,\theta_n+\delta\theta_n) - \vec{f}(\theta_1,\theta_2,\cdots,\theta_n)\nonumber \\ &=&\sum_{i=1}^{n}\;\frac{\partial\vec{f}}{\partial\theta_i}\;\delta\theta_i + \frac{1}{2}\sum_{i=1}^{n}\sum_{j=1}^{n}\;\frac{\partial^2\vec{f}}{\partial\theta_i\partial\theta_j}\;\delta\theta_i\;\delta\theta_j +\cdots \end{eqnarray} 2次以降の項を無視して書き下ろすと \begin{eqnarray} \left( \begin{array}{c} \delta p_x \\ \delta p_y \\ \delta p_z \end{array} \right) &=& \left(\frac{\partial\vec{f}}{\partial\theta_1},\frac{\partial\vec{f}}{\partial\theta_2},\cdots,\frac{\partial\vec{f}}{\partial\theta_n}\right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &=& \left( \begin{array}{cccc} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \cdots & \frac{\partial f_2}{\partial \theta_n} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \cdots & \frac{\partial f_3}{\partial \theta_n} \end{array} \right) \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \nonumber \\ &\equiv& J \left( \begin{array}{c} \delta \theta_1 \\ \delta \theta_2 \\ \vdots \\ \delta \theta_n \end{array} \right) \end{eqnarray} となる。ここで行列 $J$ はヤコビアン（Jacobian）である。いまの場合 $3 \times n$ の行列となる。要素に現れる偏微分は次式で計算できる。 \begin{equation} \frac{\partial \vec{f}}{\partial \theta_i} = R_{a_{1}}(\theta_1) L(\vec{l}_1) \cdots \frac{dR_{a_{i}}(\theta_i)}{d\theta_i} L(\vec{l}_i) \cdots R_{a_{n}}(\theta_n) L(\vec{l}_n) \left( \begin{array}{c} 0 \\ 0 \\ 0 \\ 1 \end{array} \right) \end{equation} 逆行列 $J^{-1}$を使えば位置の変位量 $\delta \vec{p}$ から角度の変位量 $\delta \vec{\theta}$ を求めることができる。 \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p} \end{equation} ${\rm rank}(J)>3$のとき $J$ の逆行列は無数に存在する。 すなわち、関節の数が3より大きい場合、1つの端点（end effector）の位置に対応する腕の姿勢は無数に存在する。したがって、任意の拘束条件を導入して $\delta \vec{\theta}$ を一意に決定する必要がある。その代表的な方法が、擬似逆行列（pseudoinverse）$J^{\#}$ を用いる方法である。 \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p} \end{equation} ここで、 \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1} \end{equation} である。 逆運動学問題を解く手順は以下の通りである。
- 端点の目標位置を $\vec{p}_{\rm G}$ とする。
- 現在の端点の位置 $\vec{p}$ からの変位量 $\vec{d} = \vec{p}_{\rm G}-\vec{p}$ から微小変位量を求める。$\delta \vec{p}=\alpha\;\vec{d}/|\vec{d}|$。ここで $\alpha$ は適当な微小量である。
- 現在の $\vec{\theta}$ を使って $J^{\#}$ を計算する。
- $\delta \vec{\theta}=J^{\#} \delta\vec{p}$ を計算する。
- $\vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}$
- 更新後の変位量 $|\vec{d}| = |\vec{p}_{\rm G}-\vec{p}|$が許容誤差 $\epsilon$ 以下なら計算終了。そうでなければ、2へ戻る。
参考文献
登録:
投稿 (Atom)