Skip to content

OpenRAVEのIKFastでMotoman用のMoveIt!プラグインを作成する

Jump to bottom Edit New page
Masaru Morita edited this page Apr 19, 2016 · 5 revisions

OpenRAVEとmoveit_ikfastのインストール

Mastering ROS for Robotics Programming 第11章 Understanding MoveIt! IKFast plugin 以降に記載された通りにインストールする．

COLLADAファイルの作成

xacro → URDF

対象xacroファイルはmotoman_project/motoman_description/robots/sia5/sia5.urdf.xacro

$ rosrun xacro xacro.py sia5.urdf.xacro > sia5.urdf

URDF → COLLADA

$ rosrun collada_urdf urdf_to_collada sia5.urdf motoman_sia5.dae
  • 後々の命名規約と合わせるため，一応robot名_型式.urdf としておく．

チェック

$ openrave motoman_sia5.dae
  • motomanが出てきたら合格．
$ openrave-robot.py motoman_sia5.dae --info links
could not import scipy.optimize.leastsq
name      index parents  
-------------------------
world     0              
base_link 1     world    
link_s    2     base_link
link_l    3     link_s   
link_e    4     link_l   
link_u    5     link_e   
link_r    6     link_u   
link_b    7     link_r   
link_t    8     link_b   
tool0     9     link_t   
-------------------------
  • こんなのが出れば合格．

IKFastによる逆運動学Solverの生成

$ python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=motoman_sia5.dae --iktype=transform6d --baselink=1 --eelink=9 --freeindex=6 --savefile=output_ikfast91.cpp

  • IKFastは6軸までの逆運動学しか解けないけど，sia5は7軸．

  • そこで，freeindexを一つ指定することで解けるようにする．6(u軸)をfreeにしたら解けた．e, r, bで試したらダメだった．（よく理解していない）

  • output_ikfast91.cpp を下記のように修正しておく．こうしないと，後のmoveit_ikfastの際に，ver. No. のパーサが例外を投げて先に進めなくなる．

  • 61にする理由は，そうしないとmoveit_ikfastが変換を実行してくれないので．

// l.15
/// ikfast version 0x10000048 generated on 2016-04-19 20:59:47.751149
↓
/// ikfast version 61 generated on 2016-04-19 20:59:47.751149

// l.26
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==0x10000048);
↓
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);

MoveIt!プラグインの作成

パッケージ作成

$ catkin_create_pkg motoman_sia5_moveit_plugins
$ cp "openraveをクローンしたフォルダ"/openrave/python/ikfast.h "motoman_project"/motoman_sia5_moveit_plugins/include
$ cd motoman_ws(ご自身の環境のもの)
$ catkin_make

ファイル名・ロボット名変更

  • moveit_ikfastによって命名規約が厳密に決まっているため
  • こうしないとmoveit_ikfastが動かないので，やむえず変えました．
  • ですので，マージする場合は互換性の確認をお願いします．従来との変更はここだけで，他はファイル追加です．
$ cd motoman_sia5_moveit_config/config
$ cp sia5.srdf motoman_sia5.srdf
  • motoman_description/robots/sia5/sia5.urdf.xacro
<robot name="sia5" xmlns:xacro="http://ros.org/wiki/xacro">
↓
<robot name="motoman_sia5" xmlns:xacro="http://ros.org/wiki/xacro">
  • 注意
  • ここでrobot名を変えているので，

moveit_ikfastによるパッケージ用ラッパーの作成

  • 変換元の Solver cppファイル名は，motoman_sia5_arm_ikfast_solver.cppとしておく．
  • [ロボット名_型式_planning group名]_ikfast_solver.cpp
  • そうしないと，処理できない．moveit_ikfastはこの命名規約に沿って厳密に色々処理するっぽい．
$ rosrun moveit_ikfast create_ikfast_moveit_plugin.py motoman_sia5 arm motoman_sia5_moveit_plugins motoman_sia5_arm_ikfast_solver.cpp 

$ rosrun moveit_ikfast create_ikfast_moveit_plugin.py [ロボット名_型式] [腕の方のplanning group名] [プラグインパッケージ名] [変換元 Solver cpp ファイル名]
$ cd motoman_ws(ご自身の環境のもの)
$ catkin_make

IKFastプラグインの適用

  • motoman_sia5_moveit_config/config/kinematics_ikfast.yaml を確認する．
arm:
  kinematics_solver: motoman_sia5_arm_kinematics/IKFastKinematicsPlugin
  kinematics_solver_attempts: 3
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  • motoman_moveit/launch/sia5_moveit_planning_execution_ikfast.launch
<launch>
  <include file="$(find motoman_sia5_moveit_config)/launch/moveit_planning_execution.launch">
    <arg name="load_robot_description" value="true"/>
    <arg name="urdf_model" value="$(find motoman_description)/robots/sia5/sia5.urdf.xacro"/>
    <arg name="srdf_model" value="$(find motoman_sia5_moveit_config)/config/motoman_sia5.srdf"/>
    <arg name="joint_limits_config" value="$(find motoman_sia5_moveit_config)/config/joint_limits.yaml"/>
    <arg name="kinematics_config" value="$(find motoman_sia5_moveit_config)/config/kinematics_ikfast.yaml"/>
    <arg name="controllers_config" value="$(find motoman_sia5_moveit_config)/config/controllers.yaml"/>
    <arg name="use_depth_sensor" value="false"/>
    <arg name="rviz_config" value="$(find motoman_moveit)/launch/rviz/moveit_sia5.rviz"/>
  </include>
</launch>

シミュレーション実行

Gazebo

roslaunch motoman_gazebo sia5_empty_world.launch

MoveIt!

roslaunch motoman_moveit sia5_moveit_planning_execution_ikfast.launch
  • メッセージを確認．
 * /robot_description_kinematics/arm/kinematics_solver: motoman_sia5_arm_...
 * /robot_description_kinematics/arm/kinematics_solver_attempts: 3
 * /robot_description_kinematics/arm/kinematics_solver_search_resolution: 0.005
 * /robot_description_kinematics/arm/kinematics_solver_timeout: 0.005
  • motoman_sia5_arm_...の部分は，motoman_sia5_arm_kinematics/IKFastKinematicsPluginに相当．
  • kdl_kinematics...で始まっていないということは，IKFastのプラグインが読めているということ．

遊ぶ

  • 一回planとExecuteを空で打たないとうまく行かないことがある．原因は不明．
  • その後は，普通にインタラクティブマーカを動かせばシャキシャキ遊べる．
  • 現状，苦労の割には，恩恵が良く分からないw
Add a custom footer
Add a custom sidebar
Clone this wiki locally