概要

Open Dynamics Engine (ODE) を用いた動力学シミュレーションを行います。ここでは ode-python を利用して Python から実行します。

インストール

以下は Linux 環境の例です。

ODE ビルド

ODE に付属する DrawStuff をビルドするためには OpenGL と GLUT が必要です。

sudo apt install -y libgl1-mesa-dev libglu1-mesa-dev

cmake を利用してビルドします。

sudo apt install -y cmake

ソースコードをこちらからダウンロードします。動的ライブラリとしてビルドします。

mv ode-0.16.1.tar.gz ode-0.16.1.tar
tar xvf ode-0.16.1.tar
mkdir -p mybuild && cd mybuild
cmake ../ode-0.16.1 -DBUILD_SHARED_LIBS=ON -DODE_WITH_DEMOS=ON
make

DrawStuff は make install でインストールできないため手動でコピーしてインストールします。

sudo cp libdrawstuff.so /usr/local/lib/
sudo cp -d libode.so.0.16.1 libode.so /usr/local/lib/

DrawStuff 利用時に必要になるためテクスチャファイルも適当な場所に保存しておきます。

sudo cp -r ode-0.16.1/drawstuff/textures /usr/local/share/ode-0.16.1-drawstuff-textures
sudo chmod -R a+x /usr/local/share/ode-0.16.1-drawstuff-textures

ode-python のインストール

python -m pip install ode-python

ボールの自由落下

ode-python では ctypes によるバインディングが行われています。ポインタ渡しでは byref を利用します。

#!/usr/bin/python
# -*- coding: utf-8 -*-

import odepy
import drawstuffpy as ds
import ctypes
import random

def Main():
    # ODE では動力学計算用の world と
    # 衝突計算用の space を利用します。
    world, space, ground, contactgroup = CreateWorld()

    geoms = [AddBall(world, space)['geom']]
    geomColors = [[random.random(), random.random(), random.random()]]
    tDelta = 0.01

    # 衝突が予想されるタイミングで呼び出される関数です。
    def __NearCallback(data, o1, o2):
        o1IsGround = ctypes.addressof(ground.contents) == ctypes.addressof(o1.contents)
        o2IsGround = ctypes.addressof(ground.contents) == ctypes.addressof(o2.contents)
        if not (o1IsGround or o2IsGround):
            return
        N = 10
        contacts = (odepy.dContact * N)()
        n = odepy.dCollide(o1, o2, N, ctypes.byref(contacts[0].geom), ctypes.sizeof(odepy.dContact))
        for i in range(n):
            contact = contacts[i]
            contact.surface.mu = float('inf')
            contact.surface.mode = odepy.dContactBounce
            contact.surface.bounce = 0.95
            contact.surface.bounce_vel = 0.0
            c = odepy.dJointCreateContact(world, contactgroup, ctypes.byref(contact))
            odepy.dJointAttach(c, odepy.dGeomGetBody(contact.geom.g1), odepy.dGeomGetBody(contact.geom.g2))

    # tDelta で world の時間が進む毎に呼び出される関数です。
    def __StepCallback(pause):
        if pause != 1:
            odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
            odepy.dWorldStep(world, tDelta)
            odepy.dJointGroupEmpty(contactgroup)
        for geom, color in zip(geoms, geomColors):
            body = odepy.dGeomGetBody(geom)
            ds.dsSetColor(*color)
            if odepy.dGeomGetClass(geom) == odepy.dSphereClass:
                r = odepy.dGeomSphereGetRadius(geom)
                pos = odepy.dBodyGetPosition(body)
                rot = odepy.dBodyGetRotation(body)
                ds.dsDrawSphereD(pos, rot, r)
            elif odepy.dGeomGetClass(geom) == odepy.dCapsuleClass:
                r = odepy.dReal()
                l = odepy.dReal()
                odepy.dGeomCapsuleGetParams(geom, ctypes.byref(r), ctypes.byref(l))
                pos = odepy.dBodyGetPosition(body)
                rot = odepy.dBodyGetRotation(body)
                ds.dsDrawCapsuleD(pos, rot, l.value, r.value)
            else:
                raise Exception('Not supported geom class: {}'.format(odepy.dGeomGetClass(geom)))

    # シミュレーションを開始します。
    RunDrawStuff(__StepCallback)
    # ODE を終了するための手続きを行います。
    DestroyWorld(world, space)

def AddBall(world, space, m=1.0, r=0.1, pos=[0, 0, 2.0]):
    mass = odepy.dMass()
    odepy.dMassSetZero(ctypes.byref(mass))
    odepy.dMassSetSphereTotal(ctypes.byref(mass), m, r)
    body = odepy.dBodyCreate(world)
    odepy.dBodySetMass(body, ctypes.byref(mass))
    geom = odepy.dCreateSphere(space, r)
    odepy.dGeomSetBody(geom, body)
    # body は world に属します。geom は space に属します。
    ball = {'body': body, 'geom': geom}
    odepy.dBodySetPosition(ball['body'], *pos)
    return ball

def RunDrawStuff(stepCallback, pathToTextures='/usr/local/share/ode-0.16.1-drawstuff-textures', w=400, h=225, cameraXyz=[3.0, 0.0, 1.0], cameraHpr=[-180.0, 0.0, 0.0]):
    def __StartCallback():
        xyz = (ctypes.c_float * 3)()
        hpr = (ctypes.c_float * 3)()
        for i, v in enumerate(cameraXyz):
            xyz[i] = v
        for i, v in enumerate(cameraHpr):
            hpr[i] = v
        ds.dsSetViewpoint(xyz, hpr)
    fn = ds.dsFunctions()
    fn.version = ds.DS_VERSION
    fn.start = ds.dsStartCallback(__StartCallback)
    fn.step = ds.dsStepCallback(stepCallback)
    fn.path_to_textures = ctypes.create_string_buffer(pathToTextures.encode('utf-8'))
    ds.dsSimulationLoop(0, ctypes.byref(ctypes.POINTER(ctypes.c_char)()), w, h, fn)

def CreateWorld():
    odepy.dInitODE()
    world = odepy.dWorldCreate()
    odepy.dWorldSetGravity(world, 0, 0, -9.8)
    space = odepy.dHashSpaceCreate(0)
    ground = odepy.dCreatePlane(space, 0, 0, 1, 0)
    contactgroup = odepy.dJointGroupCreate(0)
    return world, space, ground, contactgroup

def DestroyWorld(world, space):
    odepy.dSpaceDestroy(space)
    odepy.dWorldDestroy(world)
    odepy.dCloseODE()

if __name__ == '__main__':
    Main()

Open Dynamics Engine (ODE) のジョイント操作 (Python)

概要 こちらのページでインストールした Open Dynamics Engine (ODE) のジョイント操作について記載します。 ヒンジジョイント ロボットにはジョイントとリンクがあります。ODE に実装されているジョイントの一つにヒンジジョイントがあります。以下の例では二つの立方体をヒンジジョイントで結合して、ヒンジジョイントに角速度を与えています。
Open Dynamics Engine (ODE) ジョイント API のサンプルコード (Python)

概要 こちらのページでインストールした Open Dynamics Engine (ODE) のジョイントについて、関連する API のサンプルコードを記載します。 静的な仮想物体とのジョイント結合 0 を指定すると静的な仮想物体と結合されます。 odepy.dJointAttach(joint, body, 0)
Open Dynamics Engine (ODE) ボディ API のサンプルコード (Python)

概要 こちらのページでインストールした Open Dynamics Engine (ODE) について、ボディ操作に関する API のサンプルコードを記載します。 位置姿勢の設定 位置 odepy.dBodySetPosition(body, 0.0, 0.0, 0.0) 姿勢 (回転行列) R = odepy.dMatrix3() odepy.dRSetIdentity(R) odepy
Open Dynamics Engine によるロボットの自己位置の推定 (Python)

概要 ロボットアプリケーションを開発する際に、ロボットの自己位置を推定する必要がある場合を考えます。ここでは Open Dynamics Engine を Python から利用した場合について、自己位置推定のサンプルを記載します。自己位置推定と環境の地図作成を同時に行う場合を SLAM (Simultaneous Localization and Mapping) とよびます。
PID 制御による全方向移動ロボットの位置制御 (ODE、Python)

概要 Open Dynamics Engine (ODE) を用いて、全方向移動ロボットの位置制御を PID 制御で行う場合のサンプルを記載します。こちらのページに記載の差分駆動型ロボットと比較して、全方向移動ロボットは任意の方向に移動できるため位置制御が容易です。 モータの角速度を操作することでロボットの位置を制御
ロボットアームの先端の位置姿勢 (Open Dynamics Engine、Python)

概要 ロボットアームは複数のリンクがジョイントで結合されており、先端にマニピュレータが存在します。Open Dynamics Engine (ODE) を用いたシミュレーションを行い、ジョイントの関節値とマニピュレータの位置姿勢の関係を把握します。 順運動学によるマニピュレータの位置姿勢の計算 回転行列と回転ベクトルの変換のために以下のパッケージを利用しています。