OpenRAVE IKFast で IK を解くサンプル

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

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

実用ロボット開発のためのROSプログラミング

タミヤ楽しい工作シリーズ No.104 ブルドーザー工作基本セット (70104)

ロベールのC++入門講座

サンハヤト SAD-101 ニューブレッドボード

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

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])