概要

こちらのページでインストールした 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])

Open Dynamics Engine によるロボットの自己位置の推定 (Python)

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