2013年9月7日土曜日

運動学 〜 実装 〜

in English


はじめに


 先のページで解説したアルゴリズムをC++11で実装した。

デモ

緑の球が目的の位置を表す。ロボットの手の先がそれに到達すると、球は次の位置へ移動する。

開発環境

  1. Mac OS X 10.8.4
  2. プロセッサ:3.06 GHz Intel Core 2 Duo
  3. メモリ:4GB
  4. Xcode4.6.2 with Apple LLVM 4.2(C++ Language Dialect → C++11, C++ Standard Library → libc++)
  5. boost-1.54.0(Apple LLVM 4.2でコンパイルしたもの。)

ソース


ソースはここ。


実行方法


引数に渡しているファイルrobot.datによりロボットの形状を決定している。 そのフォーマットは文献1のものを使用した。 キーボードから
  1. 'c'を入力してスタート
  2. 'b'を入力すると逆向き
  3. 'a'を入力すると元の向き
  4. escapeキーで終了
徐々にロボットの手の先(end-effector)が目的の位置からずれていく。 フィードバック機構を導入する必要がある。

参考文献

  1. OpenGL 3Dグラフィックス入門 第2版 三浦憲二郎 朝倉書店

2013年8月23日金曜日

C++メモ 〜 std::regex 〜

大文字小文字を区別せずに検索するには? 出力: raw string literalを使えばエスケープせずにダブルクオーテーションを利用できる(7行目)。
空白以外の文字列を拾うには? 出力: std::regexのオブジェクトを作る際(10行目)、raw string literalを使えばエスケープを行う必要はない。
数字を拾うには? 出力:
検索文字の先頭と末尾に特定の文字を挿入するには? 出力: 参照

2013年8月6日火曜日

C++メモ 〜 boost::coroutine 〜

Macではlibboost_coroutine.aとlibboost_context.dylib(.a)をリンクする必要があります。 出力: 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


はじめに


 ロボット工学で使われる順運動学と逆運動学についてまとめる。 実装編はこちら。

順運動学


 3次元空間右手系の同次座標で議論を行う。各軸周りの回転行列は以下の通りである。
$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} である。 逆運動学問題を解く手順は以下の通りである。
  1. 端点の目標位置を $\vec{p}_{\rm G}$ とする。
  2. 現在の端点の位置 $\vec{p}$ からの変位量 $\vec{d} = \vec{p}_{\rm G}-\vec{p}$ から微小変位量を求める。$\delta \vec{p}=\alpha\;\vec{d}/|\vec{d}|$。ここで $\alpha$ は適当な微小量である。
  3. 現在の $\vec{\theta}$ を使って $J^{\#}$ を計算する。
  4. $\delta \vec{\theta}=J^{\#} \delta\vec{p}$ を計算する。
  5. $\vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}$
  6. 更新後の変位量 $|\vec{d}| = |\vec{p}_{\rm G}-\vec{p}|$が許容誤差 $\epsilon$ 以下なら計算終了。そうでなければ、2へ戻る。

参考文献

  1. 順運動学
  2. ヤコビアンを用いた逆運動学
  3. ロボットの運動学問題

2013年7月5日金曜日

How to build Boost-1.53.0 by Apple LLVM version 4.2 comiler

in Japanese

First install the Command Line Tools of Xcode-4.6.1 and check the version. Download and decompress the boost-1.53.0. Run the following commands: When using it on the Xcode, we need to add a variable DYLD_LIBRARY to "Product→Edit Scheme→Arguments→Environment Variables" and set the path to dylib to the variable.

How to build Opencv 2.4.5 by Apple LLVM compiler 4.2

in Japanese

Download and decompress the source code. Run the following commands: That's all.

2013年7月4日木曜日

Computational Geometry
 〜 Theory and Practice on the 2D Convex Hull 〜

in Japanese

Introduction

 In this page, I describe a brief explanation on the algorithm of the 2-dimensional convex hull upon which I touched in the previous page. I also show you my implementation of it in C++.

Algorithm

 The algorithm discussed here is called the Incremental Method which has the complexity of $O(n\log{n})$ 1.

Input: A sequence of points, $A=\{P_1,P_2,\cdots,P_n\}$ (We call the suffixes the original numbers)
Output: Convex Hull, $C(A)$
  1. Arrange points in ascending order of $x$-coordinate values. We write the resultant sequence as $A^{\prime}=\{P_{(1)},P_{(2)},\cdots,P_{(n)}\}$ and call the suffixes the sorted numbers. When two points have the same $x$-coordinate value, we arrange them in descending order of the original numbers. We store the table $T_{{\rm sorted}\rightarrow{\rm original}}$ that maps the sorted number to the original one.
  2. Make a triangle consisting of the first three points of $A^{\prime}$ and write it as $C(\{P_{(1)},P_{(2)},P_{(3)}\})$. Store two arrays $V_{\rm clockwise}$ and $V_{\rm counterclockwise}$ that allow us to follow points in clockwise order and counterclockwise order, respectively. For instance, if the sorted number $a$ is followed by $b$ when we trace the points in clockwise order, $V_{\rm clockwise}(a)=b$ holds.
  3. Add points sequentially from $(3)$ to $(n)$. Every time a point is inserted, the algorithm $X$ that is discussed later is executed.
  4. Finally we obtain the output $C(\{P_{(1)},P_{(2)},\cdots,P_{(n)}\})$. The sequence of points obtained by $V_{\rm clockwise}$ or $V_{\rm counterclockwise}$ provides the convex hull to us.
 The symbolic perturbation method discussed in the previous page is used in the above algorithm. We add the perturbation to each point as follows: \begin{eqnarray} (x_i,y_i)&\rightarrow&(x_i+\epsilon^{M^i},y_i+\epsilon^{M^{n+i}}) \nonumber \\ &\equiv&(x_i^{\ast},y_i^{\ast}) \end{eqnarray} where $n$ is the total number of points, $M$ is a large integer, and $\epsilon$ is a minutely small quantity. By selecting the original number as the suffix $i$, even though $x$-coordinate values coincides with each other, we can sequentially add points in virtual ascending order of $x$-coordinate values. For example, we suppose that there are three points that have the same $x$-coordinate values shown in the below. In the figure, the number enclosed in parentheses indicates the sorted number, and the number without them is the original number. Since the points with the same $x$-coordinate values are arranged in descending order of the original numbers, the smaller the sorted number is, the smaller the perturbation is. The points after being perturbed are colored in red and the magnitude of the perturbation in blue in the figure.

 Let us turn to the algorithm $X$.

Input:$C(\{P_{(1)},P_{(2)}, \cdots,P_{(k)}\})$, $P_{(k+1)}$, $V_{\rm clockwise}$, $V_{\rm counterclockwise}$, $T_{{\rm sorted}\rightarrow{\rm original}}$
Output:$C(\{P_{(1)},P_{(2)},\cdots,P_{(k+1)}\})$, $V_{\rm clockwise}$, $V_{\rm counterclockwise}$
  1. $P_{(s)}\leftarrow P_{(k)}$
  2. Consider three points, $P_{(k+1)}$, $P_{(s)}$, and $P_{(t)}$, where $t=V_{\rm counterclockwise}(s)$.
  3. Trace these points in this order. If the path turns right at $P_{(s)}$, return to step 2 after setting $s\leftarrow t$. Otherwise return to step 4. See the figure below.
  4. Store the sorted number $(u)$ of the point at which the path turns left.
  5. $P_{(s)}\leftarrow P_{(k)}$
  6. Consider three points, $P_{(k+1)}$, $P_{(s)}$, and $P_{(t)}$, where $t=V_{\rm clockwise}(s)$.
  7. Trace these points in this order. If the path turns left at $P_{(s)}$, return to step 6 after setting $s\leftarrow t$. Otherwise return to step 8.
  8. Store the sorted number $(l)$ of the point at which the path turns right.
  9. Set $V_{\rm counterclockwise}(l)=k+1$ and $V_{\rm counterclockwise}(k+1)=u$.
  10. Set $V_{\rm clockwise}(u)=k+1$ and $V_{\rm clockwise}(k+1)=l$.

 In order to determine if the path turns left or right, the determinant $F$ as shown in the previous page is used.
  1. Consider $F(P_{(i)},P_{(j)},P_{(k)})$.
  2. Convert the sorted numbers to the original ones using $T_{{\rm sorted}\rightarrow{\rm original}}$. \begin{eqnarray} a&=&T_{{\rm sorted}\rightarrow{\rm original}}(i) \nonumber \\ b&=&T_{{\rm sorted}\rightarrow{\rm original}}(j) \nonumber \\ c&=&T_{{\rm sorted}\rightarrow{\rm original}}(k) \nonumber \end{eqnarray}
  3. Arrange the elements of the list $L=(a,b,c)$ in descending order to generate a new list $L^{\prime}=(a^{\prime},b^{\prime},c^{\prime})$. If the conversion is done with an odd permutation, $p=-1$. Otherwise $p=1$.
  4. Calculate the value of $F(P_{a^{\prime}},P_{b^{\prime}},P_{c^{\prime}})$. Multiply its sign by $p$ to obtain the sign of $F(P_{a},P_{b},P_{c})$. If it is positive, the path turns left, otherwise turns right.

Implementation in C++

 Here is my source code.The function main is as follows:
  1. the 16th-39th lines: The input points are randomly generated.
  2. the 43th line: An object of the template class ConvexHull2d is created. In this sample, the type of int is passed to the template argument. It is also possible to pass the type of boost::multiprecision::cpp_int to the template argument.
  3. the 44th line: The algorithm is executed.
  4. after the 46th line: The input points and the convex hull are drawn.
  5. the 57th line: The sequence of points which constitutes the convex hull is extracted.

Demos

Development Environment

  1. Mac OS X 10.8.4
  2. Processor:3.06 GHz Intel Core 2 Duo
  3. Memory:4GB
  4. Xcode4.6.2 with Apple LLVM 4.2(C++ Language Dialect → C++11, C++ Standard Library → libc++)
  5. boost-1.53.0 built by Apple LLVM 4.2 (See here)
  6. opencv-2.4.5 built by Apple LLVM 4.2 (See here)

Reference

  1. 計算幾何プログラミング, 杉原厚吉, 岩波書店 (in Japanese)