作者: 古川誠
from openravepy import Environment,poseFromMatrix, with_destroy
from numpy import eye
from numpy.random import rand
env = Environment()
env.SetViewer('qtcoin')
env.AddKinBody(env.ReadKinBodyXMLFile(filename='data/mug1.kinbody.xml'))
env.AddKinBody(env.ReadKinBodyXMLFile(filename='data/mug2.kinbody.xml'))
bodies = env.GetBodies()
body1 = bodies[0]
body1.SetTransform(eye(4))
body2 = env.GetKinBody('mug2')
body2.SetTransform([1,0,0,0,0,0,0])
raw_input('キーを押すとXに+0.2移動し,移動前と移動後の変換行列を出力します.')
tran1 = body1.GetTransform()
print '移動前'
print tran1
tran1[0,3] = 0.2
body1.SetTransform(tran1)
print '移動後'
print tran1
raw_input('キーを押すとYに+0.1移動し,移動前と移動後のポーズを出力します.')
pose1 = poseFromMatrix (tran1)
print '移動前'
print 'pose: ',pose1
pose1[5] = 0.1
body1.SetTransform(pose1)
print '移動後'
print 'pose: ',pose1
while True:
raw_input("キーを押すとランダムに移動します.")
tran1[0:3,3] = 0.2*(rand(3)-0.5)
body1.SetTransform(tran1)
print "X=%f Y=%f Z=%f"%(tran1[0,3],tran1[1,3],tran1[2,3])
print 'pose: ',poseFromMatrix(tran1)
openrave.py --example tutorial_002
チュートリアルを実行すると,OpenRAVEのGUIが立ち上がり,下のような画像が現れます.
いま同じ場所に2つのマグカップを読み出してしまったため,重なってしまいました.このため,青いマグカップを移動させてみましょう.n 物体を移動するには変換行列を使う方法と,poseを使う方法の2種類があります.まずは変換行列を使って移動させます.n キーを1回押すと青いマグカップが移動し,移動前後の変換行列を出力します.
次にposeを使って移動させます.キーを1回押すと青いマグカップが再度移動し,移動前後のposeを出力します.
この次からは,キーを押す度に青いマグカップがランダムに移動します.
from openravepy import Environment,poseFromMatrix
from numpy.random import rand
env.AddKinBody(env.ReadKinBodyXMLFile(filename='data/mug1.kinbody.xml'))
env.AddKinBody(env.ReadKinBodyXMLFile(filename='data/mug2.kinbody.xml'))
bodies = env.GetBodies()
body1 = bodies[0]
body2 = env.GetKinBody('mug2')
body2.SetTransform([1,0,0,0,0,0,0])
raw_input('キーを押すとXに+0.2移動し,移動前と移動後の変換行列を出力します.')
tran1 = body1.GetTransform()
print '移動前'
print tran1
tran1[0,3] = 0.2
body1.SetTransform(tran1)
print '移動後'
print tran1
移動前
[[ 1. 0. 0. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
移動後
[[ 1. 0. 0. 0.2]
[ 0. 1. 0. 0. ]
[ 0. 0. 1. 0. ]
[ 0. 0. 0. 1. ]]
raw_input('キーを押すとYに+0.1移動し,移動前と移動後のポーズを出力します.')
pose1 = poseFromMatrix (tran1)
print '移動前'
print 'pose: ',pose1
pose1[5] = 0.1
body1.SetTransform(pose1)
print '移動後'
print 'pose: ',pose1
移動前
pose: [ 1. 0. 0. 0. 0.2 0. 0. ]
移動後
pose: [ 1. 0. 0. 0. 0.2 0.1 0. ]
while True:
raw_input("キーを押すとランダムに移動します.")
tran1[0:3,3] = 0.2*(rand(3)-0.5)
body1.SetTransform(tran1)
print "X=%f Y=%f Z=%f"%(tran1[0,3],tran1[1,3],tran1[2,3])
print 'pose: ',poseFromMatrix(tran1)