概要

こちらのページでインストールした Open Dynamics Engine (ODE) のジョイント操作について記載します。

ヒンジジョイント

ロボットにはジョイントとリンクがあります。ODE に実装されているジョイントの一つにヒンジジョイントがあります。以下の例では二つの立方体をヒンジジョイントで結合して、ヒンジジョイントに角速度を与えています。

初期位置

しばらくすると 45 度だけ回転した状態になります。

angle=-45.0000000715, angleRate=6.19802999323e-09
angle=-45.0000000687, angleRate=4.95843873643e-09

sample.py

#!/usr/bin/python
# -*- coding: utf-8 -*-

import odepy
import drawstuffpy as ds
import ctypes
import random
import numpy as np

def Main():
    world, space, ground, contactgroup = CreateWorld()

    # 立方体を二つ設置します。
    size = 0.5
    box1 = AddBox(world, space, pos=[0, size / np.sqrt(2), 2.0], size=size)
    box2 = AddBox(world, space, pos=[0, -size / np.sqrt(2), 2.0], size=size)
    geoms = [box1['geom'], box2['geom']]
    geomColors = [[random.random(), random.random(), random.random()] for geom in geoms]

    # ヒンジジョイントの作成。
    joint = AddHinge(world, box1['body'], box2['body'])

    def __NearCallback(data, o1, o2):
        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 = 1.0
            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):
        # ヒンジジョイントの回転角度と角速度を出力
        angle = odepy.dJointGetHingeAngle(joint) * 180 / np.pi  # [rad] -> [deg]
        angleRate = odepy.dJointGetHingeAngleRate(joint)  # [rad/s]
        print('angle={}, angleRate={}'.format(angle, angleRate))

        tDelta = 0.01
        if pause != 1:
            odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
            odepy.dWorldStep(world, tDelta)
            odepy.dJointGroupEmpty(contactgroup)
        for geom, color in zip(geoms, geomColors):
            body = odepy.dGeomGetBody(geom)
            ds.dsSetColor(*color)
            if odepy.dGeomGetClass(geom) == odepy.dBoxClass:
                lengths = odepy.dVector3()
                odepy.dGeomBoxGetLengths(geom, lengths)
                pos = odepy.dBodyGetPosition(body)
                rot = odepy.dBodyGetRotation(body)
                ds.dsDrawBoxD(pos, rot, lengths)
            else:
                raise Exception('Not supported geom class: {}'.format(odepy.dGeomGetClass(geom)))

    RunDrawStuff(__StepCallback)
    DestroyWorld(world, space)

def AddHinge(world, body1, body2):
    # ヒンジジョイント
    joint = odepy.dJointCreateHinge(world, 0)
    odepy.dJointAttach(joint, body1, body2)

    # 二つの立方体の中間点を初期位置として作成
    pos1 = odepy.dBodyGetPosition(body1)
    pos2 = odepy.dBodyGetPosition(body2)
    odepy.dJointSetHingeAnchor(joint,
                               (pos1[0] + pos2[0]) / 2.0,
                               (pos1[1] + pos2[1]) / 2.0,
                               (pos1[2] + pos2[2]) / 2.0)

    # z 軸方向を回転軸として設定
    odepy.dJointSetHingeAxis(joint, 0, 0, 1)

    # 最大の回転角度
    odepy.dJointSetHingeParam(joint, odepy.dParamLoStop, -np.pi / 4.0)
    odepy.dJointSetHingeParam(joint, odepy.dParamHiStop, np.pi / 4.0)

    # ジョイントに角速度を与えてみます。
    odepy.dJointSetHingeParam(joint, odepy.dParamVel, -1.0)  # 角速度 [rad/s]
    odepy.dJointSetHingeParam(joint, odepy.dParamFMax, 200)  # 最大トルク [Nm]

    # ジョイントにトルクを与えることもできます。
    # odepy.dJointAddHingeTorque(joint, -400.0)  # トルク [Nm]
    # odepy.dJointSetHingeParam(joint, odepy.dParamFMax, 200)  # 最大トルク [Nm]
    return joint

def AddBox(world, space, pos, size, m=1.0):
    mass = odepy.dMass()
    odepy.dMassSetZero(ctypes.byref(mass))
    odepy.dMassSetBoxTotal(ctypes.byref(mass), m, size, size, size)
    body = odepy.dBodyCreate(world)
    odepy.dBodySetMass(body, ctypes.byref(mass))
    geom = odepy.dCreateBox(space, size, size, size)
    odepy.dGeomSetBody(geom, body)
    box = {'body': body, 'geom': geom}
    odepy.dBodySetPosition(box['body'], *pos)
    # z 軸方向に 45 度回転させておきます。
    R = odepy.dMatrix3()
    odepy.dRFromAxisAndAngle(R, 0, 0, 1, np.pi / 4.0)
    odepy.dBodySetRotation(box['body'], R)
    return box

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

スライダージョイント

ODE に実装されているスライダージョイントのサンプルコードです。

初期位置

縮んだ状態

sample.py

#!/usr/bin/python
# -*- coding: utf-8 -*-

from ctypes import addressof
from ctypes import byref
from ctypes import sizeof
from ctypes import c_float
from ctypes import c_char
from ctypes import POINTER
from ctypes import create_string_buffer
from random import random
from numpy import pi

import odepy
import drawstuffpy as ds

step = 0

def Main():
    world, space, ground, contactgroup = CreateWorld()

    # 立方体を二つ設置します。
    box1 = AddBox(world, space, pos=[-1.0, -1.0, 1.5], size=1.0)
    box2 = AddBox(world, space, pos=[-1.0, 1.0, 1.5], size=1.0)
    geoms = [box1['geom'], box2['geom']]
    geomColors = [[random(), random(), random()] for geom in geoms]

    # スライダージョイントの作成。
    joint = AddSlider(world, box1['body'], box2['body'])

    def __NearCallback(data, o1, o2):
        # 今回、地面との衝突以外は無視します。
        o1IsGround = addressof(ground.contents) == addressof(o1.contents)
        o2IsGround = addressof(ground.contents) == addressof(o2.contents)
        if not (o1IsGround or o2IsGround):
            return
        N = 10
        contacts = (odepy.dContact * N)()
        n = odepy.dCollide(o1, o2, N, byref(contacts[0].geom), sizeof(odepy.dContact))
        for i in range(n):
            contact = contacts[i]
            contact.surface.mu = float('inf')
            contact.surface.mode = odepy.dContactBounce
            contact.surface.bounce = 1.0
            contact.surface.bounce_vel = 0.0
            c = odepy.dJointCreateContact(world, contactgroup, byref(contact))
            odepy.dJointAttach(c, odepy.dGeomGetBody(contact.geom.g1), odepy.dGeomGetBody(contact.geom.g2))

    def __StepCallback(pause):
        # スライダージョイントの伸縮と伸縮速度を出力
        pos = odepy.dJointGetSliderPosition(joint)  # [m]
        posRate = odepy.dJointGetSliderPositionRate(joint)  # [m/s]

        # スライダージョイントの伸縮を時間に応じて制御してみます。
        global step
        step += 1
        stepCycle = 200
        if 0 <= step % stepCycle and step % stepCycle <= 10:
            print('pos=%d, posRate=%d' % (pos * 100, posRate))
            target = 1.0  # 縮む
        else:
            target = 0  # 初期位置

        if pause != 1:
            kp = 25.0
            fmax = 400
            u = kp * (target - pos)
            odepy.dJointSetSliderParam(joint, odepy.dParamVel, u)
            odepy.dJointSetSliderParam(joint, odepy.dParamFMax, fmax)

            # tDelta 時間進めます。
            tDelta = 0.01
            odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
            odepy.dWorldStep(world, tDelta)
            odepy.dJointGroupEmpty(contactgroup)

        for geom, color in zip(geoms, geomColors):
            body = odepy.dGeomGetBody(geom)
            ds.dsSetColor(*color)
            if odepy.dGeomGetClass(geom) == odepy.dBoxClass:
                lengths = odepy.dVector3()
                odepy.dGeomBoxGetLengths(geom, lengths)
                pos = odepy.dBodyGetPosition(body)
                rot = odepy.dBodyGetRotation(body)
                ds.dsDrawBoxD(pos, rot, lengths)
            else:
                raise Exception('Not supported geom class: {}'.format(odepy.dGeomGetClass(geom)))

    RunDrawStuff(__StepCallback)
    DestroyWorld(world, space)

def AddSlider(world, body1, body2):
    # スライダージョイント
    joint = odepy.dJointCreateSlider(world, 0)
    odepy.dJointAttach(joint, body1, body2)
    # y 軸方向を伸縮方向として設定
    odepy.dJointSetSliderAxis(joint, 0, 1, 0)
    odepy.dJointSetSliderParam(joint, odepy.dParamLoStop, 0.0)  # 最大の伸び [m]
    odepy.dJointSetSliderParam(joint, odepy.dParamHiStop, 1.0)  # 最大の縮み [m]
    return joint

def AddBox(world, space, pos, size, m=1.0):
    mass = odepy.dMass()
    odepy.dMassSetZero(byref(mass))
    odepy.dMassSetBoxTotal(byref(mass), m, size, size, size)
    body = odepy.dBodyCreate(world)
    odepy.dBodySetMass(body, byref(mass))
    geom = odepy.dCreateBox(space, size, size, size)
    odepy.dGeomSetBody(geom, body)
    box = {'body': body, 'geom': geom}
    odepy.dBodySetPosition(box['body'], *pos)
    # z 軸方向に 45 度回転させておきます。
    R = odepy.dMatrix3()
    odepy.dRFromAxisAndAngle(R, 0, 0, 1, pi / 4.0)
    odepy.dBodySetRotation(box['body'], R)
    return box

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 = (c_float * 3)()
        hpr = (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 = create_string_buffer(pathToTextures.encode('utf-8'))
    ds.dsSimulationLoop(0, byref(POINTER(c_char)()), w, h, fn)

def CreateWorld():
    odepy.dInitODE()
    world = odepy.dWorldCreate()
    odepy.dWorldSetGravity(world, 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()