Open Dynamics Engine (ODE) のジョイントについて、関連する API のサンプルコードを記載します。
静的な仮想物体とのジョイント結合
0 を指定すると静的な仮想物体と結合されます。
odepy.dJointAttach(joint, body, 0)
ジョイントの破壊
odepy.dJointDestroy(joint)
ジョイントの種類を取得
print(odepy.dJointGetType(joint) == odepy.dJointTypeHinge)
以下のような種類があります。
odepy.dJointTypeBall
odepy.dJointTypeHinge
odepy.dJointTypeSlider
odepy.dJointTypeContact
odepy.dJointTypeFixed
odepy.dJointTypeAMotor
ボディの取得
結合されているボディ二つを取得
body1 = odepy.dJointGetBody(joint, 0)
body2 = odepy.dJointGetBody(joint, 1)
ボディの比較には ctypes.addressof
に contents
を渡します。
ctypes.addressof(body1.contents) == ctypes.addressof(body2.contents)
静的環境の場合は ValueError
が返ります。
In [16]: hoge.contents
ValueError: NULL pointer access
ボディ同士が結合されているか調査
結合されている場合は 1 が返ります。結合されていない場合は 0 が返ります。
odepy.dAreConnected(body1, body2)
特定のジョイントを除いた判定
odepy.dAreConnectedExcluding(body1, body2, odepy.dJointTypeContact)
誤差の補正
ジョイントの位置姿勢は、計算誤差などでずれが生じます。誤差を補正する力を加えるためには、ERP (Error Reduction Parameter) を設定します。0.2 から 0.8 の値が推奨値であり、0.0 は修正なし、1.0 は誤差をすべて修正するような力を加えます。0.2 が既定値です。
すべてのジョイントに対して設定する場合
odepy.dWorldSetERP(world, 0.2)
特定のジョイントに対して設定する場合
contact.surface.mode = odepy.dContactBounce | odepy.dContactSoftERP
contact.surface.bounce = 1.0
contact.surface.bounce_vel = 0.0
contact.surface.soft_erp = 0.2
物体のやわらかさ
ODE では物体が接触する箇所にジョイントが作成されます。ジョイントの CFM (Constraint Force Mixing) を小さな値にすると剛体同士の接触、大きな値にするとやわらかい物体同士の接触、のようなシミュレーションになります。0.0 から 1.0 の値で設定できます。既定値は 1e-10 です。
すべてのジョイントに対して設定する場合
odepy.dWorldSetCFM(world, 1.0)
特定のジョイントに対して設定する場合
contact.surface.mode = odepy.dContactBounce | odepy.dContactSoftCFM
contact.surface.bounce = 1.0
contact.surface.bounce_vel = 0.0
contact.surface.soft_cfm = 0.0
3自由度の回転ジョイント Ball
作成およびアタッチ
joint = odepy.dJointCreateBall(world, 0)
odepy.dJointAttach(joint, body1, body2)
初期位置の設定
pos1 = odepy.dBodyGetPosition(body1)
pos2 = odepy.dBodyGetPosition(body2)
odepy.dJointSetBallAnchor(joint,
(pos1[0] + pos2[0]) / 2.0,
(pos1[1] + pos2[1]) / 2.0,
(pos1[2] + pos2[2]) / 2.0)
一つ目のボディからみたときのジョイントの絶対座標
result = odepy.dVector3()
odepy.dJointGetBallAnchor(joint, result)
print(result[0])
print(result[1])
print(result[2])
二つ目のボディからみたときのジョイントの絶対座標 (ずれがない場合は上記値と一致)
result = odepy.dVector3()
odepy.dJointGetBallAnchor2(joint, result)
print(result[0])
print(result[1])
print(result[2])
2自由度の回転ジョイント Universal、Hinge2
作成およびアタッチ
joint = odepy.dJointCreateUniversal(world, 0)
odepy.dJointAttach(joint, body1, body2)
初期位置と二つの回転軸の指定 (二つの回転軸は直行していることが必要)
odepy.dJointSetUniversalAnchor(joint, x, y, z)
odepy.dJointSetUniversalAxis1(joint, 1, 0, 0)
odepy.dJointSetUniversalAxis2(joint, 0, 1, 0)
絶対座標、回転軸の取得
result = odepy.dVector3()
odepy.dJointGetUniversalAnchor(joint, result)
odepy.dJointGetUniversalAnchor2(joint, result)
odepy.dJointGetUniversalAxis1(joint, result)
odepy.dJointGetUniversalAxis2(joint, result)
回転角度、角速度の取得
odepy.dJointGetUniversalAngle1(joint)
odepy.dJointGetUniversalAngle2(joint)
odepy.dJointGetUniversalAngle1Rate(joint)
odepy.dJointGetUniversalAngle2Rate(joint)
Hinge2 の場合
joint = odepy.dJointCreateHinge2(world, 0)
odepy.dJointAttach(joint, body1, body2)
odepy.dJointSetHinge2Anchor(joint, x, y, z)
odepy.dJointSetHinge2Axis1(joint, 1, 0, 0)
odepy.dJointSetHinge2Axis2(joint, 0, 1, 0)
result = odepy.dVector3()
odepy.dJointGetHinge2Anchor(joint, result)
odepy.dJointGetHinge2Anchor2(joint, result)
odepy.dJointGetHinge2Axis1(joint, result)
odepy.dJointGetHinge2Axis2(joint, result)
odepy.dJointGetHinge2Angle1(joint)
odepy.dJointGetHinge2Angle2(joint)
odepy.dJointGetHinge2Angle1Rate(joint)
odepy.dJointGetHinge2Angle2Rate(joint)
1自由度の回転ジョイント Hinge
作成およびアタッチ
joint = odepy.dJointCreateHinge(world, 0)
odepy.dJointAttach(joint, body1, body2)
初期位置と回転軸の設定
odepy.dJointSetHingeAnchor(joint, x, y, z)
odepy.dJointSetHingeAxis(joint, 0, 0, 1)
絶対座標、回転軸の取得
result = odepy.dVector3()
odepy.dJointGetHingeAnchor(joint, result)
odepy.dJointGetHingeAnchor2(joint, result)
odepy.dJointGetHingeAxis(joint, result)
回転角度、角速度の取得
angle = odepy.dJointGetHingeAngle(joint)
angleRate = odepy.dJointGetHingeAngleRate(joint)
1自由度のスライダージョイント
作成およびアタッチ
joint = odepy.dJointCreateSlider(world, 0)
odepy.dJointAttach(joint, body1, body2)
スライド軸を設定
odepy.dJointSetSliderAxis(joint, 0, 0, 1)
スライド軸、伸縮している長さ、速度を取得
result = odepy.dVector3()
odepy.dJointGetSliderAxis(joint, result)
pos = odepy.dJointGetSliderPosition(joint) # [m]
posRate = odepy.dJointGetSliderPositionRate(joint) # [m/s]
0自由度の固定ジョイント
0 を指定すると静的環境と結合されます。空中に固定することもできます。
joint = odepy.dJointCreateFixed(world, 0)
odepy.dJointAttach(joint, body1, 0) # 静的環境と結合
odepy.dJointSetFixed(joint)
モータによる回転ジョイントの制御
A モータ (angular motor) を利用すると、回転ジョイントの角速度を指定した値に設定できます。
#!/usr/bin/python
# -*- coding: utf-8 -*-
import odepy
import drawstuffpy as ds
import ctypes
import numpy as np
def Main():
world, space, ground, contactgroup = CreateWorld()
# base と axisHolder を作成します。
q = odepy.dQuaternion()
odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0)
base = AddBox(world, space, lx=0.15, ly=0.25, lz=0.5, m=1.0, px=0.0, py=0.0, pz=0.25, q=q)
axisHolder = AddBox(world, space, lx=0.15, ly=0.25, lz=0.5, m=1.0, px=0.0, py=0.0, pz=1.0, q=q)
# base を動かないように環境に固定します。
jointFixed = odepy.dJointCreateFixed(world, 0)
odepy.dJointAttach(jointFixed, odepy.dGeomGetBody(base), 0)
odepy.dJointSetFixed(jointFixed)
# base と axisHolder を ball ジョイントで結合します。
jointBall = odepy.dJointCreateBall(world, 0)
odepy.dJointAttach(jointBall, odepy.dGeomGetBody(base), odepy.dGeomGetBody(axisHolder))
odepy.dJointSetBallAnchor(jointBall, 0.0, 0.0, 0.5)
# XYZ 軸を表示するための線となる capsule オブジェクトを三つ作成します。
qX = odepy.dQuaternion()
qY = odepy.dQuaternion()
qZ = odepy.dQuaternion()
odepy.dQFromAxisAndAngle(qX, 0.0, 1.0, 0.0, np.pi / 2.0)
odepy.dQFromAxisAndAngle(qY, 1.0, 0.0, 0.0, np.pi / 2.0)
odepy.dQFromAxisAndAngle(qZ, 0.0, 0.0, 1.0, 0.0)
axisX = AddCapsule(world, space, r=0.008, l=1.0, m=1.0, px=0.5, py=0.0, pz=0.5, q=qX)
axisY = AddCapsule(world, space, r=0.008, l=1.0, m=1.0, px=0.0, py=0.5, pz=0.5, q=qY)
axisZ = AddCapsule(world, space, r=0.008, l=1.0, m=1.0, px=0.0, py=0.0, pz=1.0, q=qZ)
# XYZ 軸表示用のオブジェクトと axisHolder を静的ジョイントで結合します。
jointFixedX = odepy.dJointCreateFixed(world, 0)
jointFixedY = odepy.dJointCreateFixed(world, 0)
jointFixedZ = odepy.dJointCreateFixed(world, 0)
odepy.dJointAttach(jointFixedX, odepy.dGeomGetBody(axisHolder), odepy.dGeomGetBody(axisX))
odepy.dJointAttach(jointFixedY, odepy.dGeomGetBody(axisHolder), odepy.dGeomGetBody(axisY))
odepy.dJointAttach(jointFixedZ, odepy.dGeomGetBody(axisHolder), odepy.dGeomGetBody(axisZ))
odepy.dJointSetFixed(jointFixedX)
odepy.dJointSetFixed(jointFixedY)
odepy.dJointSetFixed(jointFixedZ)
# ballJoint の回転を制御する motor を作成します。
motor = Motor(world, odepy.dGeomGetBody(base), odepy.dGeomGetBody(axisHolder))
def __StepCallback(pause):
# モータで ballJoint の回転を制御します。
motor.Control()
# 時間を進めます。
tDelta = 0.01
odepy.dWorldStep(world, tDelta)
odepy.dJointGroupEmpty(contactgroup)
# 描画処理
for geom in (base, axisHolder):
ds.dsSetColor(1.0, 1.0, 0.0)
lengths = odepy.dVector3()
odepy.dGeomBoxGetLengths(geom, lengths)
ds.dsDrawBoxD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), lengths)
for geom in (axisX, axisY, axisZ):
ds.dsSetColor(0.0, 0.0, 0.0)
r = odepy.dReal()
l = odepy.dReal()
odepy.dGeomCapsuleGetParams(geom, ctypes.byref(r), ctypes.byref(l))
ds.dsDrawCapsuleD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), l.value, r.value)
def __Command(cmd):
if chr(cmd) == 'x':
motor.SetAlpha(motor.GetAlpha() + 1)
if chr(cmd) == 'X':
motor.SetAlpha(motor.GetAlpha() - 1)
if chr(cmd) == 'y':
motor.SetBeta(motor.GetBeta() + 1)
if chr(cmd) == 'Y':
motor.SetBeta(motor.GetBeta() - 1)
if chr(cmd) == 'z':
motor.SetGamma(motor.GetGamma() + 1)
if chr(cmd) == 'Z':
motor.SetGamma(motor.GetGamma() - 1)
RunDrawStuff(__StepCallback, __Command)
DestroyWorld(world, space)
def AddBox(world, space, lx, ly, lz, m, px, py, pz, q):
geom = odepy.dCreateBox(space, lx, ly, lz)
body = odepy.dBodyCreate(world)
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))
odepy.dBodySetPosition(body, px, py, pz)
odepy.dBodySetQuaternion(body, q)
return geom
def AddCapsule(world, space, r, l, m, px, py, pz, q):
geom = odepy.dCreateCapsule(space, r, l)
body = odepy.dBodyCreate(world)
mass = odepy.dMass()
direction = 3 # z 軸方向
odepy.dMassSetZero(ctypes.byref(mass))
odepy.dMassSetCapsuleTotal(ctypes.byref(mass), m, direction, r, l)
odepy.dGeomSetBody(geom, body)
odepy.dBodySetMass(body, ctypes.byref(mass))
odepy.dBodySetPosition(body, px, py, pz)
odepy.dBodySetQuaternion(body, q)
return geom
class Motor(object):
def __init__(self, world, body1, body2, alpha=0.0, beta=0.0, gamma=0.0):
self.__alpha = alpha
self.__beta = beta
self.__gamma = gamma
self.__motor = odepy.dJointCreateAMotor(world, 0)
odepy.dJointAttach(self.__motor, body1, body2)
odepy.dJointSetAMotorMode(self.__motor, odepy.dAMotorEuler)
odepy.dJointSetAMotorNumAxes(self.__motor, 3)
# オイラー角の軸を設定します。
# 一つ目の軸 (0) は body1 に固定します。
odepy.dJointSetAMotorAxis(self.__motor, 0, 1, 1, 0, 0)
# 二つ目の軸は残りの二つの軸と直行するように計算されます。
# 三つ目の軸 (2) は body2 に固定します。
odepy.dJointSetAMotorAxis(self.__motor, 2, 2, 0, 0, 1)
def GetAlpha(self):
return self.__alpha
def GetBeta(self):
return self.__beta
def GetGamma(self):
return self.__gamma
def SetAlpha(self, alpha):
self.__alpha = max(min(alpha, 80.0), -80.0)
def SetBeta(self, beta):
self.__beta = max(min(beta, 80.0), -80.0)
def SetGamma(self, gamma):
self.__gamma = max(min(gamma, 80.0), -80.0)
def Control(self):
angles = []
for axisNum, target in enumerate([self.__alpha, self.__beta, self.__gamma]):
target = target * np.pi / 180.0
kp = 10.0
fmax = 100.0
angle = odepy.dJointGetAMotorAngle(self.__motor, axisNum)
u = kp * (target - angle)
if axisNum == 0:
odepy.dJointSetAMotorParam(self.__motor, odepy.dParamVel, u)
odepy.dJointSetAMotorParam(self.__motor, odepy.dParamFMax, fmax)
elif axisNum == 1:
odepy.dJointSetAMotorParam(self.__motor, odepy.dParamVel2, u)
odepy.dJointSetAMotorParam(self.__motor, odepy.dParamFMax2, fmax)
elif axisNum == 2:
odepy.dJointSetAMotorParam(self.__motor, odepy.dParamVel3, u)
odepy.dJointSetAMotorParam(self.__motor, odepy.dParamFMax3, fmax)
angles.append(angle * 180.0 / np.pi)
print('{}\t{}\t{}'.format(angles[0], angles[1], angles[2]))
def RunDrawStuff(stepCallback, command, 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'))
fn.command = ds.dsCommandCallback(command)
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()
関連記事
- 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 インストール 適当なパッケージ