OpenRAVE 環境構築および簡単な使い方
[History] [Last Modified] (2019/04/11 03:32:28)
Recent posts
What is this site?
A platform for makers to share their knowledge.

Share your robots/products with others.
New robots/products

概要

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

Uploaded Image

openrave.py --example graspplanning

Uploaded Image

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

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

Uploaded Image

このように、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'))

Uploaded Image

青色 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])

Uploaded Image

同次変換行列における 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)

Uploaded Image

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

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

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

Uploaded Image

回転

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

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])

Uploaded Image

物体を回転させるためには同次変換行列の回転行列部分を更新して再設定します。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])

Uploaded Image

更に 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])

Uploaded Image

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)

Uploaded Image

Uploaded Image

クォータニオン

同様の回転処理を四元数 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]

その他

Related pages
    概要 TODO OpenGL で OpenRAVE 環境からポイントクラウド情報を生成します。
    概要 ROS による開発で必要となる 3D モデルは STL や COLLADA データとして作成できます。FreeCAD と Blender を用いて直方体のデータを作成してみます。いずれの場合も、ROS 等で扱うために単位はメートルで出力します。 FreeCAD による直方体 STL の作成 STL はフェイスに三角形を利用した 3D モデル
    概要 ロボットシミュレータ Gazebo を ROS 開発で利用するための簡単な手順を記載します。ここでは Debian9 を利用します。個別にインストールすることもできますが、ROS を ros-melodic-desktop-full でインストールすれば gazebo もインストールされます。 URDF モデルの作成