2013年9月15日日曜日

Kinematics 〜 Implementation 〜

in Japanese


Introduction


 In the previous page, I described a brief explanation on the kinematics. Here I show you my implementation of the algorithm of a six-axis-arm system in C++.

Demo

video
A green sphere indicates a target position. When the end point of the arm arrives at it, the target is moved to the next position.

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.54.0(built by Apple LLVM 4.2 compiler)

Source


Here is my source code.


Usage


A file robot.dat passed as an argument determines a form of the arm system. The format of the file is expressed in 1. Using keyboard,
  1. 'c' starts the program,
  2. 'b' inverts the movement of the target sphere,
  3. 'a' restores the movement,
  4. and escape stops the program.
The end point of the arm system gradually gives rise to a distortion from the target point. We have to introduce the feedback function.

References

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

Kinematics 〜 Theory 〜

in Japanese


Introduction


 In this page, I describe a brief explanation on the kinematics used in the robotics. See also the next page as to the implementation of a simple six-axis-arm system in C++.

Forward Kinematics


 Let us consider the 3-dimensional homogeneous coordinate system. In the system, the rotation matrices around x, y, and z axises are given by \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} \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} and \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} Moreover, the translation matrix $L(\vec{l})$ with a directional vector $\vec{l}$ is obtained by \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} Suppose that we have an initial state of a three-axis-arm system shown in the below figure.
In the figure, $n_i$ and $l_i$ indicate a joint and a length of each component, respectively. $n_1$ rotates $\theta_1$ in counterclockwise direction around $x$ axis, $n_2$ rotates $\theta_2$ in counterclockwise direction around $y$ axis, and $n_3$ rotates $\theta_3$ in counterclockwise direction around $x$ axis. $\vec{l}_1=(0,0,l_1)$, $\vec{l}_2=(0,0,l_2)$, and $\vec{l}_3=(0,0,l_3)$. A position vector of an end point $n_4$ takes the form \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). \label{sample-case} \end{equation} It must be noted that parameters $\theta_i$ and $\vec{l}_i$ are defined in the local coordinate system of the $i$-th joint. We can expand eq.(\ref{sample-case}) to the case of $n$-axis-arm system as \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} where $a_{i}\in \{x,y,z\}$.

Inverse Kinematcis


 Setting lengths to constant values, the vector $\vec{p}$ becomes a function with $n$ angles in the form \begin{equation} \vec{p}= \vec{f}(\theta_1,\theta_2,\cdots,\theta_n). \end{equation} Its deviation with respect to $\{\theta_1,\theta_2,\cdots,\theta_n\}$ is written as \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} Ignoring the terms more than first derivative, we can obtain the form \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} where $J$ is called the Jacobian matrix. In this case, it is $3 \times n$ matrix. A partial differential $\frac{\partial \vec{f}}{\partial \theta_i}$ is calculated as \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} By introducing the inverse matrix $J^{-1}$, we can obtain the deviation vector $\delta \vec{\theta}$ of angles as \begin{equation} \delta \vec{\theta} = J^{-1}\;\delta\vec{p}. \end{equation} When ${\rm rank}(J)>3$, the inverse matrix $J^{-1}$ is not uniquely determined. In other words, in a case where the number of joints are greater than 3, the end position ($\vec{p}$) is achieved by different postures. The typical method to determine a unique posture is to use a pseudoinverse matrix $J^{\#}$ as \begin{equation} \delta \vec{\theta} = J^{\#} \delta\vec{p}, \end{equation} where \begin{equation} J^{\#}=J^{T}\;(J\;J^{T})^{-1}. \end{equation} The procedure to solve the inverse kinematics problem is as follows:
  1. Suppose that a target position of the end point is $\vec{p}_{\rm G}$.
  2. Calculate a displacement $\vec{d} = \vec{p}_{\rm G}-\vec{p}$ where $\vec{p}$ indicates the current positoin of the end point. Using $\vec{d}$, we make a small deviation $\delta \vec{p}=\alpha\;\vec{d}/|\vec{d}|$ where $\alpha$ is an appropriate positive value.
  3. Using current angles, we calculate $J^{\#}$.
  4. Calculate $\delta \vec{\theta}=J^{\#} \delta\vec{p}$.
  5. Update the quantities, $\vec{\theta} \leftarrow \vec{\theta} + \delta \vec{\theta}, \vec{p} \leftarrow \vec{p} + \delta \vec{p}$.
  6. After updating them, if the displacement $|\vec{d}| = |\vec{p}_{\rm G}-\vec{p}|$ is not in an acceptable error range $\epsilon$, return to 2.

References

  1. Forward Kinematics (in Japanese)
  2. Inverse Kinematics by means of Jacobian (in Japanese)
  3. Kinematics Problem of Robot (in Japanese)

2013年9月7日土曜日

運動学 〜 実装 〜

in English


はじめに


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

デモ

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

開発環境

  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版 三浦憲二郎 朝倉書店