OpenRAVE IKFast で IK を解くサンプル

[履歴] [最終更新] (2019/08/21 22:20:37)

プログラミング/IoT の関連商品 (Amazonのアソシエイトとして、当メディアは適格販売により収入を得ています。)

スーパーマリオメーカー 2 -Switch

ゼルダの伝説ブレスオブザワイルド - Switch

モンスターハンターワールド:アイスボーンマスターエディション - PS4

詳解 OpenCV 3 ―コンピュータビジョンライブラリを使った画像処理・認識

サインスマート DSO202 Note II ポケットサイズデジタルオシロスコープ Nano ARM タッチパネル操作 8MBメモリストレージ 2MHz 10Mps

tybot

概要

産業用ロボットには、垂直多関節ロボット (Articulated) や水平多関節ロボット (SCARA) があります。ロボットにはジョイントとリンクがあり、ジョイントの関節値を指定することで制御できます。

関節値を指定して、位置姿勢を求める場合を順運動学 (Forward Kinematics) とよびます。
位置姿勢を指定して、関節値を求める場合を逆運動学 (Inverse Kinematics) とよびます。

例えば、Motion planning でロボットのアームの先端にあるデバイス (エンドエフェクタ、マニピュレータ) の位置姿勢を指定したときに取り得る関節値を求めるためには、後者の IK を解きます。IK を解くためには OpenRAVE の IKFast モジュールが利用できます。

目標点 (IK goal) の準備

ロボットに行わせたいタスクによって、目標となるマニピュレータの位置姿勢 IK goal を決めます。これを動的に生成する必要がある場合もありますが、ここではマニピュレータを固定位置に動かしたいとします。向きを指定する場合は Transform6D で解きます。向きを指定しない場合は Translation3D で解きます。

また、OpenRAVE Custom XML Format で記述された puma ロボットを利用することにします。

openrave.py -i src/robots/puma.robot.xml

ロボット名を取得

robot.GetName()

ジョイントの数を取得 (DOF; degrees of freedom)

robot.GetDOF()

ジョイントの値を取得、設定

robot.GetDOFValues()
robot.SetDOFValues([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])

リンクの変換行列を取得

T = robot.GetLinks()[1].GetTransform()

変換行列で位置姿勢を設定

from openravepy import matrixFromAxisAngle
from numpy import pi

# ワールド座標系における Z 軸まわりに 45度回転
Tz = matrixFromAxisAngle([0, 0, pi/4])
robot.SetTransform(Tz.dot(robot.GetTransform()))

ログレベルを変更

RaveSetDebugLevel(DebugLevel.Verbose)

環境を複数扱うことができます。

env2 = Environment()
robot2 = env2.ReadRobotXMLFile(filename='src/robots/puma.robot.xml')
env2.AddKinBody(robot2)

関節値に設定できる値の範囲を調べる

manip = robot.GetActiveManipulator()
lower,upper = robot.GetDOFLimits(manip.GetArmIndices())

ロボット自身の衝突および環境との衝突を調べる

robot.CheckSelfCollision()
env.CheckCollision(robot)

環境のファイル保存

env.Save('newenv.zae', Environment.SelectionOptions.Everything)

IK を解く

マニピュレータが目標点にあるときに取ることができる関節値を、周囲の環境との衝突も考慮して計算します。解は複数存在し得ります。

Transform6D

マニピュレータの位置だけでなく向きも指定して IK を解きます。

# マニピュレータを取得
manip = robot.GetActiveManipulator()

# 初回のみ ikmodel を生成します。時間がかかりますが一度計算しておけば IK を解く際に再利用できます。
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=IkParameterization.Type.Transform6D)
ikmodel.autogenerate()

# 目標点を設定します。
Tgoal = numpy.array([
    [ 0, -1,  0, -0.21],
    [-1,  0,  0,  0.04],
    [ 0,  0, -1,  0.92],
    [ 0,  0,  0,     1]])

# 衝突を考慮しつつ IK を解きます。
sol = manip.FindIKSolution(Tgoal, IkFilterOptions.CheckEnvCollisions)

# 関節値をロボットに設定してみます。
robot.SetDOFValues(sol, manip.GetArmIndices())
env.UpdatePublishedBodies()

関節値を指定すると、確かに目標点に指定した位置姿勢にマニピュレータが移動されました。

tManipToWorld = manip.GetEndEffectorTransform()

In [25]: tManipToWorld[:3,:3]
Out[25]: 
array([[ 6.66133815e-15, -1.00000000e+00, -4.31315374e-15],
       [-1.00000000e+00, -6.66133815e-15,  1.57684927e-15],
       [-1.57684927e-15,  4.31315374e-15, -1.00000000e+00]])

In [26]: tManipToWorld[:,3]
Out[26]: array([-0.21,  0.04,  0.92,  1.  ])

Translation3D

マニピュレータの位置だけを指定して IK を解きます。複数の解が存在し得ります。

# マニピュレータを取得
manip = robot.GetActiveManipulator()

# 今回は Translation3D を指定します。
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterization.Type.Translation3D)
ikmodel.autogenerate()

# 目標点を設定します。
# ここでは以下のような関節値を設定したときの
robot.SetDOFValues([2.58, 0.547, 1.5, -0.7], [0,1,2,3]) # set the first 4 dof values
# マニピュレータの位置を目標点とします。向きは指定しません。
tManipToWorld = manip.GetEndEffectorTransform()
ikparam = IkParameterization(tManipToWorld[0:3,3], ikmodel.iktype) # build up the translation3d ik query

# IK を解きます。
sols = manip.FindIKSolutions(ikparam, IkFilterOptions.CheckEnvCollisions)
raveLogInfo('%d solutions' % len(sols))

# 目標点の位置に印を表示してみます。
h = env.plot3(tManipToWorld[0:3,3], 10) # plot one point

# 関節値をロボットに設定してみます。
for sol in sols:
    robot.SetDOFValues(sol, manip.GetArmIndices())
    env.UpdatePublishedBodies()
    time.sleep(10.0/len(sols))

ikmodel を保存して次回以降に利用するためには以下のようにします。

In [13]: ikmodel.save()
openravepy.databases.inversekinematics: save, inversekinematics generation is done, compiled shared object: /home/xxx/.openrave/kinematics.9559e67e82872bf8fa24cbb50bdc4388/ikfast0x1000004a.Translation3D.x86_64.0_1_2_f3_4_5.so

以下のようにして読み込みます。ikmodel.autogenerate() は不要です。

In [2]: ikmodel.load()
openravepy.databases.inversekinematics: GetDefaultIndices, found 3-intersection centered on index 4
Out[2]: True

RRT で経路計画を行う

OpenRAVE には RRT (Rapidly-exploring random tree) でロボットの軌道 trajectory を計画する機能があります。これを利用して、IK で解いた関節値になるようにロボットを移動させてみます。OpenRAVE には RRT を両端から行う BiRRT (Bi-directional RRTs) が実装されています。

# create the interface for basic manipulation programs
manipprob = interfaces.BaseManipulation(robot)

# DOF Value を指定します。
manipprob.MoveManipulator(goal=[-1, 0, 1, 1, 1, 1])

In [29]: robot.GetDOFValues()
Out[29]: 
array([-1.00000000e+00, -1.11022302e-15,  1.00000000e+00,  1.00000000e+00,
        1.00000000e+00,  1.00000000e+00,  2.11177436e-14])

IK と RRT をまとめて行う

マニピュレータの位置姿勢を Transform6D で指定して IK を解いて関節値を得て、RRT で関節値になるようにロボットを移動させる、という二つのことをまとめて行うためには以下のようにします。

ikmodel = databases.inversekinematics.InverseKinematicsModel(robot, iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

manipprob = interfaces.BaseManipulation(robot)
Tgoal = numpy.array([
    [ 0, -1,  0, -0.21],
    [-1,  0,  0,  0.04],
    [ 0,  0, -1,  0.92],
    [ 0,  0,  0,     1]])

res = manipprob.MoveToHandPosition(matrices=[Tgoal], seedik=10) # call motion planner with goal joint angles

軌道 trajectory を取得

res = manipprob.MoveToHandPosition(matrices=[Tgoal], seedik=10) # call motion planner with goal joint angles

ロボットを実際に移動させる前に、計画した軌道を取得することができます。

traj = manipprob.MoveToHandPosition(matrices=[Tgoal], execute=False, outputtrajobj=True)

軌道は複数の waypoint で区切られています。

raveLogInfo('traj has %d waypoints, last waypoint is: %s' % (traj.GetNumWaypoints(), repr(traj.GetWaypoint(-1))))

ロボットを移動させるためには以下のようにします。

robot.GetController().SetPath(traj)

各 waypoint における関節値を取得するためには以下のようにします。関節値の他に、速度やタイムスタンプなどの情報が含まれています。

for i in range(traj.GetNumWaypoints()):
    data=traj.GetWaypoint(i)
    dofvalues = traj.GetConfigurationSpecification().ExtractJointValues(data, robot, robot.GetActiveDOFIndices())
    raveLogInfo('waypoint %d is %s' % (i, dofvalues))

OpenRAVE の関連機能

動力学シミュレータ ode (Open Dynamics Engine) の利用

# ode の読み込み
physics = RaveCreatePhysicsEngine(env, 'ode')
env.SetPhysicsEngine(physics)

# シミュレーションを停止しておきます。
env.StopSimulation()

# 重力を設定します。
physics.SetGravity(numpy.array((0, 0, -9.8)))

# ロボットの土台が落下しないようにしてみます。
robot.GetLinks()[0].SetStatic(True)

# シミュレーションを開始してみます。
env.StartSimulation(timestep=0.001)

# 5秒間だけジョイントの一つにトルクを加えてみます。右方向に回転します。
robot.GetDOFTorqueLimits()
for i in range(500):
    robot.SetDOFTorques([100, 0, 0, 0, 0, 0, 0], True)
    time.sleep(0.01)

# シミュレータを終了します。
env.SetPhysicsEngine(None)

IK goal を動的に生成する例

IK goal を動的に生成する例として、ワークをマニピュレータで把持する場合を考えます。ロボットとマグカップが存在する環境をロードします。

openrave.py -i src/data/lab1.env.xml

マグカップの一つを把持対象として設定します。把持対象とマニピュレータの組で Grasping Model を生成します。対象を把持できる可能性のあるマニピュレータの姿勢をすべて生成するため、モデルの作成には時間がかかります。Box などの簡単な形状の把持対象よりも Cylinder などの場合の方が時間がかかります。

target = env.GetKinBody('mug1')
gmodel = databases.grasping.GraspingModel(robot, target)
if not gmodel.load():
    gmodel.autogenerate()

モデルを解いて把持セットを出力します。複数の解が存在し得りますが、一つだけ取得してみます。存在しないこともあります。

validgrasps, validindicees = gmodel.computeValidGrasps(returnnum=1)

このように、把持セットは把持対象に応じて動的に生成されます。動的に生成された把持セットが IK goal となり ikmodel に入力されます。RRT まで含めて IK を解いてみます。把持対象を把持するための軌道において、把持対象とロボットの衝突は回避 collisionfree=True する設定にしてみます。移動後に把持する際には衝突します。

gmodel.moveToPreshape(validgrasps[0])
Tgoal = gmodel.getGlobalGraspTransform(validgrasps[0], collisionfree=True)
manipprob = interfaces.BaseManipulation(robot)
manipprob.MoveToHandPosition(matrices=[Tgoal])