作成日

2018/11/06

最終更新

2023/11/17

記事区分

一般公開

インストール

依存パッケージ

ビルドツール関連

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]

その他

jun

Pythonでデータ解析と自動化ツールを開発しています。DjangoでのWeb開発も得意です！

フォロー

記事の執筆者にステッカーを贈る

有益な情報に対するお礼として、またはコメント欄における質問への返答に対するお礼として、記事の読者は、執筆者に有料のステッカーを贈ることができます。

>>さらに詳しくステッカーを贈る

Feedbacks

ログインするとコメントを投稿できます。

ログインする

OpenRAVE IKFast で IK を解くサンプル
産業用ロボットには、垂直多関節ロボット (Articulated) や[水平多関節ロボット (SCARA)](https://ja.wikipedia.org/wiki/%E6%B0%B4%E5%B9%B3%E5%A4%9A%E9%96%A2%E7%AF%80%E3%8
AWS Ninja2/4/2024に更新
0
- OpenRAVE
- Robotics
Python コードスニペット (条件分岐)
if-elif-else sample.py #!/usr/bin/python # -*- coding: utf-8 -*- # コメント内であっても、ASCII外の文字が含まれる場合はエンコーディング情報が必須 x = 1 # 一行スタイル if x==0: print 'a' # 参考: and,or,notが使用可能 (&&,||はエラー) elif x==1: p...
coderina10/25/2015に更新
0
- Python
Python コードスニペット (リスト、タプル、ディクショナリ)
リスト range 「0から10まで」といった範囲をリスト形式で生成します。 sample.py print range(10) # for(int i=0; i<10; ++i) ← C言語などのfor文と比較 print range(5,10) # for(int i=5; i<10; ++i) print range(5,10,2) # for(int i=5; i<10;...
coderina9/28/2020に更新
0
- Python
ZeroMQ (zmq) の Python サンプルコード
ZeroMQ を Python から利用する場合のサンプルコードを記載します。 Fixing the World To fix the world, we needed to do two things. One, to solve the general problem of "how to connect any code to any code, anywhere". Two, to wra...
coderina9/7/2021に更新
0
- Python
Matplotlib/SciPy/pandas/NumPy サンプルコード
Python で数学的なことを試すときに利用される Matplotlib/SciPy/pandas/NumPy についてサンプルコードを記載します。 Matplotlib SciPy pandas [NumPy](https://www.numpy
パンダ12/26/2024に更新
0