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