プログラミング/IoT の関連商品 (Amazonのアソシエイトとして、当メディアは適格販売により収入を得ています。)
最近の投稿
- Looker から Snowflake への OAuth 接続
- Looker (Google Cloud core) 30日間 フリートライアル設定
- AWS 落穂拾い (Security)
- AWS 落穂拾い (Storage)
- マイクロマウス
注目の記事
概要
こちらのページでインストールした 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()
関連ページ
概要 こちらのページでインストールした Open Dynamics Engine (ODE) について、ボディ操作に関する API のサンプルコードを記載します。 位置姿勢の設定 位置 odepy.dBodySetPosition(body, 0.0, 0.0, 0.0) 姿勢 (回転行列) R = odepy.dMatrix3() odepy.dRSetIdentity(R) odepy
概要 ロボットアプリケーションを開発する際に、ロボットの自己位置を推定する必要がある場合を考えます。ここでは Open Dynamics Engine を Python から利用した場合について、自己位置推定のサンプルを記載します。自己位置推定と環境の地図作成を同時に行う場合を SLAM (Simultaneous Localization and Mapping) とよびます。
