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])
関連記事
- Python コードスニペット (条件分岐)if-elif-else sample.py #!/usr/bin/python # -*- coding: utf-8 -*- # コメント内であっても、ASCII外の文字が含まれる場合はエンコーディング情報が必須 x = 1 # 一行スタイル if x==0: print 'a' # 参考: and,or,notが使用可能 (&&,||はエラー) elif x==1: p...
- Python コードスニペット (リスト、タプル、ディクショナリ)リスト range 「0から10まで」といった範囲をリスト形式で生成します。 sample.py print range(10) # for(int i=0; i<10; ++i) ← C言語などのfor文と比較 print range(5,10) # for(int i=5; i<10; ++i) print range(5,10,2) # for(int i=5; i<10;...
- ZeroMQ (zmq) の Python サンプルコードZeroMQ を Python から利用する場合のサンプルコードを記載します。 Fixing the World To fix the world, we needed to do two things. One, to solve the general problem of "how to connect any code to any code, anywhere". Two, to wra...
- Matplotlib/SciPy/pandas/NumPy サンプルコードPython で数学的なことを試すときに利用される Matplotlib/SciPy/pandas/NumPy についてサンプルコードを記載します。 Matplotlib SciPy pandas [NumPy](https://www.numpy
- pytest の基本的な使い方pytest の基本的な使い方を記載します。 適宜参照するための公式ドキュメントページ Full pytest documentation API Reference インストール 適当なパッケージ