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

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

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

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

Open Dynamics Engine (ODE) ジョイント API のサンプルコード (Python)

モーダルを閉じる

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

モーダルを閉じる

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

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

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

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

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

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

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

目次

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

    Software Engineer @ Tokyo

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

    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.addressofcontents を渡します。

    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()
    
    0
    詳細設定を開く/閉じる
    アカウント プロフィール画像 (本文下)

    Software Engineer @ Tokyo

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

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

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

    Feedbacks

    Feedbacks コンセプト画像

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

      関連記事