ノートブック

古き良きスタイルのノートブックです。

ROSでのKDL逆運動学ライブラリについて

f:id:primitivem:20170928154903p:plain

ROSでロボットアームを動かすとき、エンドエフェクターの位置からロボットの各関節の角度を求めるいわゆる逆運動学を使いたいことがあったので KDLという逆運動学のライブラリを使いました。
この記事では単純なロボットアームのモデルをRViz上に可視化して関節角度を指定して動かしアーム先端の座標情報を確認する。そしてその先端の座標から逆にKDLを使って 指定した関節角度と同じ値を逆運動学で求めるという内容です。

以下の内容は Ubuntu 16.04 と ROS kinetic の環境で実行したものです。

このURLにあるROSパッケージ https://github.com/embodmt/Gazebo_ROS/tree/master/simple_arm_2
これはシンプルな5つの関節からなるロボットモデルが記述された簡単なパッケージで、この中にあるlaunchファイル

rviz.launch (https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/launch/rviz.launch)
を起動すれば

$ roslaunch simple_arm_2 rviz.launch

ロボットアームがRViz上に表示されます。ただ単純にURDFファイルをRViz上に可視化するだけのlaunchファイルです。 RVizを起動した状態で以下のようなpythonスクリプトを別ターミナルから起動します。

publish_jointstate.py (https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/scripts/publish_jointstate.py)

import sys
import rospy
from sensor_msgs.msg import JointState

def node():
    rospy.init_node('publish_jointstate', anonymous=True)
    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
    rate = rospy.Rate(100)
    
    while not rospy.is_shutdown():
        msg = JointState()
        msg.header.stamp = rospy.Time.now()
        msg.position = [ 0.0, -0.2, -0.2, -0.2, 0.0 ]
        msg.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0 ]
        msg.effort = [ 0.0, 0.0, 0.0, 0.0, 0.0 ]
        msg.name = ["joint1", "joint2", "joint3", "joint4", "joint5"]

        pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        node()   
    except rospy.ROSInterruptException:
        pass

このようにして起動。

$ rosrun simple_arm_2 publish_jointstate.py

このスクリプトではRViz上に表示されているロボットの各関節角を指定しています。 joint1から順番に”0.0, -0.2, -0.2, -0.2, 0.0”の関節角の情報をpublishしているのがわかります。
するとRVizでは以下のようにロボットモデルの関節が曲がります。ちょうど0.2ずつ曲がっていっているね!

f:id:primitivem:20170928154903p:plain

このスクリーンショットの黄色でマーキングしたところに、link4の3次元の位置と姿勢の6DoF情報が表示されていることが確認できます。 このlink4の位置と姿勢の座標からKDLライブラリを使って逆に関節角を求めることができます。

KDLは私の環境では以下のコマンドでインストールしました。

$ sudo apt-get install ros-kinetic-kdl-parser-py

以下のスクリプトを使って逆運動学の解を求めます。

inverse_kinematics.py (https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/scripts/inverse_kinematics.py)

import kdl_parser_py.urdf  
import PyKDL as kdl

#setup KDL
filename = "./src/simple_arm_2/urdf/simple_arm.urdf"  
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
chain = tree.getChain("world", "link4")  
t = kdl.Tree(tree)  
fksolverpos = kdl.ChainFkSolverPos_recursive(chain)  
iksolvervel = kdl.ChainIkSolverVel_pinv(chain)  
iksolverpos = kdl.ChainIkSolverPos_NR(chain,fksolverpos,iksolvervel)

q_zero=kdl.JntArray(chain.getNrOfJoints())  
F2 = kdl.Frame(kdl.Rotation.Quaternion(0,-0.29552,0,0.955337),kdl.Vector(-0.0529279,0,0.201101))  
q_solved=kdl.JntArray(chain.getNrOfJoints())  
iksolverpos.CartToJnt(q_zero,F2,q_solved)  
print "joint angles"  
print q_solved  

このスクリプトでは まず kdl_parser_py.urdf.treeFromFile() で urdfファイルをkdlに読み込んで各種ソルバーを準備します。そして kdl.Frame() で先ほどRVizで調べたlink4の6DoFの情報(上のスクリーンショットの黄色でマーキングした数値)を回転姿勢と直行座標にわけて入力し、 iksolverpos.CartToJnt() でデカルト座標からジョイント角への変換が行えます。

実行します。

$ rosrun simple_arm_2 inverse_kinematics.py

このスクリプトを実行した結果はこのように出力され

joint angles  
    0
-0.199975
-0.200051
-0.199973

この結果を見ると、publish_jointstate.pyで指定した”0.0, -0.2, -0.2, -0.2”の回転角とほぼ一致していることがわかります。

以上、ROSでのKDL逆運動学ライブラリの簡単な使い方についての記事でした。