作成日
最終更新
記事区分
ロボットアプリケーションを開発する際に、ロボットの自己位置を推定する必要がある場合を考えます。ここでは Open Dynamics Engine を Python から利用した場合について、自己位置推定のサンプルを記載します。自己位置推定と環境の地図作成を同時に行う場合を SLAM (Simultaneous Localization and Mapping) とよびます。
検証に用いる車輪型ロボット
ODE を用いて、簡単な車輪型ロボットを作ります。ヒンジジョイントに内臓されているモータを速度制御することで移動します。
参考書籍: 『簡単!実践!ロボットシミュレーション - Open Dynamics Engineによるロボットプログラミング』
#!/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()
# 車輪型ロボット
robot = Robot(world, space)
def __NearCallback(data, o1, o2):
o1IsGround = ctypes.addressof(ground.contents) == ctypes.addressof(o1.contents)
o2IsGround = ctypes.addressof(ground.contents) == ctypes.addressof(o2.contents)
if not (o1IsGround or o2IsGround):
return
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 = 99 # ある程度小さな値にする必要があります。
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):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
x, y = robot.GetPos()
print('{}\t{}'.format(x, y))
odepy.dSpaceCollide(space, 0, odepy.dNearCallback(__NearCallback))
odepy.dWorldStep(world, 0.01)
odepy.dJointGroupEmpty(contactgroup)
for geom in robot.GetGeoms():
if odepy.dGeomGetClass(geom) == odepy.dBoxClass:
ds.dsSetColor(1.0, 1.0, 0.0)
lengths = odepy.dVector3()
odepy.dGeomBoxGetLengths(geom, lengths)
ds.dsDrawBoxD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), lengths)
if odepy.dGeomGetClass(geom) == odepy.dCylinderClass:
ds.dsSetColor(1.0, 1.0, 1.0)
r = odepy.dReal()
l = odepy.dReal()
odepy.dGeomCylinderGetParams(geom, ctypes.byref(r), ctypes.byref(l))
ds.dsDrawCylinderD(odepy.dGeomGetPosition(geom), odepy.dGeomGetRotation(geom), l.value, r.value)
def __Command(cmd):
if chr(cmd) == 'l':
robot.SetVelL(robot.GetVelL() + 1.0)
if chr(cmd) == 'L':
robot.SetVelL(robot.GetVelL() - 1.0)
if chr(cmd) == 'r':
robot.SetVelR(robot.GetVelR() + 1.0)
if chr(cmd) == 'R':
robot.SetVelR(robot.GetVelR() - 1.0)
if chr(cmd) == 's':
robot.SetVelL(0.0)
robot.SetVelR(0.0)
RunDrawStuff(__StepCallback, __Command)
DestroyWorld(world, space)
class Robot(object):
def __init__(self, world, space, lx=0.4, ly=0.4, lz=0.2, rLR=0.11, rFB=0.05, w=0.02, mBase=9.4, mLR=0.2, mFB=0.1, px=0.0, py=0.0, pz=0.2):
self.__velL = 0.0
self.__velR = 0.0
self.__base = self.__AddBox(world, space, lx=lx, ly=ly, lz=lz, m=mBase, px=px, py=py, pz=pz)
self.__wheelL = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=-(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelR = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelF = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__wheelB = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=-(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__jointL = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelL), odepy.dGeomGetPosition(self.__wheelL))
self.__jointR = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelR), odepy.dGeomGetPosition(self.__wheelR))
self.__jointF = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelF), odepy.dGeomGetPosition(self.__wheelF))
self.__jointB = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelB), odepy.dGeomGetPosition(self.__wheelB))
def GetVelL(self):
return self.__velL
def GetVelR(self):
return self.__velR
def SetVelL(self, velL):
self.__velL = velL
def SetVelR(self, velR):
self.__velR = velR
def GetPos(self):
pos = odepy.dGeomGetPosition(self.__base)
return pos[0], pos[1]
def Control(self):
fMax = 100
odepy.dJointSetHingeParam(self.__jointL, odepy.dParamVel, self.__velL)
odepy.dJointSetHingeParam(self.__jointL, odepy.dParamFMax, fMax)
odepy.dJointSetHingeParam(self.__jointR, odepy.dParamVel, self.__velR)
odepy.dJointSetHingeParam(self.__jointR, odepy.dParamFMax, fMax)
def GetGeoms(self):
return self.__base, self.__wheelL, self.__wheelR, self.__wheelF, self.__wheelB
def __AddBox(self, world, space, lx, ly, lz, m, px, py, pz):
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)
q = odepy.dQuaternion()
odepy.dQFromAxisAndAngle(q, 0, 0, 1, 0.0)
odepy.dBodySetQuaternion(body, q)
return geom
def __AddCylinder(self, world, space, r, l, m, px, py, pz, direction=3):
geom = odepy.dCreateCylinder(space, r, l)
body = odepy.dBodyCreate(world)
mass = odepy.dMass()
odepy.dMassSetZero(ctypes.byref(mass))
odepy.dMassSetCylinderTotal(ctypes.byref(mass), m, direction, r, l)
odepy.dGeomSetBody(geom, body)
odepy.dBodySetMass(body, ctypes.byref(mass))
odepy.dBodySetPosition(body, px, py, pz)
q = odepy.dQuaternion()
odepy.dQFromAxisAndAngle(q, 0, 1, 0, np.pi / 2.0)
odepy.dBodySetQuaternion(body, q)
return geom
def __AddJoint(self, world, body1, body2, pos):
joint = odepy.dJointCreateHinge(world, 0)
odepy.dJointAttach(joint, body1, body2)
odepy.dJointSetHingeAxis(joint, 1, 0, 0)
odepy.dJointSetHingeAnchor(joint, pos[0], pos[1], pos[2])
return joint
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()
外界センサを使う場合
距離情報を用いる方法
三つのランドマークの位置が既知であるとします。距離センサで取得したランドマークからの距離をもとに自己位置を推定することができます。
def UseDistInfo(x, y, eps=1e-10):
# ランドマークの位置は既知であるとします。
x1, y1 = 10.0, 10.0
x2, y2 = 0.0, -10.0
x3, y3 = -10.0, 10.0
# 距離センサを用いてランドマークからの距離が得られた状況を考えます。
p = np.array([x, y])
p1 = np.array([x1, y1])
p2 = np.array([x2, y2])
p3 = np.array([x3, y3])
r1 = np.linalg.norm(p1 - p)
r2 = np.linalg.norm(p2 - p)
r3 = np.linalg.norm(p3 - p)
# ランドマークの位置、ランドマークからの距離 を用いて現在位置を推定できます。
D = (x1 - x2) * (y2 - y3) - (y1 - y2) * (x2 - x3)
a = np.array([
[y2 - y3, -y1 + y2],
[-x2 + x3, x1 - x2]])
b = np.array([
[x1 ** 2 - x2 ** 2 + y1 ** 2 -y2 ** 2 + r2 ** 2 - r1 ** 2],
[x2 ** 2 - x3 ** 2 + y2 ** 2 -y3 ** 2 + r3 ** 2 - r2 ** 2]])
res = a.dot(b) / (2 * D)
# 推定結果を検証します。
assert(abs(x - res[0]) < eps)
assert(abs(y - res[1]) < eps)
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
x, y = robot.GetPos()
# print('{}\t{}'.format(x, y))
UseDistInfo(x, y)
# ...以下略
角度情報を用いる方法
三つのランドマークの位置が既知であるとします。ロボットの進行方向からランドマークまでの角度をもとに自己位置を推定できます。角度情報は、例えば全方位カメラからの画像を処理することで取得できます。ランドマークは、以下の beta の値が 360 度の範囲で均等に配置されているようなものを選ぶと推定精度が高くなります。
def UseAngleInfo(x, y, eps=1e-10):
# ランドマークの位置は既知であるとします。
x1, y1 = 10.0, 10.0
x2, y2 = 0.0, -10.0
x3, y3 = -10.0, 10.0
# 進行方向の角度は任意の値でよいです。仮に以下の値であるとします。
phi = np.pi / 4.0
# センサ情報によって、「進行方向」と「ランドマークのある方向」の角度差が得られた状況を考えます。
beta1 = np.arctan((x1 - x) / (y1 - y)) - phi
beta2 = np.arctan((x2 - x) / (y2 - y)) - phi
beta3 = np.arctan((x3 - x) / (y3 - y)) - phi
# ランドマークの位置、進行方向からランドマークまでの角度 を用いて現在位置を推定できます。
d12 = np.tan(beta2 - beta1)
d23 = np.tan(beta3 - beta2)
d31 = np.tan(beta1 - beta3)
a12 = x1 + x2 + (y2 - y1) / d12
a23 = x2 + x3 + (y3 - y2) / d23
a31 = x3 + x1 + (y1 - y3) / d31
b12 = y1 + y2 - (x2 - x1) / d12
b23 = y2 + y3 - (x3 - x2) / d23
b31 = y3 + y1 - (x1 - x3) / d31
c12 = x1 * x2 + y1 * y2 - (x2 * y1 - x1 * y2) / d12
c23 = x2 * x3 + y2 * y3 - (x3 * y2 - x2 * y3) / d23
c31 = x3 * x1 + y3 * y1 - (x1 * y3 - x3 * y1) / d31
D = (a23 - a12) * (b31 - b23) - (b23 - b12) * (a31 - a23)
xx = ((b31 - b23) * (c23 - c12) - (b23 - b12) * (c31 - c23)) / D
yy = ((a31 - a23) * (c23 - c12) - (a23 - a12) * (c31 - c23)) / -D
# 推定結果を検証します。
assert(abs(x - xx) < eps)
assert(abs(y - yy) < eps)
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
x, y = robot.GetPos()
# print('{}\t{}'.format(x, y))
UseAngleInfo(x, y)
# ...以下略
内界センサを使う場合
車輪の回転速度を時間積分していくことでロボットの現在位置と角度を推定することができます。内界センサを利用して推定する方法をデッドレコニング (Dead Reckoning) とよびます。マイクロマウス等で利用される加速度センサ、ジャイロ、ロータリーエンコーダは内界センサです。車輪と地面の間のすべり等が存在するため誤差が蓄積されていきます。定期的な補正が必要です。
ロボット重心の座標を 、、進行方向の角度を とすると、車輪の回転速度 を用いて以下の式が成り立ちます。車輪が地面と接触している箇所における速度は です。 は車体重心から車輪までの距離、 は車輪の半径です。
台形近似による数値積分を行うことでロボットの現在位置と角度を推定できます。
すべりを抑えるためにパラメータを調整します。
#contact.surface.mu = 99
contact.surface.mu = 999
#contact.surface.bounce = 1.0
contact.surface.bounce = 0.0
ODE の drawstuff のステップ毎に数値積分します。
def __StepCallback(pause):
# ロボットの速度制御
robot.Control()
# 現在座標を表示
# x, y = robot.GetPos()
# print('{}\t{}'.format(x, y))
robot.UseJointAngleRates(tDelta=0.01)
# ...以下略
インスタンス変数としてステップ毎の積分結果を保持します。
class Robot(object):
def __init__(self, world, space, lx=0.4, ly=0.4, lz=0.2, rLR=0.11, rFB=0.05, w=0.02, mBase=9.4, mLR=0.2, mFB=0.1, px=0.0, py=0.0, pz=0.2):
self.__velL = 0.0
self.__velR = 0.0
self.__base = self.__AddBox(world, space, lx=lx, ly=ly, lz=lz, m=mBase, px=px, py=py, pz=pz)
self.__wheelL = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=-(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelR = self.__AddCylinder(world, space, r=rLR, l=w, m=mLR, px=(lx + w) / 2.0, py=0.0, pz=(pz - lz / 2.0))
self.__wheelF = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__wheelB = self.__AddCylinder(world, space, r=rFB, l=w, m=mFB, px=0.0, py=-(ly / 2.0 - rFB), pz=(pz - lz / 2.0 - rFB))
self.__jointL = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelL), odepy.dGeomGetPosition(self.__wheelL))
self.__jointR = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelR), odepy.dGeomGetPosition(self.__wheelR))
self.__jointF = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelF), odepy.dGeomGetPosition(self.__wheelF))
self.__jointB = self.__AddJoint(world, odepy.dGeomGetBody(self.__base), odepy.dGeomGetBody(self.__wheelB), odepy.dGeomGetPosition(self.__wheelB))
# ロボットの重心から車輪までの距離、車輪の半径
self.__d = (lx + w) / 2.0
self.__rLR = rLR
# 左車輪と右車輪の回転速度
self.__omegaL = 0.0
self.__omegaR = 0.0
# 車体の進行方向の回転角度、現在位置
self.__phi = 0.0
self.__x = px
self.__y = py
def UseJointAngleRates(self, tDelta):
# 現時点における車輪の角速度
omegaL = odepy.dJointGetHingeAngleRate(self.__jointL)
omegaR = odepy.dJointGetHingeAngleRate(self.__jointR)
# 台形近似による数値積分
def fnPhi(omegaL, omegaR, r, d):
return r * (omegaL - omegaR) / (2.0 * d)
def fnX(omegaL, omegaR, phi, r):
return r * (omegaL + omegaR) * np.sin(phi) / 2.0
def fnY(omegaL, omegaR, phi, r):
return r * (omegaL + omegaR) * np.cos(phi) / 2.0
phi = self.__phi + (fnPhi(self.__omegaL, self.__omegaR, self.__rLR, self.__d) +
fnPhi(omegaL, omegaR, self.__rLR, self.__d)) * tDelta / 2.0
xx = self.__x + (fnX(self.__omegaL, self.__omegaR, self.__phi, self.__rLR) +
fnX(omegaL, omegaR, phi, self.__rLR)) * tDelta / 2.0
yy = self.__y + (fnY(self.__omegaL, self.__omegaR, self.__phi, self.__rLR) +
fnY(omegaL, omegaR, phi, self.__rLR)) * tDelta / 2.0
# 次のステップで利用するために結果を保存
self.__omegaL = omegaL
self.__omegaR = omegaR
self.__phi = phi
self.__x = xx
self.__y = yy
# 推定結果の確認
x, y = self.GetPos()
print(phi * 180.0 / np.pi)
# print('{}\t{}'.format(xx, x))
# print('{}\t{}'.format(yy, y))
0
関連記事
- 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 インストール 適当なパッケージ
