モーダルを閉じる工作HardwareHub ロゴ画像

工作HardwareHubは、ロボット工作や電子工作に関する情報やモノが行き交うコミュニティサイトです。さらに詳しく

利用規約プライバシーポリシー に同意したうえでログインしてください。

目次目次を開く/閉じる

OpenRAVE 環境構築および簡単な使い方

モーダルを閉じる

ステッカーを選択してください

お支払い手続きへ
モーダルを閉じる

お支払い内容をご確認ください

購入商品
」ステッカーの表示権
メッセージ
料金
(税込)
決済方法
GooglePayマーク
決済プラットフォーム
確認事項

利用規約をご確認のうえお支払いください

※カード情報はGoogleアカウント内に保存されます。本サイトやStripeには保存されません

※記事の執筆者は購入者のユーザー名を知ることができます

※購入後のキャンセルはできません

作成日作成日
2018/11/06
最終更新最終更新
2023/11/17
記事区分記事区分
一般公開

目次

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

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

    その他

    Likeボタン(off)0
    詳細設定を開く/閉じる
    アカウント プロフィール画像

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

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

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

    >>さらに詳しくステッカーを贈る
    ステッカーを贈る コンセプト画像

    Feedbacks

    Feedbacks コンセプト画像

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

      ログインする

      関連記事