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

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

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

工作HardwareHub ロゴ画像 (Laptop端末利用時)
工作HardwareHub ロゴ画像 (Mobile端末利用時)
目次目次を開く/閉じる

Open Dynamics Engine (ODE) ボディ API のサンプルコード (Python)

モーダルを閉じる

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

モーダルを閉じる

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

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

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

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

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

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

作成日作成日
2020/05/05
最終更新最終更新
2024/11/24
記事区分記事区分
一般公開

目次

    アカウント プロフィール画像 (サイドバー)

    Software Engineer @ Tokyo

    0
    ステッカーを贈るとは?

    Open Dynamics Engine (ODE) について、ボディ操作に関する API のサンプルコードを記載します。

    位置姿勢の設定

    位置

    odepy.dBodySetPosition(body, 0.0, 0.0, 0.0)
    

    姿勢 (回転行列)

    R = odepy.dMatrix3()
    odepy.dRSetIdentity(R)
    odepy.dBodySetRotation(body, R)
    

    姿勢 (クォータニオン)

    q = odepy.dQuaternion()
    odepy.dQfromR(q, R)
    odepy.dBodySetQuaternion(body, q)
    

    回転行列の作成

    単位行列

    odepy.dRSetIdentity(R)
    

    回転軸と回転角度を指定

    odepy.dRFromAxisAndAngle(R, 0, 0, 1, 3.14 / 4.0)
    

    クォータニオンの場合

    odepy.dQSetIdentity(q)
    odepy.dQFromAxisAndAngle(q, 0, 0, 1, 3.14 / 4.0)
    

    相互変換

    odepy.dRfromQ(R, q)
    odepy.dQfromR(q, R)
    

    クォータニオンの乗算

    qa = qb * qc (qc 回転してから qb)

    odepy.dQMultiply0(qa, qb, qc)
    

    qa = -qb * qc (第一引数について、逆クォータニオン)

    odepy.dQMultiply1(qa, qb, qc)
    

    qa = qb * -qc

    odepy.dQMultiply2(qa, qb, qc)
    

    qa = -qb * -qc

    odepy.dQMultiply3(qa, qb, qc)
    

    重力の取得

    gravity = odepy.dVector3()
    odepy.dWorldGetGravity(world, gravity)
    print(gravity[2])
    

    速度の設定

    速度 (m/s)

    odepy.dBodySetLinearVel(body, 0, 1, 0)
    

    角速度 (rad/s)

    odepy.dBodySetAngularVel(body, 0, 0, 1)
    

    odepy.dBodySetForce(body, 0, 100, 0)
    

    トルク

    odepy.dBodySetTorque(body, 0, 0, 10)
    

    ボディの破壊

    odepy.dBodyDestroy(body)
    

    ボディ座標系とワールド座標系の変換

    ボディ座標系 ←→ ワールド座標系 (ベクトル)

    result = odepy.dVector3()
    odepy.dBodyVectorToWorld(body, 0, 0, 1, result)
    odepy.dBodyVectorFromWorld(body, 0, 0, 1, result)
    

    ボディ座標系 → ワールド座標系 (位置と速度)

    odepy.dBodyGetRelPointPos(body, 0, 0, 0, result)
    odepy.dBodyGetRelPointVel(body, 0, 0, 0, result)
    

    ワールド座標系 → ボディ座標系 (位置)

    odepy.dBodyGetPosRelPoint(body, 0, 0, 0, result)
    

    ワールド座標系で指定した点の速度

    odepy.dBodyGetPointVel(body, 0, 0, 0, result)
    

    Sphere ジオメトリ

    ジオメトリとボデイ

    r = 0.5  # 半径 [m]
    m = 1.0  # 質量 [kg]
    geom = odepy.dCreateSphere(space, r)
    
    body = odepy.dBodyCreate(world)
    odepy.dBodySetPosition(body, 0, 0, r)
    
    mass = odepy.dMass()
    odepy.dMassSetZero(ctypes.byref(mass))
    odepy.dMassSetSphereTotal(ctypes.byref(mass), m, r)
    
    odepy.dGeomSetBody(geom, body)
    odepy.dBodySetMass(body, ctypes.byref(mass))
    

    描画

    r = odepy.dGeomSphereGetRadius(geom)
    body = odepy.dGeomGetBody(geom)
    pos = odepy.dBodyGetPosition(body)
    rot = odepy.dBodyGetRotation(body)
    ds.dsDrawSphereD(pos, rot, r)
    

    ワールド座標の点の、Sphere 表面からの距離情報

    depth = odepy.dGeomSpherePointDepth(geom, 0, 0, 0)
    

    Box ジオメトリ

    ジオメトリとボデイ

    lx, ly, lz = 0.5, 0.5, 0.5
    m = 1.0
    geom = odepy.dCreateBox(space, lx, ly, lz)
    
    body = odepy.dBodyCreate(world)
    odepy.dBodySetPosition(body, 0, 0, lz / 2.0)
    
    mass = odepy.dMass()
    odepy.dMassSetZero(ctypes.byref(mass))
    odepy.dMassSetBoxTotal(ctypes.byref(mass), m, lx, ly, lz)
    
    odepy.dGeomSetBody(geom, body)
    odepy.dBodySetMass(body, ctypes.byref(mass))
    

    描画

    lengths = odepy.dVector3()
    odepy.dGeomBoxGetLengths(geom, lengths)
    body = odepy.dGeomGetBody(geom)
    pos = odepy.dBodyGetPosition(body)
    rot = odepy.dBodyGetRotation(body)
    ds.dsDrawBoxD(pos, rot, lengths)
    

    ワールド座標の点の、Box 表面からの距離情報

    depth = odepy.dGeomBoxPointDepth(geom, 0, 0, 0)
    

    Capsule ジオメトリ

    ジオメトリとボデイ

    r = 0.2
    l = 1.0
    m = 1.0
    direction = 3  # z 軸方向
    geom = odepy.dCreateCapsule(space, r, l)
    
    body = odepy.dBodyCreate(world)
    odepy.dBodySetPosition(body, 0, 0, l / 2.0 + r)
    
    mass = odepy.dMass()
    odepy.dMassSetZero(ctypes.byref(mass))
    odepy.dMassSetCapsuleTotal(ctypes.byref(mass), m, direction, r, l)
    
    odepy.dGeomSetBody(geom, body)
    odepy.dBodySetMass(body, ctypes.byref(mass))
    

    描画

    r = odepy.dReal()
    l = odepy.dReal()
    odepy.dGeomCapsuleGetParams(geom, ctypes.byref(r), ctypes.byref(l))
    body = odepy.dGeomGetBody(geom)
    pos = odepy.dBodyGetPosition(body)
    rot = odepy.dBodyGetRotation(body)
    ds.dsDrawCapsuleD(pos, rot, l.value, r.value)
    

    Cylinder ジオメトリ

    ジオメトリとボデイ

    r = 0.2
    l = 1.0
    m = 1.0
    direction = 3  # z 軸方向
    geom = odepy.dCreateCylinder(space, r, l)
    
    body = odepy.dBodyCreate(world)
    odepy.dBodySetPosition(body, 0, 0, l / 2.0)
    
    mass = odepy.dMass()
    odepy.dMassSetZero(ctypes.byref(mass))
    odepy.dMassSetCylinderTotal(ctypes.byref(mass), m, direction, r, l)
    
    odepy.dGeomSetBody(geom, body)
    odepy.dBodySetMass(body, ctypes.byref(mass))
    

    描画

    r = odepy.dReal()
    l = odepy.dReal()
    odepy.dGeomCylinderGetParams(geom, ctypes.byref(r), ctypes.byref(l))
    body = odepy.dGeomGetBody(geom)
    pos = odepy.dBodyGetPosition(body)
    rot = odepy.dBodyGetRotation(body)
    ds.dsDrawCylinderD(pos, rot, l.value, r.value)
    

    バウンディングボックスの取得

    ジオメトリの AABB] は以下のように取得できます。

    aabb = (odepy.dReal * 6)()
    odepy.dGeomGetAABB(geom, aabb)
    minx, maxx, miny, maxy, minz, maxz = [aabb[i] for i in range(6)]
    

    メッシュファイルの読み込み

    任意の形状のポリゴンデータを利用することもできます。以下の例では、こちらのサイトで販売されている部品の STL ファイルをダウンロードして利用しています。

    python -m pip install numpy-stl
    

    sample.py

    #!/usr/bin/python
    # -*- coding: utf-8 -*-
    
    import odepy
    import drawstuffpy as ds
    import ctypes
    import numpy as np
    
    from stl.mesh import Mesh
    
    def Main():
        world, space, ground, contactgroup = CreateWorld()
    
        # STL ファイルの読み込み
        data = Mesh.from_file('STHRB3.stl').points.flatten()
        dataSize = len(data)
    
        # 今回は簡単のため STL に含まれる頂点情報を、すべて重複を考えずにそのまま使用します。
        vertices = (ctypes.c_double * dataSize)()
        indices = (ctypes.c_int32 * dataSize)()
    
        for i in range(dataSize):
            vertices[i] = (data[i] / 1000.0) * 50.0  # 小さすぎるため拡大
            indices[i] = i
    
        # メッシュデータを格納します。
        triData = odepy.dGeomTriMeshDataCreate()
    
        # 注意: 単精度用の dGeomTriMeshDataBuildSingle を使用すると期待通りに動きません。
        odepy.dGeomTriMeshDataBuildDouble(triData, vertices, 3 * ctypes.sizeof(ctypes.c_double), len(vertices),
                                          indices, len(indices), 3 * ctypes.sizeof(ctypes.c_int32))
    
        # ジオメトリの作成
    
        def __TriCallback(triMesh, refObject, triangleIndex):
            return 0
    
        def __TriArrayCallback(triMesh, refObject, triIndices, triCount):
            pass
    
        def __TriRayCallback(triMesh, ray, triangleIndex, u, v):
            return 0
    
        geom = odepy.dCreateTriMesh(space, triData,
                                    odepy.dTriCallback(__TriCallback),
                                    odepy.dTriArrayCallback(__TriArrayCallback),
                                    odepy.dTriRayCallback(__TriRayCallback))
    
        # バウンディングボックスの取得
        aabb = (odepy.dReal * 6)()
        odepy.dGeomGetAABB(geom, aabb)
        minx, maxx, miny, maxy, minz, maxz = [aabb[i] for i in range(6)]
    
        # ボディの作成
        body = odepy.dBodyCreate(world)
        odepy.dGeomSetBody(geom, body)
    
        # 質量の設定
        mass = odepy.dMass()
        lx = maxx - minx
        ly = maxy - miny
        lz = maxz - minz
        m = 1.0  # [kg]
        odepy.dMassSetBoxTotal(ctypes.byref(mass), m, lx, ly, lz)
        odepy.dBodySetMass(body, ctypes.byref(mass))
    
        # 位置と姿勢の設定
        odepy.dBodySetPosition(body, 0, 0, lz / 2.0)
        R = odepy.dMatrix3()
        odepy.dRFromAxisAndAngle(R, 0, 0, 1, np.pi / 4.0)
        odepy.dBodySetRotation(body, R)
    
        # 地面との衝突設定
        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.2
                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))
    
        # ジオメトリの描画
        def __StepCallback(pause):
            tDelta = 0.01
            odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
            odepy.dWorldStep(world, tDelta)
            odepy.dJointGroupEmpty(contactgroup)
    
            for i in range(len(indices) / 9):
                v1 = (odepy.dReal * 3)()
                v2 = (odepy.dReal * 3)()
                v3 = (odepy.dReal * 3)()
                for j in range(3):
                    v1[j] = vertices[indices[i * 9 + 0 + j]]
                    v2[j] = vertices[indices[i * 9 + 3 + j]]
                    v3[j] = vertices[indices[i * 9 + 6 + j]]
                pos = odepy.dGeomGetPosition(geom)
                rot = odepy.dGeomGetRotation(geom)
                ds.dsDrawTriangleD(pos, rot, v1, v2, v3, 1)
    
        RunDrawStuff(__StepCallback)
        DestroyWorld(world, space)
    
    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, 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()
    

    起動直後

    倒れた状態

    力およびトルクを加える

    ワールド座標系で指定したトルク、ボディ座標系で指定したトルク

    odepy.dBodyAddTorque(body, 0, 0, 99)
    odepy.dBodyAddRelTorque(body, 0, 0, 99)
    
    torque = odepy.dBodyGetTorque(body)
    print(torque[0])
    print(torque[1])
    print(torque[2])
    

    ワールド座標系で指定した力、ボディ座標系で指定した力 (重心に加える)

    odepy.dBodyAddForce(body, 0, 0, -123)
    odepy.dBodyAddRelForce(body, 0, 0, -123)
    
    force = odepy.dBodyGetForce(body)
    print(force[0])
    print(force[1])
    print(force[2])
    

    重心ではなく指定した位置に力を加える

    odepy.dBodyAddForceAtPos(body, 0, 0, -123, 0, 0, 0)
    odepy.dBodyAddForceAtRelPos(body, 0, 0, -123, 0, 0, 0)
    

    重心ではなく指定した位置に力を加える (力をボディ座標系で指定)

    odepy.dBodyAddRelForceAtPos(body, 0, 0, -123, 0, 0, 0)
    odepy.dBodyAddRelForceAtRelPos(body, 0, 0, -123, 0, 0, 0)
    

    ジョイントの制御

    ジョイントにはモータが組込まれています。モータを利用すると角速度による制御が可能です。

    odepy.dJointSetHingeParam(joint, odepy.dParamVel, -1.0)  # 角速度 [rad/s]
    

    ジョイントの回転角度を制御するための別の方法に、力やトルクを加えるものがあります。

    odepy.dJointAddHingeTorque(joint, -400.0)  # トルク [Nm]
    

    これはモータとは関係なく、ジョイントで接続されているボディに力およびトルクを加えています。ジョイントの種類に応じて以下のような API を利用します。

    odepy.dJointAddUniversalTorques(joint, -400, -400)
    odepy.dJointAddHinge2Torques(joint, -400, -400)
    odepy.dJointAddSliderForce(joint, -123)
    odepy.dJointAddAMotorTorques(joint, -400, -400, -400)
    

    位置、姿勢、速度、角速度の取得

    位置

    pos = odepy.dBodyGetPosition(body)
    print(pos[0])
    print(pos[1])
    print(pos[2])
    

    姿勢

    rot = odepy.dBodyGetRotation(body)
    q = odepy.dBodyGetQuaternion(body)
    

    速度

    v = odepy.dBodyGetLinearVel(body)
    print(v[0])
    print(v[1])
    print(v[2])
    

    角速度

    av = odepy.dBodyGetAngularVel(body)
    print(av[0])
    print(av[1])
    print(av[2])
    
    0
    詳細設定を開く/閉じる
    アカウント プロフィール画像 (本文下)

    Software Engineer @ Tokyo

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

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

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

    Feedbacks

    Feedbacks コンセプト画像

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

      関連記事