(10-1) 2019.6.19 電気通信大学大学院情報理工学研究科末廣尚士
16. ロボットアーム RTC - 6 自由度ロボットアーム 6 回転関節シリアルリンク 2
- ロボットアーム RTC 入力 目標関節角度 :target_joints TimedDoubleSeq 型 ( 長さ 6), 単位はラジアン ハンドの開き幅 :h_width TimedDouble 型単位はメートル 出力 実際の関節角度 :current_joints TimedDoubleSeq 型 ( 長さ 6), 単位はラジアン ハンドの開き幅 :current_width TimedDouble 型単位はメートル 動作周期 100 ms 10 Hz 4
- RTC を使う手順 1. RTC を起動する 2. RTC の Object Reference をネームサーバから取得する 3. RTC を使うために必要な情報 ( 入出力ポート, サービスポートなど ) を取得する 4. RTC のポートにアクセスするためのポートを生成する 5. RTC 間を接続する 6. RTC をアクティベートする 5
- Naming Service の確認 robot:~/openrtm$ ps ax grep omni 1195? Sl 0:00 /usr/bin/omninames -errlog /var/log/omniorbnameserver.log 2975 pts/11 S+ 0:00 grep --color=auto omni 上記の形で走っていれば OK 走っていない場合は以下のようにして起動する. robot:~/openrtm$ cd omninames robot:~/opnertm/omninames$ rtm-naming 6
- ロボットアーム RTC の起動 robot:~/openrtm/robots/arm6dof$ python Arm6dof.py 7
- 起動できない場合 robot:~/openrtm/robots/arm6dof$ python Arm6dof.py Traceback (most recent call last): File "/usr/lib/python2.7/dist-ackages/openrtm_aist/factory.py", line 274, in create rtobj = self._new(mgr) File "Arm6dofComp.py", line 114, in init self.arm=create_arm() NameError: global name 'create_arm' is not defined この原因は,Arm6dof.py と arm6dof.py の区別ができていないため. そこで Arm6dof.py を Arm6dofComp.py に変更して, robot:~/openrtm/robots/arm6dof$ python Arm6dofComp.py とするれば OK. 8
- python から RTC を使う 直接 pythonインタプリターから使う scriptファイルを作成する jupyter notebookで利用する 後で script ファイルに落とす 9
- scripts ディレクトリの作成 Robots の下に作る robot:~/openrtm/robots$ mkdir scripts robot:~/openrtm/robots$ cd scripts robot:~/openrtm/robots/scripts$ 10
- rtc.conf の作成 python 自身を RTC 化する際には, その (scripts) ディレクトリに rtc.conf が必要 corba.nameservers: localhost:2809 naming.formats: %n_py.rtc logger.log_level: TRACE 11
- python を起動 python インタプリタ robot:~/openrtm/robots/scripts$ python ipython インタプリタ robot:~/openrtm/robots/scripts$ ipython jupyter notebook robot:~/openrtm/robots/scripts$ jupyter notebook emacs でスクリプトをいきなり書くのもあり 12
- 設定 基本モジュールの読み込み import sys import time import subprocess from math import * 環境設定 この辺は別ファイル (set_env.py) にして使いまわせるようにすると良い. その時は,from set_env import * RtmToolsDir="../../rtmtools" MyRtcDir=".." NS0="localhost:2809" 14
- ツールの読み込み パスを設定してツールを読み込む save_path = sys.path[:] sys.path.append(rtmtoolsdir+'/rtc_handle') from rtc_handle import * from rtc_handle_util import * sys.path.append(rtmtoolsdir+'/embryonic_rtc') from EmbryonicRtc import * sys.path = save_path 15
- rtc_handle とは python 環境からの RTC の簡単操作 RTC およびロボットシステムのデバッグツール RTC のプロトタイピング ロボット作業アプリケーションの開発 実行 ロボットシステム構築の支援 16
- rtc_handle rtc_handle.py corba orb 環境の保持 (RtmEnv) ネームサーバの情報の取得, 保持 (NameSpace) 各 RTC の情報の保持 (RtcHandle) サービスポート, 入出力ポート, コンフィギュレーションサービスの情報の保持, アクセス rtc_handle_util.py rtc_handle.py のお助け便利ツール EmbryonicRtc.py python 実行環境自身を RTC 化する 入出力ポートとのデータ授受をサポート 17
-corba 環境の立ち上げ orb の立ち上げとネームサーバの URL の設定 ネームサーバはリストで複数持てる orb のパラメタを sys.argv に持たせることができる env = RtmEnv(sys.argv,[NS0]) 18
- rtc のリスト作成 必要な RTC が立ち上がっていることを確認してリストアップする raw_input("are you sure that each rtc needed is running?") for ns in env.name_space : env.name_space[ns].list_obj() 19
- ロボット RTC のプロキシ取得 リストアップの結果はネームサーバごとの辞書に登録される env.name_space[ns0].rtc_handles {'lecture2018.host_cxt/arm6dof0.rtc': <rtc_handle.rtchandle instance at 0x7ff43b769170>} 使いやすいように変数を割り当てておく arm = env.name_space[ns0].rtc_handles['lecture2018.host_cxt/arm6 dof0.rtc'] 20
- 本体の RTC 化 ロボット RTC と入出力ポートでデータをやり取りできるように,python インタプリタ自身を RTC 化する mgr=main() comp=mgr.getcomponents()[0] ロボット RTC に対応したポートを生成する make_pipe(comp,arm) 21
- ロボットとの入出力ポートの確認 OutPort との接続は OutPipe. OutPipe はデータを受け取るので実は InPort arm.out_pipe {'current_joints': <rtc_handle.outpipe instance at 0x7ff438468bd8>, 'current_width': <rtc_handle.outpipe instance at 0x7ff4384641b8>} InPort との接続は InPipe arm.in_pipe {'h_width': <rtc_handle.inpipe instance at 0x7ff43845e6c8>, 'target_joints': <rtc_handle.inpipe instance at 0x7ff43b85e098>} 22
- 入出力ポートの接続 Pipe の connect() メソッドで対応するポートに接続できる for pp in arm.out_pipe : arm.out_pipe[pp].connect() for pp in arm.in_pipe : arm.in_pipe[pp].connect() ここまでの処理をまとめて script ファイルにしておくと良い.(arm_control.py) 23
- ロボット RTC の状態 状態を見る arm.get_state() INACTIVE 状態を変更する arm.activate() RTC_OK arm.get_state() ACTIVE 24
- SystemEditor で確認 接続の様子を SystemEditor で確認 25
- ロボットの現状を取得 現在の関節角度が送られてきたかの確認 arm.out_pipe['current_joints'].pipe.isnew() True 関節角度を取得する arm.out_pipe['current_joints'].read() [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 26
- ロボットの現状を見る 目標関節角度を送る arm.in_pipe['target_joints'].write([pi/3,pi/4,pi/2,0.0,pi/ 4,pi/3]) 指も動かす arm.in_pipe['h_width'].write(0.05) 27
- 動かした結果 28
- ロボットの現状を再取得 関節角度を取得する jj=arm.out_pipe['current_joints'].read() jj [1.0471975511965976, 0.7853981633974483, 1.5707963267948966, 0.0, 0.7853981633974483, 1.0471975511965976] degree で表示 59.99999999999999, 45.0, 90.0, 0.0, 45.0, 59.99999999999999] map(degrees, jj) [59.99999999999999, 45.0, 90.0, 0.0, 45.0, 59.99999999999999] 29
- script にまとめてやってみる 別窓でロボットを起動 robot:~/openrtm/arm6dof$ python Arm6dof.py scripts ディレクトリにスクリプトを置く robot:~/openrtm/robots/scripts$ ls arm_control.ipynb arm_control.pyc rtc.conf.template set_env.py.template arm_control.py rtc.conf set_env.py set_env.pyc 30
- script にまとめてやってみる python を起動して読み込む robot:~/openrtm/robots/scripts$ python Python 2.7.12 (default, Dec 4 2017, 14:50:18) >>> from arm_control import * are you sure that each rtc needed is running? objcet lecture2018.host_cxt/embryonicrtc0.rtc was listed. port_name: lecture2018_host_cxt/arm6dof0_rtcarm6dof0_target_joints handle for lecture2018.host_cxt/embryonicrtc0.rtc was created. objcet lecture2018.host_cxt/arm6dof0.rtc was listed. port_name: target_joints handle for lecture2018.host_cxt/arm6dof0.rtc was created. >>> 31
- script にまとめてやってみる 動かしてみる >>> arm.activate() RTC_OK >>> arm.in_pipe['target_joints'].write(map(radians,[60,45,90,0,45,60])) >>> 32
- 例題 16-1 1. 自身の環境で python からロボットアームを動かせるようにする. 2. いろいろな姿勢にしてみる 3. 一気に動くのではなく目標姿勢に向かって 動いていく ようにする 4. 物理的に滑らかに動くようにする. 少し大事なヒント : ロボットアームの制御周期に合わせてデータを送らないと,,,,.time.sleep() などを使って周期 100ms( サンプルレート 10Hz) になるようにすると良い. 33
- 下準備 (script ディレクトリにて ) rtc.conf corba.nameservers: localhost:2809 naming.formats: %n_py.rtc logger.log_level: TRACE set_env.py RtmToolsDir="../../rtmtools" MyRtcDir=".." NS0="localhost:2809" 34
- arm_control.py #!/usr/bin/env python # -*- coding: utf-8 -*- import sys import time import subprocess from math import * from set_env import * save_path = sys.path[:] sys.path.append(rtmtoolsdir+'/rtc_handle') from rtc_handle import * from rtc_handle_util import * sys.path.append(rtmtoolsdir+'/embryonic_rtc') from EmbryonicRtc import * sys.path = save_path 35
- arm_control.py # corba 環境の立ち上げ env = RtmEnv(sys.argv,[NS0]) # rtc のリスト作成 raw_input("are you sure that each rtc needed is running?") for ns in env.name_space : env.name_space[ns].list_obj() # ロボット RTC のプロキシ取得 env.name_space[ns0].rtc_handles arm = env.name_space[ns0].rtc_handles['lecture2018.host_cxt/arm6dof0.rtc'] 36
- arm_control.py # 本体の RTC 化とロボット RTC との対応ポートの生成 mgr=main() comp=mgr.getcomponents()[0] make_pipe(comp,arm) for pp in arm.out_pipe : arm.out_pipe[pp].connect() for pp in arm.in_pipe : arm.in_pipe[pp].connect() 37
- ロボットアーム RTC の起動 arm6dof ディレクトリにて robot:~/openrtm/robots/arm6dof$ python Arm6dof.py 38
- arm_control.py を import して使う robot:~/openrtm/robots/scripts$ python Python 2.7.12 (default, Dec 4 2017, 14:50:18) ここで, 改 >>> from arm_control import * 行を入力 are you sure that each rtc needed is running? objcet lecture2018.host_cxt/embryonicrtc0.rtc was listed. port_name: lecture2018_host_cxt/arm6dof0_rtcarm6dof0_target_joints handle for lecture2018.host_cxt/embryonicrtc0.rtc was created. objcet lecture2018.host_cxt/arm6dof0.rtc was listed. port_name: target_joints handle for lecture2018.host_cxt/arm6dof0.rtc was created. >>> 39
- 動かしてみる アーム RTC をアクティベートして アーム RTC の目標関節角度入力ポートに python 側の in_pipe を使って 関節角度のリストを送る >>> arm.activate() RTC_OK >>> arm.in_pipe['target_joints'].write(map(radians,[60,45,90,0,45,60])) >>> 40
- 次回予告 アームの軌道生成 41