ロボットアプリケーションの開発環境の一つ 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)

クォータニオン

同様の回転処理を四元数 Quaternionsを用いて行うと以下のようになります。

from openravepy import Environment, poseFromMatrix, quatFromAxisAngle, quatRotate, quatMult, poseMult
from numpy import pi, r_

# 環境への物体の読み込み
env = Environment()
env.SetViewer('qtcoin')
body = env.ReadKinBodyXMLFile(filename='data/mug1.dae')
env.AddKinBody(body)
body.SetTransform([1,0,0,0,0,0,0])

# XYZ 方向の矢印
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 軸周りに回転
pose = poseFromMatrix(body.GetTransform())
deg = 45
rot_quat = quatFromAxisAngle([1,0,0], float(deg)*pi/180.0)
pose[0:4] = quatMult(rot_quat, pose[0:4])
body.SetTransform(pose)
P1 = quatRotate(rot_quat, [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_quat = quatFromAxisAngle([0,1,0], float(deg)*pi/180.0)
pose[0:4] = quatMult(rot_quat, pose[0:4])
body.SetTransform(pose)
P2 = quatRotate(rot_quat, P1)
env.drawarrow([0.0,0.0,0.0], P2, linewidth=0.01, color=[1.0,1.0,0.0])

# 回転しながら前進
from time import sleep
while True:
    posedelta = r_[quatFromAxisAngle([0, 0.5, 0]), 0, 0.01, 0]
    pose = poseMult(pose, posedelta)
    body.SetTransform(pose)
    sleep(1.0)

オブジェクトのサイズを確認

AABBを利用してオブジェクトのサイズを確認できます。

body = env.GetKinBody('mug2')
aabb = body.ComputeAABB()
aabb.extents()

=> array([0.1191    , 0.0244    , 0.02412521]) # x=0.1191*2[m], y=0.0244*2[m], z=0.02412521*2[m]

その他

• Command Line Tools
• Quick examples in openravepy
• Beginning with openravepy