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