Successfully reported this slideshow.
We use your LinkedIn profile and activity data to personalize ads and to show you more relevant ads. You can change your ad preferences anytime.

ROS JAPAN Users Group Meetup 03

9,427 views

Published on

ROS JAPAN Users Group Meetup 03

  1. 1. 第三回ROS勉強会 @東京 2014年8月30日  千葉工業大学 工学部 4年  ロボット設計・制御研究室 前川大輝
  2. 2. 目次 1. 自己紹介 2. 今日やること 3. NEXTAGEについて 4. 下準備 5. シミュレータ環境のテスト 6. MoveIt! + RViz 7. MoveIt!のPython API 8. 目標値をトピックでやりとりする
  3. 3. 自己紹介 ■千葉工業大学のロボカップ開発チーム「CITBrains」  のリーダー ■教育用マイコンボードの開発プロジェクトのリーダー  → ソフトウェアのライブラリ開発(2011年 ~ 2012年) ■農場用メータの自動読み取り技術  → 企業と共同開発(2013年11月 ~ 2014年8月) ■今年からつくばチャレンジの開発に参加 ■この勉強会を含む、OSSの活動
  4. 4. ロボカップ世界大会の様子
  5. 5. ブラジルで行われたロボカップ世界大会2014の成績 ■テクニカルチャレンジ1位 ■4on4(トーナメント形式)優勝 ■ベストヒューマノイド1位(ルイ・ヴィトン・カップ)
  6. 6. 今日やること 13:00 ~ 15:50 NEXTAGEで学ぶMoveIt!入門(前川) 16:00 ~ 17:50 NEXTAGEのtfとカメラ(近藤) 18:30 ~ 20:30 懇親会 鳥一新宿西口店
  7. 7. 次回のNEXTAGEハッカソンに繋がる話をします #ついでにROSに関する基礎的なことも
  8. 8. 前回のNEXTAGEハッカソンの様子
  9. 9. NEXTAGEについて ※詳しくはNEXTAGEの解説スライド(第2回ROS勉強会) https://speakerdeck.com/youtalk/nextage-openwoshi-tutarospuroguramingu
  10. 10. ハードウェアの構成
  11. 11. ソフトウェアの構成
  12. 12. 下準備 最低限必要なセットアップを行う
  13. 13. ■rosdepの初期設定を行っていない場合 $ sudo rosdep init $ rosdep update ■ROSの環境変数を追加するコマンドをbashrcに追記して いない場合 $ echo “source /opt/ros/hydro/setup.bash” >> ~/.bashrc
  14. 14. ■ワークスペースを作成していない場合 公式ドキュメント : http://wiki.ros.org/catkin/Tutorials/create_a_workspace $ source /opt/ros/hydro/setup.bash $ mkdir -p ~/catkin_ws/src $ cd ~/catkin_ws/src $ catkin_init_workspace $ cd ~/catkin_ws/ $ catkin_make $ echo “source ~/catkin_ws/devel/setup.bash” >> ~/.bashrc $ source ~/.bashrc
  15. 15. シミュレータ環境のテスト
  16. 16. ■必要なパッケージのインストール $ sudo apt-get install ros-hydro-rtmros-nextage ros-hydro-rtshell- core ■シミュレータの起動などに使うコマンドのエイリアスに関する設定 $ echo "source `rospack find openrtm_tools`/scripts/rtshell-setup. sh" >> ~/.bashrc $ source ~/.bashrc
  17. 17. ■NEXTAGEのシミュレータを起動 $ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch
  18. 18. MoveIt! + RViz RVizとMoveIt!を使ってグラフィカルなモーションプランニン グ環境を構築する
  19. 19. MoveIt!の概要
  20. 20. MoveIt!のユーザーインタフェース ■move_groupが提供する機能にアクセスする方法  → move_group_interfaceパッケージを用いてC++で書く  → moveit_commanderパッケージを用いてPythonで書く  → Motion Planningプラグインを用いてRVizでグラフィカル    に制御
  21. 21. ■シミュレータを起動した状態で以下のコマンドを実行 $ roslaunch nextage_moveit_config moveit_planning_execution.launch 目標値をグラフィカルに入力できる状態でRVizが起動
  22. 22. ■球体や矢印をマウスで動かして目標値を変更  → プランニング結果がリアルタイムで表示される
  23. 23. ■Executeボタンを押してプランを実行
  24. 24. ■プランを実行するとMoveGroupActionGoalメッセージ  が発行される  → シミュレータ(または実機)のアームが動作
  25. 25. ■Planning Groupをright_armに変更  → 右腕も操作できるようになる
  26. 26. ■干渉している場合二つのリンクが赤く表示される  → プランを実行できない
  27. 27. ■Planned PathのLoop Animationを有効に  → 開始状態から終了状態までのアームの軌跡を    繰り返し表示 ■Show Tailを有効にすると経路計画を可視化
  28. 28. MoveIt!のPython API MoveIt!のPython APIを使ってNEXTAGEを動作させる  #rospyの解説も含む
  29. 29. MoveIt!のユーザーインタフェース ■move_groupが提供する機能にアクセスする方法  → move_group_interfaceパッケージを用いてC++で書く  → moveit_commanderパッケージを用いてPythonで書く  → Motion Planningプラグインを用いてRVizでグラフィカル    に制御
  30. 30. ■パッケージの作成 $ cd ~/catkin_ws/src $ catkin_create_pkg nextage_moveit_planning_execution moveit_commander nextage_ros_bridge nextage_moveit_config rospy $ ls nextage_moveit_planning_execution CMakeLists.txt package.xml src/ パッケージ名 依存パッケージ
  31. 31. ■package.xml catkinに対応したパッケージのルートフォルダにある ・・・ <buildtool_depend>catkin</buildtool_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_moveit_config</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_moveit_config</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> ・・・ 雛形は自動生成されるので必要なものを後で追記する
  32. 32. ■package.xmlの例 <?xml version="1.0"?> <package> <name>hironx_tutorial</name> <version>0.1.1</version> <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer> <author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author> <author email="method_aspect_card@yahoo.co.jp">Daiki Maekawa</author> <license>BSD</license> <license>MIT</license> <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>leap_motion</build_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <build_depend>tf</build_depend> <run_depend>hironx_ros_bridge</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>leap_motion</run_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> <run_depend>tf</run_depend> <export> <rosdoc config="rosdoc.yaml" /> </export> </package>
  33. 33. ■package.xmlの例 <?xml version="1.0"?> <package> <name>hironx_tutorial</name> <version>0.1.1</version> <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer> <author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author> <author email="method_aspect_card@yahoo.co.jp">Daiki Maekawa</author> <license>BSD</license> <license>MIT</license> <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>leap_motion</build_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <build_depend>tf</build_depend> <run_depend>hironx_ros_bridge</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>leap_motion</run_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> <run_depend>tf</run_depend> <export> <rosdoc config="rosdoc.yaml" /> </export> </package> ■パッケージの名前 <name>hironx_tutorial</name> ■バージョン <version>0.1.1</version>
  34. 34. ■package.xmlの例 <?xml version="1.0"?> <package> <name>hironx_tutorial</name> <version>0.1.1</version> <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer> <author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author> <author email="method_aspect_card@yahoo.co.jp">Daiki Maekawa</author> <license>BSD</license> <license>MIT</license> <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>leap_motion</build_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <build_depend>tf</build_depend> <run_depend>hironx_ros_bridge</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>leap_motion</run_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> <run_depend>tf</run_depend> <export>  ■パッケージの説明  <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <rosdoc config="rosdoc.yaml" /> </export> </package>
  35. 35. ■package.xmlの例 <?xml version="1.0"?> <package> <name>hironx_tutorial</name> <version>0.1.1</version> <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer> <author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author> <author email="method_aspect_card@yahoo.co.jp">Daiki Maekawa</author> <license>BSD</license> <license>MIT</license> <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>leap_motion</build_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <build_depend>tf</build_depend> <run_depend>hironx_ros_bridge</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>leap_motion</run_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> <run_depend>tf</run_depend> <export> <rosdoc config="rosdoc.yaml" /> </export> </package> ■作成者のメールアドレスと名前  <maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer> <author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author> <author email="method_aspect_card@yahoo.co.jp">Daiki Maekawa</author> ■ライセンス <license>BSD</license> <license>MIT</license>
  36. 36. ■package.xmlの例 <?xml version="1.0"?> <package> <name>hironx_tutorial</name> <version>0.1.1</version> <description>Sample / demo / tutorial programs for <a href = "http://wiki.ros.org/hironx_ros_bridge">Hiro</a> and <a href = "http://wiki.ros.org/rtmros_nextage">NEXTAGE OPEN (NXO)</a> robot by Kawada Industries. All sample code that works on Hiro robot should work also on NEXTAGE OPEN. The opposite is, however, not always true (e.g. gripper operation is different in NXO). </description> <maintainer email="dev@opensource-robotics.tokyo.jp">Isaac Saito</maintainer> <author email="iiysaito@opensource-robotics.tokyo.jp">Isaac IY Saito</author> <author email="method_aspect_card@yahoo.co.jp">Daiki Maekawa</author> <license>BSD</license> <license>MIT</license> <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url> <buildtool_depend>catkin</buildtool_depend> <build_depend>geometry_msgs</build_depend> <build_depend>leap_motion</build_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <build_depend>tf</build_depend> <run_depend>hironx_ros_bridge</run_depend> <run_depend>geometry_msgs</run_depend> <run_depend>leap_motion</run_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend> <run_depend>tf</run_depend> <export> <rosdoc config="rosdoc.yaml" /> </export> </package> ■リンク付け  <url type="website">http://wiki.ros.org/hironx_tutorial</url> <url type="website">http://docs.ros.org/hydro/api/hironx_tutorial/html/</url> <url type="repository">https://github.com/tork-a/hironx_tutorial</url> <url type="bugtracker">https://github.com/tork-a/hironx_tutorial/issues</url>
  37. 37. ■依存パッケージのインストール $ rosdep install nextage_moveit_planning_execution package.xml <buildtool_depend>catkin</buildtool_depend> <build_depend>moveit_commander</build_depend> <build_depend>nextage_moveit_config</build_depend> <build_depend>nextage_ros_bridge</build_depend> <build_depend>rospy</build_depend> <run_depend>moveit_commander</run_depend> <run_depend>nextage_moveit_config</run_depend> <run_depend>nextage_ros_bridge</run_depend> <run_depend>rospy</run_depend>
  38. 38. ■moveit_commanderのサンプルコード作成 $ vi scripts/moveit_command_sender.py
  39. 39. ■スクリプトをPythonとして実行させる(ROSノードに必須) #!/usr/bin/env python ■必要なモジュールをインポート import moveit_commander import rospy import geometry_msgs.msg import copy
  40. 40. ■セットアップ & 基本情報の取得 def main(): rospy.init_node("moveit_command_sender") robot = moveit_commander.RobotCommander() print "=" * 10, " Robot Groups:" print robot.get_group_names() print "=" * 10, " Printing robot state" print robot.get_current_state() print "=" * 10 rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_commander.MoveGroupCommander("left_arm")
  41. 41. ■セットアップ & 基本情報の取得 def main(): rospy.init_node("moveit_command_sender") robot = moveit_commander.RobotCommander() print "=" * 10, " Robot Groups:" print robot.get_group_names() ロボット全体に対するインタフェース print "=" * 10, " Printing robot state" print robot.get_current_state() print "=" * 10 rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_commander.MoveGroupCommander("left_arm")
  42. 42. ■セットアップ & 基本情報の取得 def main(): rospy.init_node("moveit_command_sender") robot = moveit_commander.RobotCommander() print "=" * 10, " Robot Groups:" print robot.get_group_names() print "=" * 10, " Printing robot state" print robot.get_ロボcurrent_ット内のstate() すべてのグループのリストを取得 print "=" * 10 rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_commander.MoveGroupCommander("left_arm")
  43. 43. ■セットアップ & 基本情報の取得 def main(): rospy.init_node("moveit_command_sender") robot = moveit_commander.RobotCommander() print "=" * 10, " Robot Groups:" print robot.get_group_names() print "=" * 10, " Printing robot state" print robot.get_current_state() print "=" * 10 rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_デバッグの際coにm有m効anなdロerボ.Mッoトve全G体roのup状C態omをm取a得nder("left_arm")
  44. 44. ■セットアップ & 基本情報の取得 def main(): rospy.init_node("moveit_command_sender") robot = moveit_commander.RobotCommander() print "=" * 10, " Robot Groups:" print robot.get_group_names() print "=" * 10, " Printing robot state" print robot.get_current_state() print "=" * 10 rarm = moveit_commander.MoveGroupCommander("right_arm") larm = moveit_commander.MoveGroupCommander("left_arm") 特定のグループのための単純なコマンドの実行を行うクラス
  45. 45. ■sleep中のCtrl - cで投げられる例外をキャッチする if __name__ == '__main__': try: main() except rospy.ROSInterruptException: pass テキストを保存する
  46. 46. ■実行権限を付加 $ chmod +x scripts/moveit_commander_sender.py
  47. 47. ■MoveIt!の設定ファイルを追加  #デフォルト値で使うなら不要 joint_limits: 0 ~ 5 RARM_JOINT0: has_velocity_limits: true max_velocity: 15.0 has_acceleration_limits: true max_acceleration: 17.0 0 ~ 5 LARM_JOINT0: has_velocity_limits: true max_velocity: 15.0 has_acceleration_limits: true max_acceleration: 17.0 config/joint_limit.yaml
  48. 48. ■設定ファイルを読み込む launch/planning_context.launch <?xml version="1.0"?> <launch> <arg name="load_robot_description" default="false"/> <param if="$(arg load_robot_description)" name="robot_description" textfile="$(find nextage_description)/urdf/NextageOpen.urdf"/> <param name="robot_description_semantic" textfile="$(find nextage_moveit_config)/config/NextageOpen.srdf"/> <group ns="robot_description_planning"> <rosparam command="load" file="$(find nextage_moveit_planning_execution)/config/joint_limit.yaml"/> </group> </launch>
  49. 49. ■設定ファイルを読み込む <?xml version="1.0"?> URDFファイルを読み込む <launch> <arg name="load_robot_description" default="false"/> <param if="$(arg load_robot_description)" name="robot_description" textfile="$(find nextage_description)/urdf/NextageOpen.urdf"/> <param name="robot_description_semantic" textfile="$(find nextage_moveit_config)/config/NextageOpen.srdf"/> <group ns="robot_description_planning"> <rosparam command="load" file="$(find nextage_moveit_planning_execution)/config/joint_limit.yaml"/> </group> </launch> joint_limit.yamlを読み込む
  50. 50. ■move_groupの起動とコンテキストの設定を行う <?xml version="1.0"?> <launch> <master auto="start"/> <include file="$(find nextage_moveit_config)/launch/move_group.launch"> launch/planning_execution.launch <arg name="publish_monitored_planning_scene" value="true" /> </include> <include file="$(find nextage_moveit_planning_execution)/launch/planning_co ntext.launch"> <arg name="load_robot_description" value="true"/> </include> </launch>
  51. 51. ■move_groupの起動とコンテキストの設定を行う <?xml version="1.0"?> move_groupを起動 <launch> <master auto="start"/> <include file="$(find nextage_moveit_config)/launch/move_group.launch"> <arg name="publish_monitored_planning_scene" value="true" /> </include> <include file="$(find nextage_moveit_planning_execution)/launch/planning_co ntext.launch"> <arg name="load_robot_description" value="true"/> </include> </launch> コンテキストの設定
  52. 52. ■動作確認 $ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch $ roslaunch nextage_moveit_planning_execution planning_execution.launch $ rosrun nextage_moveit_planning_execution moveit_command_sender.py
  53. 53. ■出力結果 [ INFO] [1409329353.399066034]: Loading robot model 'NextageOpen'... ========== Robot Groups: ['left_arm', 'right_arm'] ========== Printing robot state joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: /WAIST_Link name: ['CHEST_JOINT0', 'HEAD_JOINT0', 'HEAD_JOINT1', 'LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5'] position: [0.0, 0.0, 0.0, 0.010472, 0.0, -1.74533, -0.26529, 0.164061, -0.055851, -0.010472, 0.0, -1.74533, 0.26529, 0.164061, 0.055851] velocity: [] effort: [] multi_dof_joint_state: header: seq: 0 stamp: secs: 0 nsecs: 0 frame_id: /WAIST_Link joint_names: [] transforms: [] twist: [] wrench: [] attached_collision_objects: [] is_diff: False ========== [ INFO] [1409329354.980926234, 832.749999999]: Ready to take MoveGroup commands for group right_arm. [ INFO] [1409329355.324125746, 833.209999999]: Ready to take MoveGroup commands for group left_arm.
  54. 54. ■目標姿勢への移動  #main関数に追記 def main() ・・・ print "=" * 15, " Right arm ", "=" * 15 print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame() print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link() rarm_initial_pose = rarm.get_current_pose().pose print "=" * 10, " Printing initial pose: " print rarm_initial_pose
  55. 55. ■目標姿勢への移動  #main関数に追記 すべての計画が実行された フレームの名前を取得 def main() ・・・ print "=" * 15, " Right arm ", "=" * 15 print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame() print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link() rarm_initial_pose = rarm.get_current_pose().pose print "=" * 10, " Printing initial pose: " print rarm_initial_pose エンドエフェクタであるリンクの名前を取得 存在しない場合空の文字列を返す エンドエフェクタの現在の姿勢を取得 エンドエフェクタが存在しない場合は例外を投げる
  56. 56. ■目標姿勢への移動  #main関数に追記 target_pose_r = geometry_msgs.msg.Pose() target_pose_r.position.x = 0.2035 target_pose_r.position.y = -0.5399 target_pose_r.position.z = 0.0709 target_pose_r.orientation.x = 0.000427 target_pose_r.orientation.y = 0.000317 target_pose_r.orientation.z = -0.000384 target_pose_r.orientation.w = 0.999999 rarm.set_pose_target(target_pose_r) print "=" * 10, " plan1..." rarm.go() rospy.sleep(1)
  57. 57. ■目標姿勢への移動  #main関数に追記 エンドエフェクタの姿勢を設定 target_pose_r = geometry_msgs.msg.Pose() target_pose_r.position.x = 0.2035 target_pose_r.position.y = -0.5399 target_pose_r.position.z = 0.0709 target_pose_r.orientation.x = 0.000427 target_pose_r.orientation.y = 0.000317 target_pose_r.orientation.z = -0.000384 target_pose_r.orientation.w = 0.999999 rarm.set_pose_target(target_pose_r) print "=" * 10, " plan1..." rarm.go() rospy.sleep(1) 指定された目標にグループを移動
  58. 58. ■目標姿勢への移動  #main関数に追記 target_pose_r = geometry_msgs.msg.Pose() target_pose_r.position.x = 0.2035 target_pose_r.position.y = -0.5399 target_pose_r.position.z = 0.0709 target_pose_r.orientation.x = 0.000427 target_pose_r.orientation.y = 0.000317 target_pose_r.orientation.z = -0.000384 target_pose_r.orientation.w = 0.999999 rarm.set_pose_target(target_pose_r) print "=" * 10, " plan1..." rarm.go() rospy.sleep(1) set_pose_target(pose, end_effector_link = “”) poseは4種類の入力に対応 * Poseメッセージ * PoseStampedメッセージ * [x, y, z, rot_x, rot_y, rot_z] * [x, y, z, qx, qy, qz, qw]
  59. 59. ■動作確認  $ rosrun nextage_moveit_planning_execution moveit_command_sender.py
  60. 60. ■左アームも動かす  #main関数に追記 print "=" * 15, " Left arm ", "=" * 15 print "=" * 10, " Reference frame: %s" % larm.get_planning_frame() print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link() larm_initial_pose = larm.get_current_pose().pose print "=" * 10, " Printing initial pose: " print larm_initial_pose target_pose_l = [ target_pose_r.position.x, -target_pose_r.position.y, target_pose_r.position.z, target_pose_r.orientation.x, target_pose_r.orientation.y, target_pose_r.orientation.z, target_pose_r.orientation.w ]l arm.set_pose_target(target_pose_l) print "=" * 10, " plan2..." larm.go() rospy.sleep(1)
  61. 61. ■左アームも動かす  #main関数に追記 print "=" * 15, " Left arm ", "=" * 15 print "=" * 10, " Reference frame: %s" % larm.get_planning_frame() print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link() larm_initial_pose = larm.get_current_pose().pose print "=" * 10, " Printing initial pose: " print larm_initial_pose target_pose_l = [ target_pose_r.position.x, -target_pose_r.position.y, target_pose_r.position.z, target_pose_r.orientation.x, target_pose_r.orientation.y, target_pose_r.orientation.z, target_pose_r.orientation.w ]l arm.set_pose_target(target_pose_l) print "=" * 10, " plan2..." larm.go() rospy.sleep(1) Poseメッセージではなくリストを使用
  62. 62. ■動作確認  $ rosrun nextage_moveit_planning_execution moveit_command_sender.py
  63. 63. ■関節空間で目標を設定  #main関数に追記 print "=" * 10, " Planning to a joint-space goal" rarm.clear_pose_targets() print "=" * 10, " Joint values: ", rarm.get_current_joint_values() rarm_variable_values = [ 1.4377544509919726, -1.3161643133168621, -2.126307271452489, 1.4335761224859305, 0.02359653211486051, 0.55989121526186 ] rarm.set_joint_value_target(rarm_variable_values) print "=" * 10, " plan3..." rarm.go() rospy.sleep(1)
  64. 64. ■関節空間で目標を設定  #main関数に追記 print "=" * 10, " Planning to a joint-space goal" rarm.clear_pose_targets() print "=" * 10, " Joint values: ", rarm.get_current_joint_values() rarm_variable_values = [ 1.4377544509919726, -1.3161643133168621, -2.126307271452489, 1.4335761224859305, 0.02359653211486051, 0.55989121526186 すべての目標をクリア ] rarm.set_joint_value_target(rarm_variable_values) print "=" * 10, " plan3..." rarm.go() rospy.sleep(1) すべての変数の値をリストで渡して 目標を設定
  65. 65. ■動作確認  $ rosrun nextage_moveit_planning_execution moveit_command_sender.py
  66. 66. ■中間地点を結ぶ経路を計画  #main関数に追記 print "=" * 10, " Cartesian Paths" waypoints = [] waypoints.append(larm.get_current_pose().pose) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x wpose.position.y = waypoints[0].position.y - 0.15 wpose.position.z = waypoints[0].position.z waypoints.append(copy.deepcopy(wpose)) wpose.position.z -= 0.1 waypoints.append(copy.deepcopy(wpose)) wpose.position.y -= 0.05 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = larm.compute_cartesian_path(waypoints, 0.01, 0.0) print "=" * 10, " plan4..." larm.execute(plan) rospy.sleep(5)
  67. 67. ■中間地点を結ぶ経路を計画  #main関数に追記 print "=" * 10, " Cartesian Paths" waypoints = [] waypoints.append(larm.get_current_pose().pose) wpose = geometry_msgs.msg.Pose() compute_cartesian_path(waypoints, eef_step, wpose.orientation.w = 1.0 jump_threshold, avoid_collisions = True) wpose.position.x = waypoints[* waypoints 0].position.x wpose.position.y = waypoints[中間地点 0].position.y - 0.15 wpose.position.z = waypoints[* eef_step 0].position.z waypoints.append(copy.deepcopy(補完する分解wpose)) 能[m] wpose.position.z -= 0.1 * jump_threshold waypoints.append(copy.deepcopy(経路内の点wpose)) 間の最大距離 wpose.position.y -= 0.05 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = larm.compute_cartesian_path(waypoints, 0.01, 0.0) print "=" * 10, " plan4..." larm.execute(plan) rospy.sleep(5)
  68. 68. ■中間地点を結ぶ経路を計画  #main関数に追記 print "=" * 10, " Cartesian Paths" waypoints = [] waypoints.append(larm.get_current_pose().pose) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x wpose.position.y = waypoints[0].position.y - 0.15 wpose.position.z = waypoints[0].position.z waypoints.append(copy.deepcopy(wpose)) wpose.position.z -= 0.1 waypoints.append(copy.deepcopy(wpose)) wpose.position.y -= 0.05 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = larm.compute_cartesian_path(waypoints, 0.01, 0.0) print "=" * 10, " plan4..." larm.execute(plan) 動作計画の結果(RobotTrajectoryクラス) rospy.sleep(5) に従って経路を追従
  69. 69. ■動作確認  $ rosrun nextage_moveit_planning_execution moveit_command_sender.py
  70. 70. ■初期姿勢に戻す  #main関数に追記 print "=" * 10, " Moving to an initial pose" rarm.set_pose_target(rarm_initial_pose) larm.set_pose_target(larm_initial_pose) rarm.go() larm.go() rospy.sleep(2) プログラム開始時の姿勢に戻して終了
  71. 71. ■アームの移動速度を変更してみよう config/joint_limit.txtを書き換える  → max_velocity  → max_acceleration ■planning_execution.launchを再度立ち上げ直す $ roslaunch nextage_moveit_planning_execution planning_execution.launch
  72. 72. 目標値をトピックでやりとりする Poseメッセージの送信・受信を二つのノードに分ける キーボード操作でアームの動きを制御するプログラムを作成
  73. 73. ■必要なモジュールをインポート scripts/nextage_command_server.py import moveit_commander import rospy import geometry_msgs.msg
  74. 74. ■Poseメッセージを購読するクラスを作成 scripts/nextage_command_server.py class NextageCommandServer: def __init__(self): self._robot = moveit_commander.RobotCommander() rospy.Subscriber("right_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_r_cb) rospy.Subscriber("left_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_l_cb) self._rarm = moveit_commander.MoveGroupCommander("right_arm") self._larm = moveit_commander.MoveGroupCommander("left_arm") ・・・
  75. 75. ■Poseメッセージを購読するクラスを作成 scripts/nextage_command_server.py class NextageCommandServer: def __init__(self): self._robot = moveit_commander.RobotCommander() rospy.Subscriber("right_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_r_cb) rospy.Subscriber("left_arm/delta_pose", geometry_msgs.msg.Pose, self.delta_pose_l_cb) self._rarm = moveit_commander.MoveGroupCommander("right_arm") self._larm = moveit_commander.MoveGroupCommander("left_arm") ・・・ rospy.Subscriber(トピック名, メッセージ, コールバック関数)
  76. 76. ■right_arm/delta_poseのコールバックメソッド scripts/nextage_command_server.py class NextageCommandServer: ・・・ def delta_pose_r_cb(self, pose): rospy.loginfo(rospy.get_caller_id()) target_pose_r = self._rarm.get_current_pose().pose target_pose_r.position.x += pose.position.x target_pose_r.position.y += pose.position.y target_pose_r.position.z += pose.position.z target_pose_r.orientation.w += pose.orientation.w target_pose_r.orientation.x += pose.orientation.x target_pose_r.orientation.y += pose.orientation.y target_pose_r.orientation.z += pose.orientation.z self._rarm.set_pose_target(target_pose_r) self._rarm.go()  ・・・
  77. 77. ■right_arm/delta_poseのコールバックメソッド scripts/nextage_command_server.py class NextageCommandServer:  ・・・ def delta_pose_r_cb(self, pose): rospy.loginfo(rospy.get_caller_id()) target_pose_r = self._rarm.get_current_pose().pose target_pose_r.position.x += pose.position.x target_pose_r.position.y += pose.position.y target_pose_r.position.z += pose.position.z target_pose_r.orientation.w += pose.orientation.w target_pose_r.orientation.x += pose.orientation.x target_pose_r.orientation.y += pose.orientation.y target_pose_r.orientation.z += pose.orientation.z self._rarm.set_pose_target(target_pose_r) self._rarm.go()  ・・・ right_arm/delta_poseトピックを 受け取った際に呼び出される
  78. 78. ■left_arm/delta_poseのコールバックメソッド scripts/nextage_command_server.py class NextageCommandServer:  ・・・ def delta_pose_l_cb(self, pose): rospy.loginfo(rospy.get_caller_id()) target_pose_l = self._larm.get_current_pose().pose target_pose_l.position.x += pose.position.x target_pose_l.position.y += pose.position.y target_pose_l.position.z += pose.position.z target_pose_l.orientation.w += pose.orientation.w target_pose_l.orientation.x += pose.orientation.x target_pose_l.orientation.y += pose.orientation.y target_pose_l.orientation.z += pose.orientation.z self._larm.set_pose_target(target_pose_l) self._larm.go()  ・・・
  79. 79. ■rospy.spin() scripts/nextage_command_server.py class NextageCommandServer:  ・・・ def run(self): rospy.spin()  ・・・
  80. 80. ■rospy.spin() scripts/nextage_command_server.py class NextageCommandServer:  ・・・ def run(self): rospy.spin()  ・・・ ノードが終了するまでキープ
  81. 81. ■ノードの初期化 scripts/nextage_command_server.py ・・・ def main(): rospy.init_node("nextage_command_server") nextage_command_server = NextageCommandServer() nextage_command_server.run() if __name__ == '__main__': main() nextage_command_serverの実装は完了
  82. 82. ■スクリプトをPythonとして実行させる(ROSノードに必須) #!/usr/bin/env python ■必要なモジュールをインポート scripts/nextage_teleop_key.py import rospy, geometry_msgs.msg import sys, select, termios, tty
  83. 83. ■Poseメッセージを配信するクラスを作成 scripts/nextage_teleop_key.py class TeleopNextage: MOVE_BINDINGS = { 'h' : (1, 0.1), 'j' : (0, -0.1), 'k' : (0, 0.1), 'l' : (1, -0.1), } MSG = """ ********** h : y += 0.1 j : x += -0.1 k : x += 0.1 l : y += -0.1 ********** """ ・・・
  84. 84. ■Poseメッセージを配信するクラスを作成 scripts/nextage_teleop_key.py class TeleopNextage: ・・・ def __init__(self): self._settings = termios.tcgetattr(sys.stdin) self._delta_pose_pub = rospy.Publisher('right_arm/delta_pose', geometry_msgs.msg.Pose) ・・・
  85. 85. ■ユーザーに押されたキーを取得するメソッド scripts/nextage_teleop_key.py class TeleopNextage: ・・・ def get_key(self): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self._settings) return key ・・・
  86. 86. ■押されたキーによって異なるPoseメッセージを発行 scripts/nextage_teleop_key.py class TeleopNextage: ・・・ def run(self): r = rospy.Rate(10) while not rospy.is_shutdown(): key = self.get_key() print key if key in TeleopNextage.MOVE_BINDINGS.keys(): move = TeleopNextage.MOVE_BINDINGS[key] diff_list = [move[1] if index is move[0] else 0 for index in range(7)] delta_pose = geometry_msgs.msg.Pose() delta_pose.position.x = diff_list[0] delta_pose.position.y = diff_list[1] delta_pose.position.z = diff_list[2] delta_pose.orientation.x = diff_list[3] delta_pose.orientation.y = diff_list[4] delta_pose.orientation.z = diff_list[5] delta_pose.orientation.w = diff_list[6] self._delta_pose_pub.publish(delta_pose) elif key == 'x03': print "Quit" break else: print "Unknown type" r.sleep() ・・・
  87. 87. ■終了処理 scripts/nextage_teleop_key.py class TeleopNextage: ・・・ def __del__(self): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self._settings) ・・・
  88. 88. ■ノードの初期化など scripts/nextage_teleop_key.py def main(): rospy.init_node('teleop_pose_key') teleop_nextage = TeleopNextage() print TeleopNextage.MSG teleop_nextage.run() if __name__ == "__main__": try: main() except rospy.ROSInterruptException: pass
  89. 89. ■動作確認 $ chmod +x scripts/nextage_command_server.py scripts/nextage_teleop_key.py $ rosrun nextage_moveit_planning_execution nextage_command_server.py $ rosrun nextage_moveit_planning_execution nextage_teleop_key.py
  90. 90. アームを自由に動かせるようになった 足りないのは環境認識 次は「NEXTAGEのカメラとtf」
  91. 91. ご清聴ありがとうございました!

×