概要

ロボットアプリケーションの開発環境の一つ OpenRAVE (Open Robotics Automation Virtual Environment) の環境を構築するための手順を記載します。ここでは特に Debian9 を利用します。

インストール

依存パッケージ

ビルドツール関連

sudo apt install git build-essential cmake

Python モジュール

sudo apt install python-ipython python-dev python-numpy python-scipy python-sympy python-h5py

Boost 関連

sudo apt install libboost-all-dev libboost-date-time-dev

Qt 関連

sudo apt install qt4-dev-tools libqt4-dev libsoqt-dev-common libsoqt4-dev

3D モデル関連、その他ライブラリ

sudo apt install minizip rapidjson-dev libassimp-dev libavcodec-dev libavformat-dev libbullet-dev libcairo2-dev libccd-dev libfaad-dev libglew-dev libgsm1-dev liblapack-dev liblog4cxx-dev libminizip-dev libmpfr-dev libode-dev libogg-dev libpcre3-dev libpcrecpp0v5 libpoppler-glib-dev libqhull-dev libsdl2-dev libswscale-dev libtiff5-dev libvorbis-dev libx264-dev libxml2-dev libxrandr-dev libxvidcore-dev

collada-dom

COLLADA ファイルを C++ から扱うためのライブラリをインストールします。Git tag v2.5.0 をチェックアウトします。

git clone https://github.com/rdiankov/collada-dom.git
cd collada-dom
git checkout v2.5.0

CMake でビルドします。

mkdir build && cd build
cmake ..
make
sudo make install

以下のような場所にインストールされます。

ls -ltr /usr/local/include/collada-dom2.5/
ls -ltr /usr/local/lib/

OpenSceneGraph (OSG)

git clone https://github.com/openscenegraph/OpenSceneGraph.git
cd OpenSceneGraph
mkdir build && cd build
cmake .. -DDESIRED_QT_VERSION=4
make
sudo make install

以下のような場所にインストールされます。

ls -ltr /usr/local/lib64/
ls -ltr /usr/local/include/osg*

Flexible Collision Library (fcl)

git clone https://github.com/flexible-collision-library/fcl.git
cd fcl
git checkout 0.5.0
mkdir build && cd build
cmake ..
make
sudo make install

以下のような場所にインストールされます。

ls -ltr /usr/local/include/fcl
ls -ltr /usr/local/lib/

OpenRAVE

ソースコードのダウンロード

git clone https://github.com/rdiankov/openrave.git
git checkout master

本ページで利用するハッシュは以下のとおりです。

$ git log --oneline -1
7c5f5e27 Merge pull request #557 from rdiankov/verification

CMake でビルドします。

mkdir build && cd build
cmake .. -DOSG_DIR=/usr/local/lib64/
make
sudo make install

以下のような場所にインストールされます。

ls -ltr /usr/local/include/openrave-0.9/
ls -ltr /usr/local/share/openrave-0.9/
ls -ltr /usr/local/lib/
ls -ltr /usr/local/lib/python2.7/dist-packages/openravepy/
ls -ltr /usr/local/lib/openrave0.9-plugins/
ls -ltr /usr/local/bin/

インストールされた実行ファイルは以下のとおりです。

openrave-config
openrave
openrave.py
openrave-robot.py
openrave-createplugin.py

openrave-config を用いて so ファイルと python モジュールへのパスを追加しておきます。

echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(openrave-config --python-dir)/openravepy/_openravepy_
export PYTHONPATH=$PYTHONPATH:$(openrave-config --python-dir)' >> ~/.bashrc
source ~/.bashrc

以下のようなサンプルを実行できることを確認します。

openrave.py --example hanoi

openrave.py --example graspplanning

サンプル名の一覧は以下のコマンドで取得できます。

openrave.py --listexamples

サンプルコード

COLLADA ファイルの読み込み

OpenRAVE で読み込めるファイル形式の一つに COLLADA (.dae) があります。ただし、バージョンは 1.5 であることが必要です。ソースコードからビルドしてインストールした際に作成されたディレクトリに v1.5 の dae ファイルが格納されています。

$ ls /usr/local/share/openrave-0.9/data/*.dae
/usr/local/share/openrave-0.9/data/mug1.dae
$ grep 'version' /usr/local/share/openrave-0.9/data/mug1.dae
<?xml version="1.0" encoding="UTF-8"?>
<COLLADA xmlns="http://www.collada.org/2008/03/COLLADASchema" version="1.5.0">

これを読み込むためには以下のようにします。

from openravepy import Environment, with_destroy
from numpy import eye
env = Environment()
env.SetViewer('qtcoin')
body = env.ReadKinBodyXMLFile(filename='data/mug1.dae')
env.AddKinBody(body)
body.SetTransform(eye(4))

SetTransform() では同次変換行列を指定します。ワールド座標系に対して、物体の座標系における各メッシュの頂点座標を回転も平行移動もさせない、ということになります。

このように、OpenRAVE では環境 env に物体の body を追加していきます。

In [9]: env.GetBodies()
Out[9]: []

In [10]: env.AddKinBody(body)

In [11]: env.GetBodies()
Out[11]: [RaveGetEnvironment(1).GetKinBody('mug')]

In [12]: env.Reset()

In [13]: env.GetBodies()
Out[13]: []

最初から物体を環境に読み込んだ状態で起動することもできます。

openrave.py -i 'data/mug1.dae'
In [1]: env.GetBodies()
Out[1]: [RaveGetEnvironment(1).GetKinBody('mug')]

平行移動

環境に物体 body を追加します。以下で利用しているのは COLLADA ファイルではなく OpenRAVE Custom XML Format で記述されたファイルです。

from openravepy import Environment, poseFromMatrix, with_destroy
from numpy import eye

env = Environment()
env.SetViewer('qtcoin')
env.AddKinBody(env.ReadKinBodyXMLFile(filename='data/mug1.kinbody.xml'))
env.AddKinBody(env.ReadKinBodyXMLFile(filename='data/mug2.kinbody.xml'))

青色 mug1 の KinBody には RotationAxis が設定されています。物体メッシュの各頂点座標をワールド座標系における X 軸周りに 90 度回転させた状態が初期位置となります。

$ cat /usr/local/share/openrave-0.9/data/mug1.kinbody.xml
<KinBody name="mug">
  <RotationAxis>1 0 0 90</RotationAxis>
  <Body name = "mugbase" type="dynamic">
    <Geom type="trimesh">
      <Data>models/objects/blue_mug_y_up.iv</Data>
      <Render>models/objects/blue_mug_y_up.iv</Render>
    </Geom>
  </Body>
</KinBody>

$ cat /usr/local/share/openrave-0.9/data/mug2.kinbody.xml
<KinBody name="mug2">
  <Body type="dynamic">
    <Geom type="trimesh">
      <Data>models/objects/plastic_mug2p.iv</Data>
      <Render>models/objects/plastic_mug2p.iv</Render>
    </Geom>
  </Body>
</KinBody>

同次変換行列 で初期位置に設定すると確かに青色マグカップだけが X 軸周りに回転した状態になります。body を環境から取得する方法には GetBodies() を経由する方法と GetKinBody() で名前指定する方法があります。

bodies = env.GetBodies()
body1 = bodies[0]
body2 = env.GetKinBody('mug2')
body1.SetTransform(eye(4))
body2.SetTransform(eye(4))

四元数 Quaternions 4つと同次変換行列の平行移動要素 3つを合わせて openrave では pose とよびます。この pose を用いて物体の姿勢を設定することもできます。

In [22]: poseFromMatrix(eye(4))
Out[22]: array([1., 0., 0., 0., 0., 0., 0.])

body1.SetTransform([1,0,0,0,0,0,0])
body2.SetTransform([1,0,0,0,0,0,0])

同次変換行列における X 軸方向の平行移動距離を変更して反映してみます。

array([[1. , 0. , 0. , 0.2],
       [0. , 1. , 0. , 0. ],
       [0. , 0. , 1. , 0. ],
       [0. , 0. , 0. , 1. ]])

tran1 = body1.GetTransform()
tran1[0,3] = 0.2
body1.SetTransform(tran1)

更に Y 軸方向の平行移動距離を pose 指定で変更してみます。

array([1. , 0. , 0. , 0. , 0.2, 0.1, 0. ])

pose1 = poseFromMatrix(tran1)
pose1[5] = 0.1
body1.SetTransform(pose1)

回転

環境に物体を追加して姿勢を設定します。

from openravepy import Environment, rotationMatrixFromAxisAngle, matrixFromAxisAngle, with_destroy
from numpy import eye, dot, pi

env = Environment()
env.SetViewer('qtcoin')
body = env.ReadKinBodyXMLFile(filename='data/mug1.dae')
env.AddKinBody(body)
body.SetTransform(eye(4))

物体の回転が分かりやすくなるように、矢印を環境に追加します。p1 から p2 までの線分の太さと RGB を指定します。

env.drawarrow(p1=[0.0,0.0,0.0], p2=[0.5,0.0,0.0], linewidth=0.01, color=[1.0,0.0,0.0])
env.drawarrow(p1=[0.0,0.0,0.0], p2=[0.0,0.5,0.0], linewidth=0.01, color=[0.0,1.0,0.0])
env.drawarrow(p1=[0.0,0.0,0.0], p2=[0.0,0.0,0.5], linewidth=0.01, color=[0.0,0.0,1.0])

物体を回転させるためには同次変換行列の回転行列部分を更新して再設定します。X 軸周りに回転させてみます。

deg = 45
tran = body.GetTransform()
rot_mat = rotationMatrixFromAxisAngle([1,0,0], float(deg)*pi/180.0)
tran[0:3,0:3] = dot(rot_mat, tran[0:3,0:3])
body.SetTransform(tran)

物体と同じ回転を Y 軸方向の矢印にも適用してみます。

P1 = dot(rot_mat, [0,1,0])
env.drawarrow([0.0,0.0,0.0], P1, linewidth=0.01, color=[1.0,1.0,0.0])

更に Y 軸周りに回転させてみます。

deg = 45
rot_mat = rotationMatrixFromAxisAngle([0,1,0], float(deg)*pi/180.0)
tran[0:3,0:3] = dot(rot_mat, tran[0:3,0:3])
body.SetTransform(tran)
P2 = dot(rot_mat, P1)
env.drawarrow([0.0,0.0,0.0], P2, linewidth=0.01, color=[1.0,1.0,0.0])

matrixFromAxisAngle() で Y 軸周りの回転行列を含む同次変換行列を取得して、Y 軸方向の平行移動を 0.01 に設定します。これを繰り返し適用すると Y 軸周りに回転しながら Y 軸方向に進んでいきます。以下のように tran と合わせて繰り返し適用すると、物体が Y 軸方向に進んだ後に tran を最後に一回だけ適用したのと同じ状態となり、「Y 軸を tran で回転させた軸」方向に進んでいくことになります。

from time import sleep
while True:
    Tdelta = matrixFromAxisAngle([0, 0.5, 0])
    Tdelta[1,3] = 0.01
    tran = dot(tran, Tdelta)
    body.SetTransform(tran)
    sleep(1.0)

クォータニオン

Python から C ライブラリを利用 (ctypes)
概要 FFI (Foreign Function Interface) の一つである ctypes を利用すると、C 言語のライブラリを Python から利用できます。サンプルコードを記載します。 適宜参照するための公式ドキュメント libm の sqrt を利用する例 main.py #!/usr/bin/python # -*- coding: utf-8 -*- from c
ロボットシミュレータ Gazebo の簡単な使い方 (ROS)

概要 ロボットシミュレータ Gazebo を ROS 開発で利用するための簡単な手順を記載します。ここでは Debian9 を利用します。個別にインストールすることもできますが、ROS を ros-melodic-desktop-full でインストールすれば gazebo もインストールされます。 URDF モデルの作成
STL ファイルに Blender でテクスチャを適用して COLLADA を作成

概要 ROS による開発で必要となる 3D モデルは STL や COLLADA データとして作成できます。FreeCAD と Blender を用いて直方体のデータを作成してみます。いずれの場合も、ROS 等で扱うために単位はメートルで出力します。 FreeCAD による直方体 STL の作成 STL はフェイスに三角形を利用した 3D モデル
OpenRAVE IKFast で IK を解くサンプル

概要 産業用ロボットには、垂直多関節ロボット (Articulated) や水平多関節ロボット (SCARA) があります。ロボットにはジョイントとリンクがあり、ジョイントの関節値を指定することで制御できます。 関節値を指定して、位置姿勢を求める場合を順運動学 (Forward Kinematics) とよびます。
カメラキャリブレーションについて簡単なまとめ

概要 カメラキャリブレーション (Camera Calibration, Camera Resectioning) を行うと、レンズの歪みを表現するパラメータや、カメラのワールド座標系での位置姿勢を推定できます。 チェスボードのようなキャリブレーション専用のボードが利用されます。 キャリブレーションで得られたパラメータを用いると、例えば歪みを補正することができます。
ロボットアームの逆運動学をヤコビ行列で数値的に解く (Open Dynamics Engine、Python)

概要 ロボットアームのマニピュレータの位置姿勢が与えられたときに、ジョイントの関節値を求める逆運動学について、3自由度の場合を対象として解析解と数値解を用いる場合を記載します。 解析解 3 自由度の場合は解析的に逆運動学を解くことができます。以下は 4 つ存在する解析解のうちの一つを利用しています。詳細はこちらの書籍をご覧ください。