Nextage Open チュートリアル¶
The code is open source, and available on github.
はじめに¶
安全について¶
NEXTAGE OPEN ロボットを動作させる前に必ず次の事項を確認してください．
- ロボットが適切に設置されている
- 周辺に動作に必要な空間が確保されている
- 緊急停止スイッチがいつでも押せる状態になっている
クイックスタート¶
コンピュータの起動¶
- 台座の下部にキャスターを上下に動かすノブがあります． このノブを反時計回りに回してロボットを床に固定します.
- AC電源プラグは台座から出ています．電源につないでください．
- キーボードとマウス，ディスプレイモニタをUIコントロールボックスに接続してください．
UIコントロールボックスは2つあるコンピュータのうち上側のコンピュータです．
- AC電源を入れてください．コントローラボックスのメインスイッチが赤く点灯します．
- スイッチボックスをロボットの後ろ側にあるコネクタに接続してください．
- ロボットの後ろ側にある緑色のボタンを押してください．ビープ音が1分ほど鳴ります．
ロボット胸部にある4つのLEDライトが全て点滅します．
- ロボットの後ろ側にある青色のボタンを押してください．
ロボットの胸部にある緑色と白色のLEDライトのみが点滅していたら，
ロボットの動作準備が整った状態となっています．
ソフトウェアのアップデート¶
出荷状態のロボットにおいては Ubuntu 側のソフトウェアに最新のバージョンがインストールされていない場合があります．
UIコントロールボックスのターミナルを開いて次の1行のコマンドを実行するとアップデートされます． 本コマンド実行の際には別途通知されているルートパスワードが必要です．
$ sudo apt-get update && sudo apt-get dist-upgrade
ROS サーバの起動¶
rtm_ros_bridge の実行¶
UIコントロールボックスのターミナルで下記のコマンドを実行して ROS ソフトウェアを実行します．
$ roslaunch hironx_ros_bridge hironx_ros_bridge_real.launch (HIRO)
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch (NEXTAGE OPEN)
デフォルト状態の NEXTAGE OPEN ではホスト名 nextage でアクセスすることができ，
それは nextage_ros_bridge_real.launch 内に既に記述されています．
ホスト名を変更した場合には下記コマンドの%HOSTNAME% の部分を変更したホスト名に置き換え，明示的にコマンドを実行してください．
$ roslaunch hironx_ros_bridge hironx_ros_bridge_real.launch nameserver:=%HOSTNAME% (HIRO)
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME% (NEXTAGE OPEN)
このプログラムはプログラムからロボットを操作するときには実行された状態である必要があります．
ROS プロセスの確認¶
rtm_ros_bridge を実行しているターミナルとは別のターミナルを開いてください． 新しいターミナルで次のコマンドを実行することで動作しているROSノードを確認することができます．
$ rosnode list
/diagnostic_aggregator
/hrpsys_profile
/hrpsys_ros_diagnostics
/hrpsys_state_publisher
/rosout
Hironx Dashboard によるロボットの初期化¶
rqt での Hironx Dashboard の起動¶
Hironx Dashboard は rqt 上で動きますのでターミナルから rqt を起動します．
$ rqt
rqt の Plugins で Hironx Dashboard を選択します．
ロボットのキャリブレーション¶
Hironx Dashboard からロボットの関節角度のエンコーダのキャリブレーションを行います．
- 注意-1: スイッチボックスの緊急停止スイッチをいつでも押せる状態にしてください．
- 注意-2: ロボットが動きます．
Hironx Dashboard の [ Joint Calibration ] ボタンを押してください．
次のようにロボットが動きます．
FROM:
TO:
ロボットを初期姿勢にする¶
ロボットの各関節を初期姿勢にします．
- 注意: ロボットの全関節が動きます．（初期姿勢から外れいてる場合）
Hironx Dahsboard の [ Goto init pose ] ボタンを押してください．
MoveIt! での動作の実行¶
ROS の GUI も備えたモーションプランニングフレームワークの MoveIt! を用いてロボットを動かしてみます．
MoveIt! の起動¶
MoveIt! を起動します．新しくターミナルを開いて次のコマンドを実行してください．
$ roslaunch hironx_moveit_config moveit_planning_execution.launch (HIRO)
$ roslaunch nextage_moveit_config moveit_planning_execution.launch (NEXTAGE OPEN)
MoveIt! での動作計画と実行¶
MoveIt! 内に表示されているロボットモデルの手先に矢印や球で表現されているマーカがあります． これは InteractiveMarker と呼ばれるもので，これで手先の位置と姿勢を指定します．
手先姿勢を変更する前に次の準備を実行してください．
- Planning タブ Querry の Select Start State: < current > で [ Update ] ボタンをクリック
- Planning タブ Querry の Select Goal State: < current > で [ Update ] ボタンをクリック
InteractiveMarker をマウスでドラッグして少し移動させてみます． 色違いの腕が表示されていることと思います． 現在のロボットの姿勢と InteractiveMarker で指定した目標姿勢が表示されています．
Planning タブ内の [ Plan ] ボタンをクリックしてください． MoveIt! 内においてアニメーションで腕が目標姿勢になるように動作計画が表示されていることと思います．
アニメーション表示された動作計画で実機ロボット周囲の状況も含めて干渉など問題がないようでしたら， 実際にロボットを目標姿勢になるように動かしてみます．
- 注意: ロボットが動きます．
Planning タブ内の [ Plan and Execute ] ボタンをクリックしてください．
MoveIt! で動作計画したように実際にロボットが動いたことと思います．
他の目標姿勢にも動かしてみます． ここでは MoveIt! にランダムな姿勢を生成させます．
- 注意: 生成された目標姿勢に対するロボット周辺の状況を確認しながら下記の手順を進めてください．
- Planning タブ Querry の Select Start State: で < random valid > を選択
- Select Start State: [ Update ] ボタンをクリック（再クリックで再生成）
- Commands の [ Plan ] ボタンをクリック → 動作計画の確認
- ロボット動作環境などの安全確認
- Commands の [ Plan and Execute ] ボタンをクリック → 動作の実行
MoveIt! での動作計画と実行の手順まとめ
- Planning タブ Querry の Select Start State: < current > で [ Update ] ボタンをクリック
- Planning タブ Querry の Select Goal State: < current > で [ Update ] ボタンをクリック
- MoveIt! 上の InteractiveMarker でロボットの手をドラックして目標姿勢に移動
- Planning タブ Commands の [ Plan ] ボタンをクリックして動作計画を確認
- ロボットの動作による周辺環境との干渉などの問題がないことを確認
- Planning タブ Commands の [ Plan and Execute ] ボタンをクリック
ロボットのシャットダウン処理¶
ロボットをサーボオフ姿勢にする¶
ロボットの各関節をサーボオフ姿勢にしてサーボ切ります．
- 注意: ロボットが動きます．
Hironx Dahsboard の [ Goto init pose ] ボタンを押してください．
これによりロボットの各関節がサーボオフ姿勢になり，サーボが切れます．
全プログラムの終了¶
実行しているプログラムを全て終了します．
全てのターミナルで Ctrl-c により終了してください．
QNX の停止¶
Ubuntu デスクトップ上にある NextageOpenSupervisor ツールを用いて QNX コンピュータのシャットダウンを行ってください．
動力学シミュレーション¶
RTM の動力学シミュレーション - hrpsys-simulator¶
ロボットの核となる機能は OpenRTM というフレームワーク上で動いています． この OpenRTM をベースとした hrpsys-simulator と呼ばれるシミュレータでロボットの機能を仮想的に実現することができます． このシミュレータは ROS が提供する高層レイヤを使用しなくても利用できます．
特別な目的ではないプログラムを実行する多くの場合においてこのシミュレータで十分です．
シミュレータの実行¶
実際のロボットを「模倣する」シミュレータを起動します．
roslaunch ではなく rtmlaunch であることに注意して次のコマンドを実行してください．
$ rtmlaunch hironx_ros_bridge hironx_ros_bridge_simulation.launch (HIRO)
$ rtmlaunch nextage_ros_bridge nextage_ros_bridge_simulation.launch (NEXTAGE OPEN)
この launch ファイルは主に次の2つを実行します．
まずはシミュレータに仮想ロボットを読み込みます．
$ rtmlaunch hironx_ros_bridge hironx_startup.launchこのシミュレーションのロボットは OpenRTM ベースのソフトウェア上のみで実行されています．
次に ROS 経由でこのロボットを動作させるために ROS と OpenRTM の2つの空間を結ぶ「橋」をつくります．
$ roslaunch hironx_ros_bridge hironx_ros_bridge.launch (HIRO) $ roslaunch nextage_ros_bridge nextage_ros_bridge.launch (NEXTAGE OPEN)
次のようなコマンドプロンプトが表示されていたら，シミュレーションが正常に動作しています．
[ INFO] [1375160303.399785831, 483.554999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 201[Hz]
[ INFO] [1375160304.408500642, 484.544999999]: [HrpsysSeqStateROSBridge0] @onExecutece 0 is working at 198[Hz]
そして hrpsys simulator viewer が次のように表示されます．
MoveIt! の起動とシミュレーションロボットの操作¶
ロボット実機と同じように MoveIt! を起動してシミュレーション上のロボットを操作することができます．
$ roslaunch hironx_moveit_config moveit_planning_execution.launch (HIRO)
$ roslaunch nextage_moveit_config moveit_planning_execution.launch (NEXTAGE OPEN)
MoveIt! 内での操作も実機のロボットとシミュレーション上のロボットでは同じです． InteractiveMarker を動かし，動作計画をし，実行します．
ROS の動力学シミュレーション - Gazebo¶
NEXTAGE OPEN の Gazebo シミュレーションを起動します．
- メモ: ROS / Gazebo でのシミュレーションのため rtm_ros_bridge は不要
$ roslaunch nextage_gazebo nextage_world.launch
Gazebo nextage_world に セスナ機，カフェのテーブル，建設用のバレル，HUSKYロボットオブジェクトをインサートした図
ロボットハードウェアの概要¶
ロボット諸元¶
詳しくはカワダロボティクスのサイト内にある NEXTAGE 製品仕様を参照してください．
- Kawada Robotics NEXTAGE - 製品仕様
- メーカサイト
- 自由度: 15軸
- 腕6軸 x 2 : 肩 Yaw-Pitch，肘 Pitch，手首 Pitch-Roll
- 首2軸 : 首 Yaw-Pitch
- 腰1軸 : 腰 Yaw
- 各関節動作仕様
- 最大可搬質量
- 片腕 : 1.5 kg
- 両腕 : 3.0 kg
- カメラ
- 頭部 x 2
- ハンドカメラ（オプション）
座標系¶
HIRO / NEXTAGE OPEN の座標系は下の図のようになっています．
ベース座標は腰基部中心に設定されています．
- ベース座標
- X軸: 正 - 前方 / 負 - 後方
- Y軸: 正 - 左側 / 負 - 右側
- Z軸: 正 - 上方 / 負 - 下方
各フレームの座標系は次のようになっています．
- 各座標軸と色表記
- X軸: Red
- Y軸: Green
- Z軸: Blue
コンピュータとソフトウェアの概要¶
制御用コンピュータ¶
カワダロボティクスの HIRO および NEXTAGE OPEN ロボットシステムには次の2つのコンピュータがあります．
- コントロールボックス（QNXコンピュータ）
- ロボットを制御するコンピュータ
- QNX: 実時間オペレーティングシステム
- デフォルトではソフトウェアのインストール・改変は不可
- 保守・アップデートは可能
- UIコントロールボックス（Ubuntuコンピュータ/ビジョンPC）
- 作業を行うコンピュータ
- Ubuntu
- 高次レイヤソフトウェアの実行
開発用コンピュータ¶
HIRO および NEXTAGE OPEN のシステムでは2台の制御用コンピュータの他に 開発用コンピュータも使用することができます．
- 開発用コンピュータ要求仕様
- Intel i5 以上のプロセッサ
- 4GB 以上のメモリ
- 15GB 以上のディスク空き容量
- Ubuntu 14.04
- ROS Indigo
- QNXコンピュータにイーサネット経由で接続(ロボット所有者のみ)
ソフトウェアの概要¶
下に HIRO / NEXTAGE OPEN の QNX と Ubuntu のソフトウェアコンポーネント構成図を示します． ユーザーは ROS と RTM(hrpsys) のどちらを利用してもロボットを制御するソフトウェアをプログラムすることができます．
API の概要¶
HIRO / NEXTAGE OPEN で使用する API をシステムの観点で大別すると RTM と ROS の2種があり， ROS API でも ROS インタフェース と MoveIt! インタフェースに分けることができます．
- RTM API on hrpsys
- ROS API
- ROS インタフェース
- MoveIt! インタフェース
それぞれの API が Python や C++ などのプログラミング言語で利用することができます．
下図にそれら API の構成図を示します．
各 API の利用法¶
各 API の利用法や使い分けについては次を参考にしてください．
- RTM ベースのインタフェースの方が実装されている機能が多い
- HIRONX ロボットが元々 OpenRTM 上で動作しており，それに対して RTM プロセスと ROS で書かれたプログラムを繋ぐ hrpsys_ros_bridge を作った経緯のため
- キャリブレーションやサーボの ON/OFF などの基本的な操作は RTM インタフェースを使用
- 全ての API は同一プログラム言語で書かれた1つのプログラムファイル内で混成利用可能
保守・管理¶
Ubuntuソフトウェアのアップデート¶
Ubuntu 上の全てのソフトウェアをアップデートする場合は次のコマンドを実行してください．
$ sudo apt-get update && sudo apt-get dist-upgrade
HIRO / NEXTAGE OPEN のソフトウェアのみをアップデートする場合は次のコマンドを実行してください．
$ sudo apt-get update && sudo apt-get ros-indigo-rtmros-nextage ros-indigo-rtmros-hironx
QNX での作業¶
QNX GUIツール - NextageOpenSupervisor¶
QNX に関する日常的な作業のうち次のものは カワダロボティクスから提供されている GUI ツール NextageOpenSupervisor を用いて操作することができます．
- コントロールボックス（QNXコンピュータ）のシャットダウン処理
- アップデート
このツールは UIコンピュータ（Ubuntuコンピュータ/ビジョンPC）に nxouser アカウントでログオンするとデスクトップ上にアイコンがあります．
QNXのコマンド操作¶
本作業は QNX にログオンできることを前提としたものです． 次のコマンドで QNX にSSH接続とログオンを行います．
YOURHOST$ ssh -l %QNX_YOUR_USER% %IPADDR_YOUR_QNX%
QNX シャットダウンコマンド¶
QNX$ su -c '/opt/grx/bin/shutdown.sh'
QNX リブートコマンド¶
QNX$ su -c '/bin/shutdown'
ログファイルの定期メンテナンス¶
QNXのログファイル¶
QNX のログファイルは /opt/jsk/var/log にあります．
- Nameserver.log
- Modelloader.log
- rtcd.log
これらのログファイルは自動的に生成されますが，自動的には削除されません．
ログファイルの圧縮・削除¶
ディスクスペースにおいてログファイルはすぐに何ギガバイトにもなってしまします．
これらのログファイルは自動的に削除される仕組みにはなっておりませんので，
時おり /opt/jsk/var/log 下のログファイルを削除することをお勧めします．
ログファイルの削除はスクリプトの実行もしくはリモートログインによる手動操作で行うことができます．
スクリプト操作によるログファイルの圧縮¶
# Remove all raw .log files to free disk space. Same .zip file will be kept in the log folder.
$ rosrun hironx_ros_bridge qnx_fetch_log.sh nextage qnx_nxo_user archive
手動でのログファイルの削除¶
QNX コンピュータログオンできる場合においては SSH 接続をして，
ディレクトリ /opt/jsk/var/log 下にあるログファイルを削除することができます．
ディスク空き容量の確認¶
QNX コンピュータログオンできる場合においては SSH 接続をして， ディスクの空き容量を確認することができます．
QNX$ df -h
ディスク空き容量の例
/dev/hd0t179 7.8G 7.1G 725M 92% /
/dev/hd0t178 229G 22G 207G 10% /opt/
/dev/hd0 466G 466G 0 100%
QNXでのインストールやソフトウェア利用¶
HIRO / NEXTAGE OPEN ソフトウェアはすべて公開されていますが， それは HIRO / NEXTAGE OPEN でダウンロードしてただ実行するだけで済むということを意味していません． それらのソフトウェアは QNX オペレーティングシステム上で動いているコントローラボックスで ビルド・コンパイルする必要があります． QNX 上ででコンパイルするには，地域のベンダーから購入できる開発者ライセンスが必要です．
QNX に必要なソフトウェアをインストールする方法も開示されていません． QNX は開発者ライセンス以外にも，その運用者は運用認定が必要もしくは十分に運用に精通している必要がある商用ソフトウェアです． また，QNX では頑強なパッケージングインフラストラクチャ（ROSのようなもの）がないため， インストール作業は非常に長い手作業となる可能性があり，エラーが発生しやすくなります．
しかし，次のような場合には QNX 内での作業が必要となります．
- 以前のクローズドソースの GRX コントローラに戻す必要があるとき
- デバッグ時に Ubuntu ソフトウェアの API で見られるログだけでは原因が不明な場合に QNX 上のログファイルを見るとき
このような場合には製造元またはソフトウェアサービスプロバイダからログオンアカウント情報を入手することができます．
開発用ワークステーションのセットアップ¶
Debian バイナリパッケージからのインストール¶
Debian バイナリパッケージからソフトウェアのインストールを行います．
ROS および HIRO / NEXTAGE OPEN ソフトウェアのインストール¶
ROS Indigo および HIRO，NEXTAGE OPEN のパッケージをインストールします． ターミナルを開いて次のコマンドを実行してください．．
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net --recv-key0xB01FA116
$ sudo apt-get update && sudo apt-get install ros-indigo-rtmros-nextage ros-indigo-rtmros-hironx
インストールの最後に setup.bash を読み込み，ROS の環境を設定します．
$ source /opt/ros/indigo/setup.bash
これは新しくターミナルを立ち上げて ROS を使用する前に毎回必要になります． 下記のように .bashrc ファイルに設定を加えて ターミナル起動時に setup.bash を自動で実行し ROS 環境になるようにしておくと便利です．
$ echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc
- 注意: 上記コマンドの
>>を>にしてしまうと元々あった .bashrc 内の設定が消えてしまうので気をつけてください．
HIRO / NEXTAGE OPEN ソフトウェアのみのインストール¶
ROS Indigo が既にインストールされていて HIRO / NEXTAGE OPEN のソフトウェアのみをインストールする必要がある場合は次の1行だけ実行してください．
$ sudo apt-get update && sudo apt-get install ros-indigo-rtmros-nextage ros-indigo-rtmros-hironx
ネットワーク設定¶
ホスト名の設定（オプション）¶
デフォルトの HIRO / NEXTAGE OPEN のソフトウェアでは QNX コンピュータのホスト名を通信の際に使います． ホスト名の設定をオペレーティングシステム上で行っておくと便利です．
- 次の行を
/etc/hostsに追加
# For accessing QNX for NEXTAGE Open
192.168.128.10 nextage
- IP アドレスはユーザの環境により上記のものとは異なる場合があります．
pingを打って nextage から返ってくることを確認してください．- シミュレータを利用するために上記の設定を変更する必要はありません．
192.168.128.xセグメントを使用するネットワークアプリケーションを使用している場合を除き， この設定はネットワークの使用には有害ではありません．- 既知の OpenRTM の問題 https://github.com/start-jsk/rtmros_hironx/issues/33 を回避するため
ネットワーク接続に
eth0が使用されていることを確認してください．
ワークスペースのセットアップ¶
HIRO / NEXTAGE OPEN のプログラムコードを作成・ビルドする必要がない場合は 本項目のインストール手順は不要です．
下記のセットアップ手順ではワークスペース名を catkin_ws (catkin workspace) としていますが
他の名前・フォルダ名でも大丈夫です．
$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws
$ wstool init src
$ wstool merge -t src https://raw.githubusercontent.com/tork-a/rtmros_nextage/indigo-devel/hironxo.rosinstall
$ wstool update -t src
$ source /opt/ros/indigo/setup.bash
$ rosdep update && rosdep install -r -y --from-paths src --ignore-src
$ catkin build
$ source devel/setup.bash
ターミナルを開くたびに source ~/catkin_ws/devel/setup.bash
を実行したくない場合は .bashrc に設定します．
$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
- 注意: 上記コマンドの
>>を>にしてしまうと元々あった .bashrc 内の設定が消えてしまうので気をつけてください．
トラブルシューティング¶
ソフトウェア情報の確認¶
コントローラボックス（QNX）の hrpsys のバージョンの確認¶
Python インタフェースから次のコマンドを実行して コントローラボックス（QNXコンピュータ）で動作している hrpsys のバージョンを確認してください．
In [1]: robot.hrpsys_version
Out[1]: '315.2.8
コントローラボックスにログオンできる場合にはリモート接続をして 次のコマンドを実行して確認することもできます．
- 注意: バージョン 315.2.0 以降にて可能
$ cd /opt/jsk/lib
$ ls -l RobotHardware.so
$ strings RobotHardware.so | grep ^[0-9]*\\.[0-9]*\\.[0-9]*$
315.2.0
RTM ステートのモニタリング¶
ロボットホスト上で動作しているRTコンポーネントのリストを取得するには rtls を利用します．
- 注意: 最後のスラッシュ
/を忘れずに入力してください．
$ rtls %HOST_ROBOT%:15005/
シミュレーションの場合の例
$ rtls localhost:15005/Nextage Open の場合の例（ホスト名をユーザの環境に合わせて変更してください）
$ rtls nextage:15005/
rtls の実行例（シミュレーション）
$ rtls localhost:15005/
robotuser-PC.host_cxt/ StateHolderServiceROSBridge.rtc
fk.rtc DataLoggerServiceROSBridge.rtc
longfloor(Robot)0.rtc ImageSensorROSBridge_HandLeft.rtc
sh.rtc HiroNX(Robot)0.rtc
ImageSensorROSBridge_HeadRight.rtc seq.rtc
HrpsysJointTrajectoryBridge0.rtc log.rtc
HGcontroller0.rtc sc.rtc
ModelLoader el.rtc
CollisionDetector0.rtc ImageSensorROSBridge_HandRight.rtc
ImageSensorROSBridge_HeadLeft.rtc HrpsysSeqStateROSBridge0.rtc
SequencePlayerServiceROSBridge.rtc ForwardKinematicsServiceROSBridge.rtc
ic.rtc rmfo.rtc
rtls で得られたリストの各RTコンポーネントの情報を得るには rtcat を利用します．
$ rtls %HOST_ROBOT%:15005/%CONPONENT_NAME%
rtcat の実行例（シミュレーション）
$ rtcat localhost:15005/fk.rtc
fk.rtc Active
Category example
Description forward kinematics component
Instance name fk
Type name ForwardKinematics
Vendor AIST
Version 315.14.0
Parent
Type Monolithic
+Extra properties
+Execution Context 0
+DataInPort: q
+DataInPort: sensorRpy
+DataInPort: qRef
+DataInPort: basePosRef
+DataInPort: baseRpyRef
+CorbaPort: ForwardKinematicsService
$ rtcat localhost:15005/CollisionDetector0.rtc
CollisionDetector0.rtc Active
Category example
Description collisoin detector component
Instance name CollisionDetector0
Type name CollisionDetector
Vendor AIST
Version 315.14.0
Parent
Type Monolithic
+Extra properties
+Execution Context 0
+DataInPort: qRef
+DataInPort: qCurrent
+DataInPort: servoStateIn
+DataOutPort: q
+DataOutPort: beepCommand
+CorbaPort: CollisionDetectorService
$ rtcat localhost:15005/ImageSensorROSBridge_HeadRight.rtc
ImageSensorROSBridge_HeadRight.rtc Active
Category example
Description openrhp image - ros bridge
Instance name ImageSensorROSBridge_HeadRight
Type name ImageSensorROSBridge
Vendor Kei Okada
Version 1.0
Parent
Type Monolithic
+Extra properties
+Execution Context 0
+DataInPort: image
+DataInPort: timedImage
robotuser@robotuser-PC:~$
ROS ステートのモニタリング¶
rqt を用いたモニタリング¶
ROS の rqt は開発時におけるデータの可視化に大変役立つツールセットです． ここでは HIRO / NEXTAGE OPEN において特に便利ないくつかのツールを紹介します．
- 参考: rqt/Plugins - ROS Wiki http://wiki.ros.org/rqt/Plugins
まず rqt を起動します．
$ rqt
次に “Plugins” をクリックして以下のプラグインを選択します．
- ROS Graph ( rqt_graph )
- Robot Monitor ( rqt_robot_monitor )
- Topic Introspection ( rqt_topic )
下図のようなウィンドウが表示されるはずです．
- 注意: この画像はロボットモニタの診断が無効なシミュレータでキャプチャされたため，失効ステータスが表示されています．
QNXでのトラブルシューティング¶
HIRO / NEXTAGE OPEN の QNX 内プロセスの状態を見るリモート監視ツールがないため， その状態を見るためには QNX コンピュータ上のログファイルの内容を見る必要があります．
ログの回収¶
QNX のログファイルは /opt/jsk/var/log にあります．
- Nameserver.log
- OpenRTM または CORBA に関係したログの多くが記載
- Modelloader.log
- OpenHRP3 に関するログ
- rtcd.log
- hrpsys のRTコンポーネントに関連したログ
これらのログファイルは次のいずれかの方法で取得することができます．
【推奨】 ログファイルの zip ボールを取得するスクリプトを実行する¶
- 注意: rtmros_hironx 1.1.25 以降で利用可能
# Simplest
$ rosrun hironx_ros_bridge qnx_fetch_log.sh nextage qnx_nxo_user
:
$ ls
opt_jsk_var_logs_20170602-020236.zip
# Fetch only files generated after certain date. "1" can be anything except "archive"
$ rosrun hironx_ros_bridge qnx_fetch_log.sh nextage qnx_nxo_user 1 2017-01-11
【代替】 QNX にリモート接続する¶
QNX コンピュータに SSH 接続をして，
ディレクトリ /opt/jsk/var/log 下にあるログファイルにアクセスしてください．
ログのチェック¶
ロボット胸部の4つ全てのランプが電源投入後も点滅し続けている場合¶
電源投入後正常起動した場合はロボット胸部の緑と白の LED ライトだけが点滅する状態となります． この場合においてもまだロボット胸部の4つのランプが全て点滅し続けている場合は QNX のログを確認してください．
/opt/jsk/var/log/rtcd.log が次のようになってるかを確認してください．
Logger::Logger: streambuf address = 0x805fc70
hrpExecutionContext is registered
pdgains.file_name: /opt/jsk/etc/HIRONX/hrprtc/PDgains.sav
dof = 15
open_iob - shmif instance at 0x80b3f58
the number of gyros = 0
the number of accelerometers = 0
the number of force sensors = 0
period = 5[ms], priority = 49
/opt/jsk/var/log/Nameserver.log が次のようになってるかを確認してください．
Sat Jan 24 10:55:33 2015:
Starting omniNames for the first time.
Wrote initial log file.
Read log file successfully.
Root context is IOR:010000002b00000049444c3a6f6d672e6f72672f436f734e616d696e672f4e616d696e67436f6e74
6578744578743a312e300000010000000000000070000000010102000d0000003139322e3136382e312e313600009d3a0b00
00004e616d6553657276696365000300000000000000080000000100000000545441010000001c0000000100000001000100
0100000001000105090101000100000009010100035454410800000095fbc2540100001b
Checkpointing Phase 1: Prepare.
Checkpointing Phase 2: Commit.
Checkpointing completed.
Sat Jan 24 11:10:33 2015:
Checkpointing Phase 1: Prepare.
Checkpointing Phase 2: Commit.
Checkpointing completed.
/opt/jsk/var/log/ModelLoader.log が次のようになっているかを確認してください．
ready
loading /opt/jsk/etc/HIRONX/model/main.wrl
Humanoid node
Joint nodeWAIST
Segment node WAIST_Link
Joint nodeCHEST_JOINT0
Segment node CHEST_JOINT0_Link
Joint nodeHEAD_JOINT0
Segment node HEAD_JOINT0_Link
Joint nodeHEAD_JOINT1
Segment node HEAD_JOINT1_Link
VisionSensorCAMERA_HEAD_R
VisionSensorCAMERA_HEAD_L
Joint nodeRARM_JOINT0
Segment node RARM_JOINT0_Link
Joint nodeRARM_JOINT1
Segment node RARM_JOINT1_Link
Joint nodeRARM_JOINT2
Segment node RARM_JOINT2_Link
Joint nodeRARM_JOINT3
Segment node RARM_JOINT3_Link
Joint nodeRARM_JOINT4
Segment node RARM_JOINT4_Link
Joint nodeRARM_JOINT5
Segment node RARM_JOINT5_Link
Joint nodeLARM_JOINT0
Segment node LARM_JOINT0_Link
Joint nodeLARM_JOINT1
Segment node LARM_JOINT1_Link
Joint nodeLARM_JOINT2
Segment node LARM_JOINT2_Link
Joint nodeLARM_JOINT3
Segment node LARM_JOINT3_Link
Joint nodeLARM_JOINT4
Segment node LARM_JOINT4_Link
Joint nodeLARM_JOINT5
Segment node LARM_JOINT5_Link
The model was successfully loaded !
すべてのログが上記のような表示でしたらログに関しては正常で， 他に何か複雑なことが起こっている可能性があります． サポートにお問い合わせください．
チュートリアル: Python インタフェース¶
RTM Python インタフェース¶
インタラクティブモードでの操作¶
rtm_ros_bridge の起動¶
rtm_ros_bridge を起動します．シミュレーションの場合は不要です．
$ roslaunch hironx_ros_bridge hironx_ros_bridge_real.launch nameserver:=%HOSTNAME% (HIRO)
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME% (NEXTAGE OPEN)
iPython インタフェースの起動¶
HIRO での iPython インタフェースの起動 ホスト名は各ロボットの設定に応じて変更してください．
ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py (Simulation)
ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host hiro014 (Real robot example)
NEXTAGE OPEN での iPython インタフェースの起動
ipython -i `rospack find nextage_ros_bridge`/script/nextage.py (Simulation)
ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nextage (Real Robot Example)
他のオプションも必要であれば指定することができます．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --modelfile /opt/jsk/etc/HIRONX/model/main.wrl --host hiro014 --port 15005
$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --modelfile /opt/jsk/etc/HIRONX/model/main.wrl --host nextage101 --port 15005
リモート接続する場合は --robot 引数を使用してロボットのインスタンス名を指定する必要があります．
例えば次のように指定して実行します．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host nxo_simulation_host --robot "HiroNX(Robot)0"
iPython インタフェース初期化時に行っていることは次のようになっています．
- RTCManager と RobotHardware の検出
- RTCManager: RT コンポーネントを起動する OpenRTM のクラス
- RobotHardware: ロボットハードウェアとのインタフェースである hrpsys に定義されているRTコンポーネント
- hrpsys コントローラを動作させるのに必要な RT コンポーネントへの接続とアクティベーション
- ロガーの開始
- Joint Groups への割当て（HIRO/NEXTAGE OPEN 固有）
よく使う RTM Python インタフェースコマンド¶
ロボット実機操作において特有なコマンドを紹介します．
- 注意-1: コマンドを実行するとロボットが動きます．
- 注意-2: 緊急停止スイッチをいつでも押せる状態にしておいてください．
ロボット実機のキャリブレーションを行うコマンドです．
ipython>>> robot.checkEncoders()
- ロボット実機のキャリブレーションがなされていない場合にのみ実行されます．
- ほとんどの場合においてロボット胸部の緑のロボットステートインジケータライトが点滅します．
ロボットを初期姿勢するコマンドです．
ipython>>> robot.goInitial()
作業終了姿勢に移行するコマンドです．終了姿勢に移行した後にサーボが切れます．
ipython>>> robot.goOffPose()
- システムの再起動・シャットダウン時の前にはこのコマンドを実行してください．
サーボを入れるコマンドです．
ipython>>> robot.servoOn()
- 次の動作を行ってサーボが切れた状態のときには手動でサーボを入れる必要があります．
goOffPoseを実行したとき- リレースイッチを押したとき（この場合はまず
servoOffを行う必要があります） - ロボットに異常動作を与えて緊急停止したとき
- 現状の物理的関節角度にて角度指示値を設定します．
goActual()を内部的に呼び出しています．
サーボを切るコマンドです．
ipython>>> robot.servoOff()
- 明示的にサーボを切るのではなくリレースイッチを押した場合にはこのコマンドを実行してください．
RTM Python インタフェースの利用¶
iPython の初期化時にロボットのクライアントインタフェースクラスである HIRONX/NextageClient が iPython ターミナル上で robot ととしてインスタンス化されます．
robot において何ができるのかを見てみます．
In : robot.
上記のように iPython ターミナル上で入力したのに tab キーを押すと選択可能なものが表示されます．
robot.Groups robot.getCurrentRPY robot.rh_svc
robot.HandClose robot.getCurrentRotation robot.saveLog
robot.HandGroups robot.getJointAngles robot.sc
robot.HandOpen robot.getRTCInstanceList robot.sc_svc
robot.InitialPose robot.getRTCList robot.sensors
robot.OffPose robot.getReferencePose robot.seq
robot.RtcList robot.getReferencePosition robot.seq_svc
robot.abc robot.getReferenceRPY robot.servoOff
robot.activateComps robot.getReferenceRotation robot.servoOn
robot.afs robot.getSensors robot.setHandEffort
robot.checkEncoders robot.goActual robot.setHandJointAngles
robot.clearLog robot.goInitial robot.setHandWidth
robot.co robot.goOffPose robot.setJointAngle
robot.co_svc robot.hand_width2angles robot.setJointAngles
robot.configurator_name robot.hgc robot.setJointAnglesOfGroup
robot.connectComps robot.ic robot.setSelfGroups
robot.connectLoggerPort robot.init robot.setTargetPose
robot.createComp robot.isCalibDone robot.setupLogger
robot.createComps robot.isServoOn robot.sh
robot.el robot.kf robot.sh_svc
robot.el_svc robot.lengthDigitalInput robot.simulation_mode
robot.ep_svc robot.lengthDigitalOutput robot.st
robot.findModelLoader robot.liftRobotUp robot.stOff
robot.fk robot.loadPattern robot.tf
robot.fk_svc robot.log robot.vs
robot.flat2Groups robot.log_svc robot.waitForModelLoader
robot.getActualState robot.moveHand robot.waitForRTCManagerAndRoboHardware
robot.getBodyInfo robot.ms robot.waitInterpolation
robot.getCurrentPose robot.readDigitalInput robot.waitInterpolationOfGroup
robot.getCurrentPosition robot.rh robot.writeDigitalOutput
Groups のリストとそのメンバを見てみます．
In : robot.Groups
Out:
[['torso', ['CHEST_JOINT0']],
['head', ['HEAD_JOINT0', 'HEAD_JOINT1']],
['rarm',
['RARM_JOINT0',
'RARM_JOINT1',
'RARM_JOINT2',
'RARM_JOINT3',
'RARM_JOINT4',
'RARM_JOINT5']],
['larm',
['LARM_JOINT0',
'LARM_JOINT1',
'LARM_JOINT2',
'LARM_JOINT3',
'LARM_JOINT4',
'LARM_JOINT5']]]
スクリプトインタラプタとしての iPython の利点はそこから API の情報を得られることにあります．
例えば，ロボットの現在の姿勢を知りたいがそのコマンドを知らないような場合でも， まず少し推測してからタブ補完を利用することで次のように情報を得ることができます．
In : robot.getCurrent
robot.getCurrentPose robot.getCurrentPosition robot.getCurrentRPY robot.getCurrentRotation
getCurren に対して4つの選択肢があり，
この内 getCurrentPose が意図するもののコマンドらしいことが分ります．
さらにそのメソッドの引数を知る必要がある場合にはコマンドの最後に ? を入力します．
In : robot.getCurrentPose?
Type: instancemethod
Base Class: <type 'instancemethod'>
String Form:<bound method HIRONX.getCurrentPose of <__main__.HIRONX instance at 0x1f39758>>
Namespace: Interactive
File: /opt/ros/hydro/lib/python2.7/dist-packages/hrpsys_config.py
Definition: robot.getCurrentPose(self, lname)
Docstring: <no docstring>
ここで getCurrentPose は lname（link name の略）を受け取ることが分ります．
よって次のように実行します．
In: robot.getCurrentPose('RARM_JOINT0')
Out:
[0.912826202314136,
-0.4083482880688395,
0.0,
0.0,
0.39443415756662026,
0.8817224037285941,
-0.25881904510252074,
-0.145,
0.1056883139872261,
0.2362568060275051,
0.9659258262890683,
0.370296,
0.0,
0.0,
0.0,
1.0]
この getCurrentPose は指示したリンクの回転行列と位置の値を1次元のリストとして戻します．
位置だけを知りたい場合には他の次の方法で取得することもできます．
In: robot.getCurrent
robot.getCurrentPose robot.getCurrentPosition robot.getCurrentRPY robot.getCurrentRotation
In : robot.getCurrentPosition('RARM_JOINT0')
Out: [0.0, -0.145, 0.370296]
hrpsys においては位置ベクトルは次のように対応した3つの要素 [x, y, z] で表されます．
- x: 前
- y: 左
- z: 上
次に腕を動かしてみます． まず初期姿勢まで動かします．
In : robot.goInitial()
目標姿勢を設定するにはどうすれば良いかを調べます．
In : robot.setTargetPose?
Type: instancemethod
Base Class: <type 'instancemethod'>
String Form:<bound method HIRONX.setTargetPose of <__main__.HIRONX instance at 0x333b758>>
Namespace: Interactive
File: /opt/ros/hydro/lib/python2.7/dist-packages/hrpsys/hrpsys_config.py
Definition: robot.setTargetPose(self, gname, pos, rpy, tm)
Docstring: <no docstring>
gname は joint group の名前です．
pos と rpy はリスト形式です．
目標姿勢を指定する前に現在のロボットの姿勢を変数に格納します．
In : pos = robot.getCurrentPosition('RARM_JOINT5')
In : rpy = robot.getReferenceRPY('RARM_JOINT5')
In : tm = 3
ロボットの姿勢はおそらく下図のようになっていることと思います．
それでは目標位置を現在の姿勢から少し変えて指示して，そこへの動作を実行させてみましょう．
In : pos[2] = 0.1
In : robot.setTargetPose('rarm', pos, rpy, tm)
Out: True
次の図のように右腕の手先が指定した位置へと移動したことと思います．
- 注意: 下図は MoveIt! が実行されているときにキャプチャしたもので，MoveIt! 由来の黄緑色で表示されている開始姿勢の腕はここのチュートリアルでは関係しませんので無視してください．
ロボットでの作業が終了したら終了姿勢にしてください．
In : robot.goOffPose()
hrpsys-based API のソースとドキュメント¶
hrpsys-based API は次のリンク先にソースとドキュメントがあります．
- 多くのコマンドは hrpsys_config.HrpsysConfigurator のペアレントクラスに定義されています．
- HIRO: hironx_ros_bridge/scripts/
- NEXTAGE OPEN: nextage_ros_bridge/scripts/
RTM Python インタフェースプログラミング¶
Python を用いた HIRO / NEXTAGE OPEN のプログラミングは1つの統合されたインタフェースによって行うことができます． インタフェースの名称はロボットによりそれぞれ異なります．
- HIRONX : HIRO / HIRONX ユーザ
- NextageClient : NEXTAGE OPEN ユーザ
本項では “HIRONX” を使用しますが NEXTAGE OPEN ユーザは NextageClient インタフェースを用いて同様のことを行うことができます．
サンプルコード - Acceptance Test (RTM)¶
Acceptance Test (RTM) のコードを参考に RTM Python インタフェースプログタミング の方法を見たあとにコードを実行します．
acceptancetest_rtm.py¶
まずこのサンプルは下図のような2段階の依存関係を持っています．
このサンプルコードの動作は AcceptanceTest_Hiro クラスに記述されていて
HIRONX クラスに接続する AcceptancetestRTM クラスを利用しています．
AcceptancetestRTM について見てみます．コードの全体は次のようになっています．
1 # -*- coding: utf-8 -*-
2
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of TORK. nor the
19 # names of its contributors may be used to endorse or promote products
20 # derived from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34
35 import time
36
37 from hironx_ros_bridge.constant import Constant
38 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
39
40
41 class AcceptanceTestRTM(AbstAcceptanceTest):
42
43 def __init__(self, robot_client):
44 '''
45 @type robot_client: hironx_ros_bridge.hironx_client.HIRONX
46 '''
47 self._robotclient = robot_client
48
49 def go_initpos(self):
50 self._robotclient.goInitial()
51
52 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None,
53 task_duration=7.0, do_wait=True):
54 '''
55 @see: AbstAcceptanceTest.set_joint_angles
56 '''
57 print("== RTM; {} ==".format(msg_tasktitle))
58 self._robotclient.setJointAnglesOfGroup(
59 joint_group, joint_angles, task_duration, do_wait)
60
61 def set_pose(self, joint_group, pose, rpy, msg_tasktitle,
62 task_duration=7.0, do_wait=True, ref_frame_name=None):
63
64 print("== RTM; {} ==".format(msg_tasktitle))
65 self._robotclient.setTargetPose(joint_group, pose, rpy, task_duration,
66 ref_frame_name)
67 if do_wait:
68 self._robotclient.waitInterpolationOfGroup(joint_group)
69
70 def set_pose_relative(
71 self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0,
72 msg_tasktitle=None, task_duration=7.0, do_wait=True):
73 if joint_group == Constant.GRNAME_LEFT_ARM:
74 eef = 'LARM_JOINT5'
75 elif joint_group == Constant.GRNAME_RIGHT_ARM:
76 eef = 'RARM_JOINT5'
77
78 print("== RTM; {} ==".format(msg_tasktitle))
79 self._robotclient.setTargetPoseRelative(
80 joint_group, eef, dx, dy, dz, dr, dp, dw,
81 task_duration, do_wait)
82
83 def _run_tests_hrpsys(self):
84 '''
85 @deprecated: This method remains as a reference. This used to function
86 when being called directly from ipython commandline and
87 now replaced by optimized codes.
88 '''
89 _TIME_SETTARGETP_L = 3
90 _TIME_SETTARGETP_R = 2
91 _TIME_BW_TESTS = 5
92
93 self.robot.goInitial()
94
95 # === TASK-1 ===
96 # L arm setTargetPose
97 _POS_L_INIT = self.robot.getCurrentPosition('LARM_JOINT5')
98 _POS_L_INIT[2] += 0.8
99 _RPY_L_INIT = self.robot.getCurrentRPY('LARM_JOINT5')
100 self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_SETTARGETP_L)
101 self.robot.waitInterpolationOfGroup('larm')
102
103 # R arm setTargetPose
104 _POS_R_INIT = self.robot.getCurrentPosition('RARM_JOINT5')
105 _POS_R_INIT[2] -= 0.07
106 _RPY_R_INIT = self.robot.getCurrentRPY('RARM_JOINT5')
107 self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_SETTARGETP_R)
108 self.robot.waitInterpolationOfGroup('rarm')
109 time.sleep(_TIME_BW_TESTS)
110
111 # === TASK-2 ===
112 self.robot.goInitial()
113 # Both arm setTargetPose
114 _Z_SETTARGETP_L = 0.08
115 _Z_SETTARGETP_R = 0.08
116 self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
117 dz=_Z_SETTARGETP_L,
118 tm=_TIME_SETTARGETP_L, wait=False)
119 self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
120 dz=_Z_SETTARGETP_R,
121 tm=_TIME_SETTARGETP_R, wait=False)
122
123 # === TASK-3 ===
124 # Head toward down
125 _TIME_HEAD = 5
126 self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=0.1, tm=_TIME_HEAD)
127 self.robot.waitInterpolationOfGroup('head')
128 # Head toward up
129 self.robot.setTargetPoseRelative('head', 'HEAD_JOINT0', dp=-0.2, tm=_TIME_HEAD)
130 self.robot.waitInterpolationOfGroup('head')
131 # See left by position
132 self.robot.setJointAnglesOfGroup('head', [50, 10], 2, wait=True)
133 # See right by position
134 self.robot.setJointAnglesOfGroup('head', [-50, -10], 2, wait=True)
135 # Set back face to the starting pose w/o wait.
136 self.robot.setJointAnglesOfGroup( 'head', [0, 0], 2, wait=False)
137
138 # === TASK-4 ===
139 # 0.1mm increment is not working for some reason.
140 self.robot.goInitial()
141 # Move by iterating 0.1mm at cartesian space
142 _TIME_CARTESIAN = 0.1
143 _INCREMENT_MIN = 0.0001
144 for i in range(300):
145 self.robot.setTargetPoseRelative('larm', 'LARM_JOINT5',
146 dy=_INCREMENT_MIN,
147 tm=_TIME_CARTESIAN)
148 self.robot.setTargetPoseRelative('rarm', 'RARM_JOINT5',
149 dy=_INCREMENT_MIN,
150 tm=_TIME_CARTESIAN)
151 print('{}th move'.format(i))
152
153 self.robot.goInitial()
154 # === TASK-5 ===
155 # Turn torso
156 _TORSO_ANGLE = 120
157 _TIME_TORSO_R = 7
158 self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R, wait=True)
159 self.robot.waitInterpolationOfGroup('torso')
160 self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
161
162 self.robot.goInitial()
163
164 # === TASK-6.1 ===
165 # Overwrite previous command, for torso using setJointAnglesOfGroup
166 self.robot.setJointAnglesOfGroup('torso', [_TORSO_ANGLE], _TIME_TORSO_R,
167 wait=False)
168 time.sleep(1)
169 self.robot.setJointAnglesOfGroup('torso', [-_TORSO_ANGLE], 10, wait=True)
170
171 self.robot.goInitial(5)
172
173 # === TASK-6.2 ===
174 # Overwrite previous command, for arms using setTargetPose
175 _X_EEF_OVERWRITE = 0.05
176 _Z_EEF_OVERWRITE = 0.1
177 _TIME_EEF_OVERWRITE = 7
178 _POS_L_INIT[0] += _X_EEF_OVERWRITE
179 _POS_L_INIT[2] += _Z_EEF_OVERWRITE
180 self.robot.setTargetPose('larm', _POS_L_INIT, _RPY_L_INIT, _TIME_EEF_OVERWRITE)
181 self.robot.waitInterpolationOfGroup('larm')
182 # Trying to raise rarm to the same level of larm.
183 _POS_R_INIT[0] += _X_EEF_OVERWRITE
184 _POS_R_INIT[2] += _Z_EEF_OVERWRITE
185 self.robot.setTargetPose('rarm', _POS_R_INIT, _RPY_R_INIT, _TIME_EEF_OVERWRITE)
186 self.robot.waitInterpolationOfGroup('rarm')
187 time.sleep(3)
188 # Stop rarm
189 self.robot.clearOfGroup('rarm') # Movement should stop here.
190
191 # === TASK-7.1 ===
192 # Cover wide workspace.
193 _TIME_COVER_WORKSPACE = 3
194 # Close to the max width the robot can spread arms with the hand kept
195 # at table level.
196 _POS_L_X_NEAR_Y_FAR = [0.32552812002303166, 0.47428609880442024, 1.0376656470275407]
197 _RPY_L_X_NEAR_Y_FAR = (-3.07491977663752, -1.5690249316560323, 3.074732073335767)
198 _POS_R_X_NEAR_Y_FAR = [0.32556456455769633, -0.47239119592815987, 1.0476131608682244]
199 _RPY_R_X_NEAR_Y_FAR = (3.072515432213872, -1.5690200270375372, -3.072326882451363)
200
201 # Close to the farthest distance the robot can reach, with the hand kept
202 # at table level.
203 _POS_L_X_FAR_Y_FAR = [0.47548142379781055, 0.17430276793604782, 1.0376878025614884]
204 _RPY_L_X_FAR_Y_FAR = (-3.075954857224205, -1.5690261926181046, 3.0757659493049574)
205 _POS_R_X_FAR_Y_FAR = [0.4755337947019357, -0.17242322190721648, 1.0476395479774052]
206 _RPY_R_X_FAR_Y_FAR = (3.0715850722714944, -1.5690204449882248, -3.071395243174742)
207 self.robot.setTargetPose('larm', _POS_L_X_NEAR_Y_FAR, _RPY_L_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
208 self.robot.setTargetPose('rarm', _POS_R_X_NEAR_Y_FAR, _RPY_R_X_NEAR_Y_FAR, _TIME_COVER_WORKSPACE)
209 self.robot.waitInterpolationOfGroup('larm')
210 self.robot.waitInterpolationOfGroup('rarm')
211 time.sleep(3)
212 self.robot.setTargetPose('larm', _POS_L_X_FAR_Y_FAR, _RPY_L_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
213 self.robot.setTargetPose('rarm', _POS_R_X_FAR_Y_FAR, _RPY_R_X_FAR_Y_FAR, _TIME_COVER_WORKSPACE)
214 self.robot.waitInterpolationOfGroup('larm')
215 self.robot.waitInterpolationOfGroup('rarm')
216
217 self.robot.goInitial()
タスクの内容を少し見てみます．
37 from hironx_ros_bridge.constant import Constant
38 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
RTM API の HIRONX もしくは NextageClient クラスがインポートされていません．しかし，次の部分を見ると...
43 def __init__(self, robot_client):
44 '''
45 @type robot_client: hironx_ros_bridge.hironx_client.HIRONX
46 '''
47 self._robotclient = robot_client
コンストラクタが HIRONX を受け取り，メンバー self._robotclient に格納していることが分ります．
以後は self._robotclient が HIRONX クラスインスタンスを参照することとなります．
コードの残りの部分は基本的な操作を行っているだけです．
例えば次のような部分です． 簡潔に全ての関節が初期姿勢になるように記述されています．
49 def go_initpos(self):
50 self._robotclient.goInitial()
後々，自分のコードを開発してゆくうちに， 各クラスとメソッドの API ドキュメントを見てオプションを知りたくなるかもしれません． このチュートリアルでは詳しく説明しませんので，ぜひご自身で探索してみてください． 例えばgoInitialメソッドには次のオプションがあります．
def hironx_ros_bridge.hironx_client.HIRONX.goInitial(self, tm = 7, wait = True, init_pose_type = 0)
更に例を挙げると次の部分です．
52 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None,
53 task_duration=7.0, do_wait=True):
54 '''
55 @see: AbstAcceptanceTest.set_joint_angles
56 '''
57 print("== RTM; {} ==".format(msg_tasktitle))
58 self._robotclient.setJointAnglesOfGroup(
59 joint_group, joint_angles, task_duration, do_wait)
こちらも簡潔に HIRONX のメソッドを呼び出すだけで， 多くのことがすでに HIRONX 内で処理されているので， このような簡単な方法でコードを書くことができます．
サンプルコードの実行¶
新しい iPython ターミナルを開きます． 少なくとも3つのターミナルが開いた状態になっています． 次のように iPython ターミナルを起動します． シミュレーション環境の場合はそれに合わせて起動してください．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/acceptancetest_hironx.py -- --host %HOSTNAME%
次のコマンドで RTM インタフェースを通じてタスクが実行されます．
- 注意: ロボットが動きます．
IN [1]: acceptance.run_tests_rtm()
次のように記述することでタスクの逐次実行もできます．
IN [1]: acceptance.run_tests_rtm(do_wait_input=True)
iPython インタフェースを終了するときは Ctrl-d にてエスケーブします．
サンプルコード - 円を描く¶
ロボットのエンドエフェクタで円を描くサンプルコードを下記に示します．
- 注意: 本コードを改変する場合はまずシミュレーションでその動作を確認してから実機で動作させてください．
[NEXTAGE OPEN] Robots hands drawing circles: https://www.youtube.com/watch?v=OVae1xa5Rak
変数 robot は何らかの方法でユーザの HIRONX/NextageClient クラスのインスタンスに置き換える必要があります．
def circle_eef(radius=0.01, eef='larm', step_degree=5, ccw=True, duration=0.1):
'''
Moves the designated eef point-by-point so that the trajectory as a whole draws a circle.
Currently this only works on the Y-Z plane of *ARM_JOINT5 joint.
And it's the most intuitive when eef maintains a "goInitial" pose where circle gets drawn on robot's X-Y plane
(see the wiki for the robot's coordinate if you're confused http://wiki.ros.org/rtmros_nextage/Tutorials/Programming#HiroNXO_3D_model_coordination).
Points on the circular trajectory is based on a standard equation https://en.wikipedia.org/wiki/Circle#Equations
@param radius: (Unit: meter) Radius of the circle to be drawn.
@param step_degree: Angle in degree each iteration increments.
@param ccw: counter clock-wise.
@param duration: Time for each iteration to be completed.
'''
goal_deg = GOAL_DEGREE = 360
start_deg = 0
if eef == 'larm':
joint_eef = 'LARM_JOINT5'
elif eef == 'rarm':
joint_eef = 'RARM_JOINT5'
eef_pos = robot.getCurrentPosition(joint_eef)
eef_rpy = robot.getCurrentRPY(joint_eef)
print('eef_pos={}'.format(eef_pos))
X0 = eef_pos[0]
Y0 = eef_pos[1]
ORIGIN_x = X0
ORIGIN_y = Y0 - radius
print('ORIGIN_x={} ORIGIN_y={}'.format(ORIGIN_x, ORIGIN_y))
i = 0
for theta in range(start_deg, goal_deg, step_degree):
if not ccw:
theta = -theta
x = ORIGIN_x + radius*math.sin(math.radians(theta)) # x-axis in robot's eef space is y in x-y graph
y = ORIGIN_y + radius*math.cos(math.radians(theta))
eef_pos[0] = x
eef_pos[1] = y
print('#{}th theta={} x={} y={} X0={} Y0={}'.format(i, theta, x, y, X0, Y0))
robot.setTargetPose(eef, eef_pos, eef_rpy, duration)
robot.waitInterpolation()
i += 1
使用場面に応じたプログラミング¶
デフォルトではいくつかの HIRONX / NextageClient クラスのコマンドは
動作終了を待ってから次の動作を開始します．
またいくつかのコマンドはそのように待たないものもあります．
それは各メソッドが受け取る引数から明確になる実装に依存します．
wait 引数を持つメソッドは待機するかどうかを指定できます．
それ以外のものは特に API ドキュメントに記述の無いかぎり待機の可否を指定できません．
次は “待機” の特性を持つメソッドの例です．
70 def set_pose_relative(
71 self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0,
72 msg_tasktitle=None, task_duration=7.0, do_wait=True):
73 if joint_group == Constant.GRNAME_LEFT_ARM:
74 eef = 'LARM_JOINT5'
75 elif joint_group == Constant.GRNAME_RIGHT_ARM:
76 eef = 'RARM_JOINT5'
77
78 print("== RTM; {} ==".format(msg_tasktitle))
79 self._robotclient.setTargetPoseRelative(
80 joint_group, eef, dx, dy, dz, dr, dp, dw,
81 task_duration, do_wait)
一般にはデフォルトでは wait=True とした方が安全となるでしょう．
次の setTargetPose は待機・中断の信号は受け取りません．
この場合においては HrpsysConfigurator の waitInterpolationOfGroup() を呼び出して対処することができます．
HrpSysConfigurator は HIRONX の親クラスです．
61 def set_pose(self, joint_group, pose, rpy, msg_tasktitle,
62 task_duration=7.0, do_wait=True, ref_frame_name=None):
63
64 print("== RTM; {} ==".format(msg_tasktitle))
65 self._robotclient.setTargetPose(joint_group, pose, rpy, task_duration,
66 ref_frame_name)
67 if do_wait:
68 self._robotclient.waitInterpolationOfGroup(joint_group)
連続した軌道座標指示による動作¶
次のようにすることで連続した軌道座標指示による動作を行うことができます．
hcf.playPatternOfGroup('LARM',
[[0.010,0.0,-1.745,-0.265,0.164,0.06],
[0.010,-0.2,-2.045,-0.265,0.164,0.06],
[0.010,-0.4,-2.245,-0.265,0.164,0.06],
[0.010,-0.6,-2.445,-0.265,0.164,0.06],
[0.010,-0.8,-2.645,-0.265,0.164,0.06]],
[1,1,1,1,1])
hcf は robot など Python インタフェースを起動した状況に応じて変更してください．
hrpsys 315.6.0 以降では setJointAnglesSequenceOfGroup も利用することができます．
相対姿勢指示による動作¶
エンドエフェクタのフレームや関節を現在の姿勢からの相対姿勢を指示して動作させるのも
HIRONX インタフェースの setTargetPoseRelative を用いることで簡単に行うことができます．
下記の [1] では torso を 3[s] かけて 0.1[rad] 回転させます．
[2] では右腕のエンドエフェクタフレームを 3[s] かけて前方に 0.1[m] 移動させます．
In [1]: robot.setTargetPoseRelative('torso', 'CHEST_JOINT0', dw=0.1, tm=3)
In [2]: robot.setTargetPoseRelative('rarm', 'RARM_JOINT5', dx=0.1, tm=3)
ユーザ Python コードの作成¶
ここまでの RTM Python インタフェースのチュートリアルでは hironx.py や nextage.py といったスクリプトを実行していました．
クライアントインタフェースクラスのメソッドを利用して
アプリケーションモジュールを作成する方法を考えてみます．
そのモジュールを your_nxo_sample.py と名付け，
それをシミュレーションなり実機なりで動作させるものとします．
- まず1つの方法として
nextage.pyを複製してNextageClientクラスがインスタンス化される行を書き換えます． 他のNextageClientクラスがアクセスする部分も消したり書き換えたりします． - 上記の方法はあまりスマートで簡潔ではありません． それは大部分のコードを複製するというのはソフトウェア開発においては悪い実践法であるからです． 例えば hrpsys_tools/hrpsys_tools_config.py は この目的に役立つことを意図して作られています．
- 参考: 作成時のディスカッション - https://github.com/start-jsk/rtmros_common/issues/340
デジタルI/O の利用（NEXTAGE OPEN）¶
本項はデジタルI/O（DIO）が備わっている NEXTAGE OPEN でのみ有効です．
DIO 操作で利用できるメソッドの全体については利用可能なメソッドが集約されている API ドキュメント，
特にデフォルトで利用可能なメソッドが集められている NextageClient クラスを参照してください。
- APIドキュメント: http://docs.ros.org/hydro/api/nextage_ros_bridge/html/annotated.html
- NextageClient: http://docs.ros.org/hydro/api/nextage_ros_bridge/html/classnextage__ros__bridge_1_1nextage__client_1_1NextageClient.html
RTM クライアントからのインタラクティブな操作¶
まず RTM クライアントを実行し，ネームサーバー内部（つまり QNX コントローラボックス上）のロボットに接続する必要があります．
$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host %HOST%
そうすると _hands オブジェクトから全ての DIO メソッドにアクセスすることができます．
iPython ターミナルで表示させると次のようになります．
In [6]: robot._hands.h
robot._hands.handlight_both robot._hands.handtool_l_attach robot._hands.handtool_r_eject
robot._hands.handlight_l robot._hands.handtool_l_eject
robot._hands.handlight_r robot._hands.handtool_r_attach
In [6]: robot._hands.g
robot._hands.gripper_l_close robot._hands.gripper_r_close
robot._hands.gripper_l_open robot._hands.gripper_r_open
In [6]: robot._hands.a
robot._hands.airhand_l_drawin robot._hands.airhand_l_release robot._hands.airhand_r_keep
robot._hands.airhand_l_keep robot._hands.airhand_r_drawin robot._hands.airhand_r_release
ハンドツールを接続・取り外しする場合は次のように行います．
- ハンドへのツールの接続
- ツールチェンジャの先端にあるソレノイドバルブが閉じている必要があります．
- ハンドからツールの取り外し
他のよく使われるメソッドについてはその名前自体から大体の機能は類推できるかと思います．
- グリッパの開閉:
gripper_l_open,gripper_r_open,gripper_l_close,gripper_r_close - ライトの点灯等:
handlight_both,handlight_l,handlight_r
サンプルとしてハンドDIOのシステムテストツールも参考にしてください．
2014年8月よりも前に出荷された NEXTAGE OPEN での DIO の利用¶
2014年8月よりも前に出荷された NEXTAGE OPEN で DIO を利用する場合は DIO のバージョンを指定する必要があります．
$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host %HOST% --dio_ver 0.4.2
NextageClient インスタンスに DIO を指定する場合には
set_hand_version メソッドにて引数 0.4.2 を与えて指定してください．
このバージョン番号は固定で，変更する必要はありません．
ROS Python インタフェース¶
インタラクティブモードでの操作¶
rtm_ros_bridge の起動¶
rtm_ros_bridge を起動します．シミュレーションンの場合は不要です．
$ roslaunch hironx_ros_bridge hironx_ros_bridge_real.launch nameserver:=%HOSTNAME% (HIRO)
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME% (NEXTAGE OPEN)
ROSのノードが動作しているかを確認してみます．
$ rosnode list
/diagnostic_aggregator
/hrpsys_profile
/hrpsys_ros_diagnostics
/hrpsys_state_publisher
/rosout
iPython ターミナルの実行¶
インタラクティブにロボットを操作できるように iPython インタラクティブコンソールを実行します．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host hiro014 （HIRO)
$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nextage101 (NEXTAGE OPEN)
: (same initialization step as simulation)
[hrpsys.py] initialized successfully
シミュレーションの場合は引数は不要です．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py
$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py
これらは RTM インタフェースの時と同じです．
つまり hironx.py や nextage.py を実行したときに
それらが RTM クライアントと ROS クライアントの両方を起動るということです．
iPython コンソールを起動したときに次のようなエラーが出ることがあります． これは MoveIt! のサービスが起動していないという内容のエラーです． ROS クライアントメソッドのいくつかは MoveIt! が無くても動作しますので ここではこのエラーを無視しても大丈夫です．
:
[hrpsys.py] initialized successfully
[INFO] [WallTime: 1410918126.749067] [206.795000] Joint names; Torso: ['CHEST_JOINT0']
Head: ['HEAD_JOINT0', 'HEAD_JOINT1']
L-Arm: ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5']
R-Arm: ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']
[ERROR] [1410918130.900627643, 210.289999999]: Robot semantic description not found. Did you forget to define or remap '/robot_description_semantic'?
[ INFO] [1410918130.900757547, 210.289999999]: Loading robot model 'HiroNX'...
[ INFO] [1410918130.900794422, 210.289999999]: No root joint specified. Assuming fixed joint
[FATAL] [1410918131.991522557, 211.249999999]: Group 'left_arm' was not found.
[ERROR] [WallTime: 1410918132.006310] [211.285000] Group 'left_arm' was not found.
Make sure you've launched MoveGroup (e.g. by launching moveit_planning_execution.launch)
ROS Python インタフェースコマンド¶
実行している iPython コンソールから ROS Python インタフェースのコマンドを見てみます．
次のように ros. と入力し TAB キーを押します．
In [1]: ros.
ros.Joint ros.get_joint ros.go_init
ros.Link ros.get_joint_names ros.go_offpose
ros.get_current_state ros.get_link ros.has_group
ros.get_current_variable_values ros.get_link_names ros.set_joint_angles_deg
ros.get_default_owner_group ros.get_planning_frame ros.set_joint_angles_rad
ros.get_group ros.get_root_link ros.set_pose
ros.get_group_names ros.goInitial
ROS_Client は ros というオブジェクトから利用することができます．
ROS_Client は MoveIt! から RobotCommander を継承するしていて，
その派生クラスから多くのメソッドが上記のように見えているのです．
加えて goInitial,go_init, go_offpose, set_pose などの ROS_Client 用にいくつかのメソッドが実装されています．
これらのメソッドはすべて RTM バージョン の Hironx/NEXTAGE OPEN Python インターフェイスと同等のものです．
次のコマンドでロボットの全関節が初期姿勢に移行します．
- 注意-1: コマンドを実行するとロボットが動きます．
- 注意-2: 緊急停止スイッチをいつでも押せる状態にしておいてください．
In [1]: ros.go_init()
[INFO] [WallTime: 1410918153.591171] [226.790000] *** go_init begins ***
[INFO] [WallTime: 1410918165.419528] [233.825000] wait_for_result for the joint group rarm = True
[INFO] [WallTime: 1410918165.423372] [233.825000] [positions: [0.010471975511965976, 0.0, -1.7453292519943295, -0.26529004630313807, 0.16406094968746698, -0.05585053606381855]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 7
nsecs: 0]
iPython の機能を使うと RTM の場合と同様に各 ROS Python インタフェースコマンドのAPIドキュメントを見ることができます．
In [2]: ros.go_init?
Type: instancemethod
Base Class: <type 'instancemethod'>
String Form:<bound method ROS_Client.go_init of <hironx_ros_bridge.ros_client.ROS_Client object at 0x49b7210>>
Namespace: Interactive
File: /home/rosnoodle/cws_hiro_nxo/src/start-jsk/rtmros_hironx/hironx_ros_bridge/src/hironx_ros_bridge/ros_client.py
Definition: ros.go_init(self, task_duration=7.0)
Docstring:
Init positions are taken from HIRONX.
TODO: Need to add factory position too that's so convenient when
working with the manufacturer.
@type task_duration: float
RTM Python インタフェースとの互換性により，
同等の機能を持つメソッドの名前が違っていても（goInitial）同じでもどちらでも利用することができます．
IN [3]: ros.goInitial?
Type: instancemethod
String Form:<bound method ROS_Client.goInitial of <hironx_ros_bridge.ros_client.ROS_Client object at 0x7f23458f0c50>>
File: /home/n130s/link/ROS/indigo_trusty/cws_hironxo/src/start-jsk/rtmros_hironx/hironx_ros_bridge/src/hironx_ros_bridge/ros_client.py
Definition: ros.goInitial(self, init_pose_type=0, task_duration=7.0)
Docstring:
This method internally calls self.go_init.
This method exists solely because of compatibility purpose with
hironx_ros_bridge.hironx_client.HIRONX.goInitial, which
holds a method "goInitial".
@param init_pose_type:
0: default init pose (specified as _InitialPose)
1: factory init pose (specified as _InitialPose_Factory)
ROS Python インタフェースプログラミング¶
サンプルコード - Acceptance Test (ROS)¶
“RTM Python インタフェースプログラミング” の “サンプルコード - Acceptance Test (RTM)” におけるプログラムと同様のサンプルコードを見てみます． このプログラムは RTM クライアントで行ったことと同じタスクを ROS クライアントで行っています．
acceptancetest_ros.py¶
ROS の Python サンプルコード acceptancetest_ros.py は次のようになっています．
1 # -*- coding: utf-8 -*-
2
3 # Software License Agreement (BSD License)
4 #
5 # Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
6 # All rights reserved.
7 #
8 # Redistribution and use in source and binary forms, with or without
9 # modification, are permitted provided that the following conditions
10 # are met:
11 #
12 # * Redistributions of source code must retain the above copyright
13 # notice, this list of conditions and the following disclaimer.
14 # * Redistributions in binary form must reproduce the above
15 # copyright notice, this list of conditions and the following
16 # disclaimer in the documentation and/or other materials provided
17 # with the distribution.
18 # * Neither the name of TORK. nor the
19 # names of its contributors may be used to endorse or promote products
20 # derived from this software without specific prior written permission.
21 #
22 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34
35 import rospy
36
37 from hironx_ros_bridge.testutil.abst_acceptancetest import AbstAcceptanceTest
38
39
40 class AcceptanceTestROS(AbstAcceptanceTest):
41
42 def __init__(self, robot_client):
43 '''
44 @type robot_client: hironx_ros_bridge.ros_client.ROS_Client
45 '''
46 self._robotclient = robot_client
47
48 def go_initpos(self, default_task_duration=7.0):
49 self._robotclient.go_init(default_task_duration)
50
51 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None,
52 task_duration=7.0, do_wait=True):
53 '''
54 @see: AbstAcceptanceTest.move_armbyarm_impl
55 '''
56 rospy.loginfo("'''{}'''".format(msg_tasktitle))
57 self._robotclient.set_joint_angles_deg(
58 joint_group, joint_angles, task_duration, do_wait)
59
60 def set_pose(self, joint_group, posision, rpy, msg_tasktitle=None,
61 task_duration=7.0, do_wait=True, ref_frame_name=None):
62 '''
63 @see: AbstAcceptanceTest.set_pose
64 '''
65 rospy.loginfo('ROS {}'.format(msg_tasktitle))
66 self._robotclient.set_pose(joint_group, posision, rpy, task_duration,
67 do_wait, ref_frame_name)
68
69 def set_pose_relative(
70 self, joint_group, dx=0, dy=0, dz=0, dr=0, dp=0, dw=0,
71 msg_tasktitle=None, task_duration=7.0, do_wait=True):
72 rospy.logerr('AcceptanceTestROS; set_pose_relative is not implemented yet')
73 pass
最初に ROS_Client クラスのインスタンス化が必要ですが， それは上記のコードの外側，acceptancetest_hironx.py 内で次のように行われています．
self._robotclient = ROS_Client()
上記のコードに戻ります． 初期姿勢へ動くのは簡潔に1行で書かれています．
48 def go_initpos(self, default_task_duration=7.0):
49 self._robotclient.go_init(default_task_duration)
各関節を角度指示を与えるのも1行で書かれています．
51 def set_joint_angles(self, joint_group, joint_angles, msg_tasktitle=None, task_duration=7.0, do_wait=True):
:
57 self._robotclient.set_joint_angles_deg(joint_group, joint_angles, task_duration, do_wait)
直交座標系における姿勢指示も行うことができます． （このコマンドでは MoveIt! サービスを利用しています）
60 def set_pose(self, joint_group, posision, rpy, msg_tasktitle=None, task_duration=7.0, do_wait=True, ref_frame_name=None):
:
66 self._robotclient.set_pose(joint_group, posision, rpy, task_duration, do_wait, ref_frame_name)
サンプルコードの実行¶
iPython コンソールから ROS クライアントを利用した acceptance.run_tests_ros() を実行して
どのように動作しているかを確認してみます．
- 注意: ロボットが動きます．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/acceptancetest_hironx.py -- --host %HOSTNAME%
:
IN [1]: acceptance.run_tests_ros()
チュートリアル: MoveIt! GUI¶
MoveIt! - ROS の動作計画ソフトウェア¶
ROS の動作計画アプリケーションの代表的なソフトウェアパッケージが MoveIt! です． HIRO / NEXTAGE OPEN においても rtm_ros_bridge がありますので MoveIt! の動作計画機能を利用することができます．
MoveIt! GUI の操作¶
基本的な操作は “クイックスタート” の “MoveIt! での動作の実行” を参照してください．
左腕・両腕の動作計画への切り替え¶
HIRO / NEXTAGE OPEN において MoveIt! を起動したときは デフォルトで右手の InteractiveMarker が操作できる状態になっています． これを左手を操作して左腕の動作計画を作成するには，次の手順にて MoveIt! の設定を変更します．
- 子ウィンドウ Displays 内の MotionPlanning → Plannning Request → Planning Group で left_arm を選択
- 右手と同様に InteractiveMarker を操作して [ Plan and Execute ]
HIRO / NEXTAGE OPEN の MoveIt! では両腕での動作計画もできます．
MoveIt! の GUI: RViz 上では 次のように選択します．
- Displays 内の MotionPlanning → Plannning Request → Planning Group で botharms を選択
- 各腕の InteractiveMarker を操作して [ Plan and Execute ]
動作計画のループアニメーションと軌跡の表示¶
MoveIt! にて動作計画が作成されたときにはその動作アニメーションが1度表示されます． 動作計画をより確認しやすくするためにその動作アニメーションの繰り返し表示や その軌跡表示を行うことができます．
次の手順で MoveIt! の設定を変更します．
- 子ウィンドウ Displays 内の MotionPlanning → Planning Path → Loop Animation のチェックボックスを ON
- Loop Animation のすぐ下にある Show Trail のチェックボックスを ON
MoveIt! でのアニメーションと軌跡の表示の様子（画像はアニメーションの途中をキャプチャ）
動作計画上の各経過姿勢の確認¶
ループアニメーションでは確認しづらい動作計画上の任意の経過点の姿勢を確認するには MotionPlanning - Slider を使います．
- MoveIt! のメニュー Panels 内の MotionPlanning - Slider を選択
- 動作計画を作成した後にスライダを操作して経過点の姿勢を表示
実行動作の速度・加速度の調整¶
MotionPlanning 内 Planning タブにある Options の
- Velocity Scalling: 1.00
- Acceleration Scalling: 1.00
の値を 0〜1 の間で変更して動作の実行速度と加速度をそれぞれ動的に変更することができます．
逆運動学解のない姿勢での最善解の利用¶
デフォルトの設定では逆運動学解のない姿勢まで InteractiveMarker を動かすと エンドエフェクタ（手先など）は解の存在するところで止まってしまいます．
MotionPlanning 子ウィンドウの Context タブ内の Kinematics 下 Allow Approximate IK Solutions のチェックを入れると， 厳密に逆運動学解がない姿勢でもそれに近い最善の解が利用されます．
ジョイスティクの利用¶
HIRO / NEXTAGE OPEN にてジョイスティックを利用するためには moveit_ros_visualization パッケージがインストールされてる必要があります．
moveit_ros_visualization パッケージがインストールされていない場合には 下記手順によりインストールしてください．
$ sudo apt-get install ros-indigo-moveit-ros-visualization
OR
$ sudo apt-get install ros-indigo-moveit # This is simpler and better
ジョイスティックのプロセスを起動するには次のコマンドを実行してください．
$ roslaunch hironx_moveit_config joystick_control.launch
OR
$ roslaunch nextage_moveit_config joystick_control.launch
NEXTAGE OPEN での IKFast の利用¶
NEXTAGE_OPEN では IKFast パッケージを利用することができます． MoveIt! を実行するときに引数として運動学の設定を IKFast を利用するように指定します．
$ roslaunch nextage_moveit_config moveit_planning_execution.launch kinematics_conf:=`rospack find nextage_moveit_config`/config/kinematics_ikfast.yaml
MoveIt! GUI での障害物を回避する動作計画¶
MoveIt! の最大の特徴の1つは障害物を回避するような動作が自動生成されるところにあります．
障害物モデルデータを利用した MoveIt! の動作計画¶
環境が既知とした場合に 障害物の 3D モデルデータのファイルを MoveIt! に読み込んで動作計画を行う方法を紹介します．
障害物モデルのシーンファイルの読み込み¶
MoveIt! 内に次の手順により障害物モデルファイルの読込みを行います．
- MotionPlanning 内 Scene Objects タブで [ Import From Text ] ボタンをクリック
- ファイル nxo_pillar.scene を選択して [ OK ] をクリック
- 必要であれば障害物の InteractiveMarker を操作してロボットの手の近くに移動
- Context タブで [ Publish Current Scene ] をクリック → 障害物がシーンに反映
- 障害物モデルファイル
- /opt/ros/indigo/nextage_moveit_config/models/nxo_pillar.scene
- もしくは Catkin ワークスペース内 CATKIN_WORKSPACE/src/rtmros_nextage/nextage_moveit_config/models/nxo_pillar.scene
- ソースURL: https://github.com/tork-a/rtmros_nextage/blob/indigo-devel/nextage_moveit_config/models/nxo_pillar.scene
Import From Text
Publish Current Scene
シーンファイル nxo_pillar.scene の内容
nxo_pillar
* nxo_pillar
1
box
0.1 0.1 1.1
0.4 -0.2 -0.42
0 0 0 1
0 0 0 0
.
障害物を回避する動作計画とその実行¶
読み込んだ障害物モデルのある環境に応じた動作計画を行ってロボットを動かします．
障害物を回避する経路は自動的に生成されるので，動作計画を行う操作は障害物が無い場合と同じです． 次の操作で障害物を回避したモーションが自動生成されてロボットにその動作が出力されます．
- 注意: ロボットが動きます．
- Planning タブ Querry の Select Start State: < current > で [ Update ] ボタンをクリック
- Planning タブ Querry の Select Goal State: < current > で [ Update ] ボタンをクリック
- MoveIt! 上の InteractiveMarker でロボットの手を現在の位置から障害物の反対側にドラッグ
- Planning タブ Commands の [ Plan and Execute ] ボタンをクリック
障害物を回避する動作計画の例
障害物が無い場合の動作計画の例
STL / COLLADA(DAE) モデルファイルの利用¶
MoveIt! には STL や COLLADA(DAE) モデルのファイルを直接読み込む機能もあります．
MoveIt! 内に次の手順により障害物モデルファイルの読込みを行います．
- MotionPlanning 内 Scene Objects タブで [ Import File ] ボタンをクリック
- ファイル tsubo.dae もしくは tsubo.stl を選択して [ OK ] をクリック
- 必要であれば障害物の InteractiveMarker を操作してロボットの手の近くに移動
- Context タブで [ Publish Current Scene ] をクリック → 障害物がシーンに反映
- STL / COLLADA モデルファイル
- /opt/ros/indigo/nextage_moveit_config/models/
- もしくは Catkin ワークスペース内 CATKIN_WORKSPACE/src/rtmros_nextage/nextage_moveit_config/models/tsubo.*
- ソースURL: https://github.com/tork-a/rtmros_nextage/blob/indigo-devel/nextage_moveit_config/models/tubo.stl OR tsubo.dae
STL / COLLADA(DAE) モデルファイルを用いた場合も シーンファイルを読み込んだ場合と同じ手順で MoveIt! で障害物を回避する動作計画を作成できます．
Motion Planning - tsubo.dae
チュートリアル: MoveIt! Python インタフェース¶
MoveIt! Commander¶
MoveIt! の動作計画などの機能は GUI（RViz）からの操作を提供しているだけではありません． そのプログラミングインタフェースである MoveIt! Commander も提供されていますので， プログラミング言語から MoveIt! の機能を利用してロボットを動かすこともできます．
MoveIt! Python インタフェース使用環境¶
MoveIt! の Python インタフェース MoveIt! Python Commander を使用するときの環境は次のようになっています．
- rtm_ros_hironx: バージョン 1.1.4 以降（ Hironx/Nextage Open ）
- 推奨プログラミングインタフェース:
ROS_Client（ RobotCommander から派生 ） - RobotCommander は MoveIt! の Python プログラミングインターフェス
MoveIt! の Python インタフェースである moveit_commander パッケージが Ubuntu コンピュータ上にない場合はインストールする必要があります．
$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-moveit-commander
MoveIt! Python インタフェースからのロボット操作¶
MoveIt! Python インタフェースの起動¶
rtm_ros_bridge の実行¶
rtm_ros_bridge を実行します．
$ roslaunch hironx_ros_bridge hironx_ros_bridge_real.launch nameserver:=%HOSTNAME% (Hironx)
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME% (NEXTAGE OPEN)
MoveIt! サーバの起動¶
ROS_Client クラスをベースにしたプログラムを実行するためには
その “クライアント” に対する “サーバ” として MoveIt! のノードが走っている必要があります．
MoveIt! を起動します．
$ roslaunch hironx_moveit_config moveit_planning_execution.launch (HIRO)
$ roslaunch nextage_moveit_config moveit_planning_execution.launch (NEXTAGE OPEN)
Python での MoveIt! Commander を用いたロボット操作¶
Python から MoveIt! Commander を利用しているサンプルプログラムで， HIRO / NEXTAGE OPEN ロボットをどのように動作させているのかを見てみます．
サンプルコード全体を下に記載します．その後に各行について何をしているのかを見てみます．
1 #!/usr/bin/env python
2 ##########################################
3 # @file nextage_moveit_sample.py #
4 # @brief Nextage Move it demo program #
5 # @author Ryu Yamamoto #
6 # @date 2015/05/26 #
7 ##########################################
8 import moveit_commander
9 import rospy
10 import geometry_msgs.msg
11
12 def main():
13 rospy.init_node("moveit_command_sender")
14
15 robot = moveit_commander.RobotCommander()
16
17 print "=" * 10, " Robot Groups:"
18 print robot.get_group_names()
19
20 print "=" * 10, " Printing robot state"
21 print robot.get_current_state()
22 print "=" * 10
23
24 rarm = moveit_commander.MoveGroupCommander("right_arm")
25 larm = moveit_commander.MoveGroupCommander("left_arm")
26
27 print "=" * 15, " Right arm ", "=" * 15
28 print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame()
29 print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link()
30
31 print "=" * 15, " Left ight arm ", "=" * 15
32 print "=" * 10, " Reference frame: %s" % larm.get_planning_frame()
33 print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link()
34
35 #Right Arm Initial Pose
36 rarm_initial_pose = rarm.get_current_pose().pose
37 print "=" * 10, " Printing Right Hand initial pose: "
38 print rarm_initial_pose
39
40 #Light Arm Initial Pose
41 larm_initial_pose = larm.get_current_pose().pose
42 print "=" * 10, " Printing Left Hand initial pose: "
43 print larm_initial_pose
44
45 target_pose_r = geometry_msgs.msg.Pose()
46 target_pose_r.position.x = 0.325471850974-0.01
47 target_pose_r.position.y = -0.182271241593-0.3
48 target_pose_r.position.z = 0.0676272396419+0.3
49 target_pose_r.orientation.x = -0.000556712307053
50 target_pose_r.orientation.y = -0.706576742941
51 target_pose_r.orientation.z = -0.00102461782513
52 target_pose_r.orientation.w = 0.707635461636
53 rarm.set_pose_target(target_pose_r)
54
55 print "=" * 10," plan1 ..."
56 rarm.go()
57 rospy.sleep(1)
58
59 target_pose_l = [
60 target_pose_r.position.x,
61 -target_pose_r.position.y,
62 target_pose_r.position.z,
63 target_pose_r.orientation.x,
64 target_pose_r.orientation.y,
65 target_pose_r.orientation.z,
66 target_pose_r.orientation.w
67 ]
68 larm.set_pose_target(target_pose_l)
69
70 print "=" * 10," plan2 ..."
71 larm.go()
72 rospy.sleep(1)
73
74 #Clear pose
75 rarm.clear_pose_targets()
76
77 #Right Hand
78 target_pose_r.position.x = 0.221486843301
79 target_pose_r.position.y = -0.0746407547512
80 target_pose_r.position.z = 0.642545484602
81 target_pose_r.orientation.x = 0.0669013615474
82 target_pose_r.orientation.y = -0.993519060661
83 target_pose_r.orientation.z = 0.00834224628291
84 target_pose_r.orientation.w = 0.0915122442864
85 rarm.set_pose_target(target_pose_r)
86
87 print "=" * 10, " plan3..."
88 rarm.go()
89 rospy.sleep(1)
90
91 print "=" * 10,"Initial pose ..."
92 rarm.set_pose_target(rarm_initial_pose)
93 larm.set_pose_target(larm_initial_pose)
94 rarm.go()
95 larm.go()
96 rospy.sleep(2)
97
98 if __name__ == '__main__':
99 try:
100 main()
101 except rospy.ROSInterruptException:
102 pass
動作計画と実行を行う Python スクリプトの主要な部分は
- エンドエフェクタのリンクの位置と姿勢をターゲットとして指定
- ターゲットの姿勢まで動作させる
という手順になります． また，そのための準備としてエンドエフェクタの姿勢などを取得しています．
Python スクリプトの各行を具体的に見ていきます．
MoveIt! の Python インタフェース は
moveit_commander.MoveGroupCommander で提供されます．
import moveit_commander
import rospy
import geometry_msgs.msg
この Python スクリプトから MoveGroupCommander を使うために
rospy.init_node() を呼び出して ROS ノードを実行します．
rospy.init_node("moveit_command_sender")
RobotCommander をインスタンス化します．
robot = moveit_commander.RobotCommander()
ロボットの全ての Group の名前のリストを取得，表示します．
print "=" * 10, " Robot Groups:"
print robot.get_group_names()
ロボット全体の状態を表示するとデバッグに役立ちます．
print "=" * 10, " Printing robot state"
print robot.get_current_state()
現在のロボットの各腕の姿勢を取得します．
rarm_current_pose = rarm.get_current_pose().pose
larm_current_pose = larm.get_current_pose().pose
初期姿勢オブジェクトに現在のロボットの各腕の姿勢を代入します．
rarm_initial_pose = rarm.get_current_pose().pose
print "=" * 10, " Printing Right Hand initial pose: "
print rarm_initial_pose
larm_initial_pose = larm.get_current_pose().pose
print "=" * 10, " Printing Left Hand initial pose: "
print larm_initial_pose
動作計画を行いロボットを動作させます．
Execution Plan 1
target_pose_r = geometry_msgs.msg.Pose()
target_pose_r.position.x = 0.325471850974-0.01
target_pose_r.position.y = -0.182271241593-0.3
target_pose_r.position.z = 0.0676272396419+0.3
target_pose_r.orientation.x = -0.000556712307053
target_pose_r.orientation.y = -0.706576742941
target_pose_r.orientation.z = -0.00102461782513
target_pose_r.orientation.w = 0.707635461636
rarm.set_pose_target(target_pose_r)
print "=" * 10," plan1 ..."
rarm.go()
rospy.sleep(1)
Execution Plan 2
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
]
larm.set_pose_target(target_pose_l)
print "=" * 10," plan2 ..."
larm.go()
rospy.sleep(1)
Execution Plan 3
rarm.clear_pose_targets()
#Right Hand
target_pose_r.position.x = 0.221486843301
target_pose_r.position.y = -0.0746407547512
target_pose_r.position.z = 0.642545484602
target_pose_r.orientation.x = 0.0669013615474
target_pose_r.orientation.y = -0.993519060661
target_pose_r.orientation.z = 0.00834224628291
target_pose_r.orientation.w = 0.0915122442864
rarm.set_pose_target(target_pose_r)
print "=" * 10, " plan3..."
rarm.go()
rospy.sleep(1)
Go Initial
print "=" * 10,"Initial pose ..."
rarm.set_pose_target(rarm_initial_pose)
larm.set_pose_target(larm_initial_pose)
rarm.go()
larm.go()
rospy.sleep(2)
動画: “Nextage Move it! demo” https://www.youtube.com/watch?v=heKEKg3I7cQ
また，MoveIt! を表示している RViz 上でもこれらの動作計画が表示されます．
実際にサンプルプログラムを実行して， HIRO / NEXTAGE OPEN ロボットが動作計画どおりに動いているかを見てみます．
- 注意: ロボットが動きます．
ターミナルから次のコマンドを実行してサンプルプログラムを実行します．
$ rosrun nextage_ros_bridge nextage_moveit_sample.py (NEXTAGE OPEN)
MoveIt! Commander では更に様々なメソッドがありますので ROS Wiki の moveit_commander のページを見てください．
- ROS Wiki - moveit_commander
MoveIt! Python インタフェースでの両腕の動作計画¶
MoveIt! Python インタフェースでの両腕の動作計画を利用したサンプルコードを下記に記します．
両腕での動作計画サンプルコード
import moveit_commander
import rospy
from geometry_msgs.msg import Pose
def main():
rospy.init_node("moveit_command_sender")
botharms = moveit_commander.MoveGroupCommander("botharms")
target_pose_r = Pose()
target_pose_l = Pose()
q = quaternion_from_euler(0, -math.pi/2,0)
target_pose_r.position.x = 0.3
target_pose_r.position.y = 0.1
target_pose_r.position.z = 0.0
target_pose_r.orientation.x = q[0]
target_pose_r.orientation.y = q[1]
target_pose_r.orientation.z = q[2]
target_pose_r.orientation.w = q[3]
target_pose_l.position.x = 0.3
target_pose_l.position.y =-0.1
target_pose_l.position.z = 0.3
target_pose_l.orientation.x = q[0]
target_pose_l.orientation.y = q[1]
target_pose_l.orientation.z = q[2]
target_pose_l.orientation.w = q[3]
botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
botharms.go()
rospy.sleep(1)
target_pose_r.position.x = 0.3
target_pose_r.position.y =-0.2
target_pose_r.position.z = 0.0
target_pose_l.position.x = 0.3
target_pose_l.position.y = 0.2
target_pose_l.position.z = 0.0
botharms.set_pose_target(target_pose_r, 'RARM_JOINT5_Link')
botharms.set_pose_target(target_pose_l, 'LARM_JOINT5_Link')
botharms.go()
その他の ROS_Client クラス利用法¶
もう少し詳しく ROS_Client クラスについて見てみます．
ここでは iPython コンソールを用います．
$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py (simulation)
$ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py -- --host %HOSTNAME% (for real robot)
クライアントクラスの ros がインスタンス化されていることを確認します．
In [1]: ros
Out[1]: <hironx_ros_bridge.ros_client.ROS_Client at 0x7f0d34a00590>
初期姿勢にします．
In [2]: ros.go_init()
[INFO] [WallTime: 1453005565.730952] [1923.265000] *** go_init begins ***
[INFO] [WallTime: 1453005572.749935] [1930.345000] wait_for_result for the joint group rarm = True
[INFO] [WallTime: 1453005572.750268] [1930.345000] [positions: [0.010471975511965976, 0.0, -1.7453292519943295, -0.26529004630313807, 0.16406094968746698, -0.05585053606381855]
velocities: []
accelerations: []
effort: []
time_from_start:
secs: 7
nsecs: 0]
関節角度指令コマンド¶
ロボットの腕関節角度（ラジアン）の目標値指示による動作をさせるコマンドです．
In [3]: ros.set_joint_angles_rad('larm', [0.010, 0.0, -2.094, -0.265, 0.164, 0.06], duration=2, wait=True)
[INFO] [WallTime: 1453005602.774010] [1960.440000] wait_for_result for the joint group larm = True
In [4]: ros.set_joint_angles_rad('larm', [0.010, 0.0, -1.745, -0.265, 0.164, 0.06], duration=2, wait=True)
[INFO] [WallTime: 1453005606.789887] [1964.500000] wait_for_result for the joint group larm = True
set_joint_angles_degにより “度/degree” 指示も可能です．set_joint_angles_*メソッドは目標値の設定だけでなく，動作の実行も行います．
エンドエフェクタ姿勢指令コマンド¶
ロボットのエンドエフェクタ（EEF）姿勢の目標値指示による動作をさせるコマンドです．
IN [4]: ros.set_pose?
Type: instancemethod
Definition: ros.set_pose(self, joint_group, position, rpy=None, task_duration=7.0, do_wait=True, ref_frame_name=None)
Docstring:
@deprecated: Use set_pose_target (from MoveGroupCommander) directly.
Accept pose defined by position and RPY in Cartesian format.
@type joint_group: str
@type position: [float]
@param position: x, y, z.
@type rpy: [float]
@param rpy: If None, keep the current orientation by using
MoveGroupCommander.set_position_target. See:
'http://moveit.ros.org/doxygen/' +
'classmoveit__commander_1_1move__group_1_1' +
'MoveGroupCommander.html#acfe2220fd85eeb0a971c51353e437753'
@param ref_frame_name: reference frame for target pose, i.e. "LARM_JOINT5_Link".
In [5]: ros.set_pose('larm', [0.3256221413929748, 0.18216922581330303, 0.16756590383473382], [-2.784279171494696, -1.5690964385875825, 2.899351051232168], task_duration=7, do_wait=True)
[INFO] [WallTime: 1453007509.751512] [3869.365000] setpose jntgr=larm mvgr=<moveit_commander.move_group.MoveGroupCommander object at 0x7f21aaa8a950> pose=[0.3256221413929748, 0.18216922581330303, 0.16756590383473383, -2.784279171494696, -1.5690964385875825, 2.899351051232168] posi=[0.3256221413929748, 0.18216922581330303, 0.16756590383473383] rpy=[-2.784279171494696, -1.5690964385875825, 2.899351051232168]
In [6]: ros.set_pose('larm', [0.3256221413929748, 0.18216922581330303, 0.06756590383473382], [-2.784279171494696, -1.5690964385875825, 2.899351051232168], task_duration=7, do_wait=True)
[INFO] [WallTime: 1453007518.445929] [3878.220000] setpose jntgr=larm mvgr=<moveit_commander.move_group.MoveGroupCommander object at 0x7f21aaa8a950> pose=[0.3256221413929748, 0.18216922581330303, 0.06756590383473382, -2.784279171494696, -1.5690964385875825, 2.899351051232168] posi=[0.3256221413929748, 0.18216922581330303, 0.06756590383473382] rpy=[-2.784279171494696, -1.5690964385875825, 2.899351051232168]
tf の利用¶
エンドエフェクタ（EEF）の任意のフレームに対する相対姿勢を取得するために
ROS の tf とそのリスナークライアントの TransformListener という強力なライブラリを使います．
次のコードは左手のリンクフレーム /LARM_JOINT5_Link の腰フレーム /WAIST に対する相対姿勢を取得する方法です．
import tf
import rospy
listener = tf.TransformListener()
try:
(trans, rot) = listener.lookupTransform('/WAIST', '/LARM_JOINT5_Link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
取得された姿勢をロボットに送ります．
In [7]: pos = list(trans)
In [8]: rpy = list(tf.transformations.euler_from_quaternion(rot))
In [9]: ros.set_pose('larm', pos, rpy, task_duration=7, do_wait=True)
[INFO] [WallTime: 1453006377.256805] [2735.650000] setpose jntgr=larm mvgr=<moveit_commander.move_group.MoveGroupCommander object at 0x7f0d1d31b950> pose=[0.3255715961317417, 0.18217283734133255, 0.06760844121713444, 2.8644073530085747, -1.569564170540247, -2.7497620461224517] posi=[0.3255715961317417, 0.18217283734133255, 0.06760844121713444] rpy=[2.8644073530085747, -1.569564170540247, -2.7497620461224517]
- 注意
lookupTransform戻り値はタプル型なのに対してset_poseはリスト型を受け取る- 姿勢を
set_poseメソッドに送る前に “タプル→リスト変換” を忘れないようにする．
- 姿勢を
tf.transformation にクォータニオンからオイラー角への変換を行うツールがあります．
NumPy ライブラリを利用することで位置や姿勢のベクトルを変更することができます．
In [7]: ros.set_pose('larm', list(numpy.array(pos) + numpy.array([0,0,0.1])), rpy, task_duration=7, do_wait=True)
[INFO] [WallTime: 1453006471.587639] [2830.075000] setpose jntgr=larm mvgr=<moveit_commander.move_group.MoveGroupCommander object at 0x7f0d1d31b950> pose=[0.32557159613174169, 0.18217283734133255, 0.16760844121713445, 2.8644073530085747, -1.569564170540247, -2.7497620461224517] posi=[0.32557159613174169, 0.18217283734133255, 0.16760844121713445] rpy=[2.8644073530085747, -1.569564170540247, -2.7497620461224517]
- メモ
- 認識プロセスによって検出されたオブジェクト姿勢からエンドエフェクタの目標姿勢を算出するために
lookupTransformを利用することもできます．- 例）ar_track_alvar などを用いて AR マーカオブジェクトのカメラからの相対位置を取得した場合
- ar_track_alvar: http://wiki.ros.org/ar_track_alvar
- 例）ar_track_alvar などを用いて AR マーカオブジェクトのカメラからの相対位置を取得した場合
- 上記のコマンドでは HIRO / NEXTAGE OPEN のデフォルトのベースフレームである
/WAISTフレームに対する相対姿勢を見ているので，必然的に Move Group のget_current_poseの結果と同じになります．- 使用目的に応じて参照フレームを変更してください．
- 認識プロセスによって検出されたオブジェクト姿勢からエンドエフェクタの目標姿勢を算出するために
チュートリアル: 3次元認識¶
ロボット搭載カメラの設定¶
NEXTAGE OPEN のカメラ¶
NEXTAGE OPEN（HIRO ではない）はデフォルトで頭部に2つ，左右ハンドに1つずつで 合計4つの IDS Ueye カメラが搭載されています． それらのカメラの操作は ueye_cam パッケージにて行われ，基本的にそれは
- nextage_ros_bridge/launch/nextage_ueye_stereo.launch
- nextage_ros_bridge/launch/hands_ueye.launch
内にてまとめられています．
- 注意: HIRO のカメラについて
- 本項で記述されている内容は HIRO に搭載されているカメラに関するものではありません．
- HIRO のカメラを動作させる方法は本項で説明されている内容と異なる可能性があります．
ユーザ独自のカメラの利用について¶
デフォルトで搭載されている以外のカメラも ROS のドライバがあるものについては使用することができます． 既に ROS のドライバが存在しているカメラの情報は次のリンク先で知ることができます．
- Sensors/Cameras - ROS Wiki
- http://wiki.ros.org/Sensors/Cameras
- 注意: 手作業で更新されているので全ての情報を網羅するものではありません．
ユーザによっては Kinect や Xtion カメラをロボット頭部に搭載して利用したりしています． そのような場合には tf の確認や MoveIt! で衝突判定を行うのであれば カメラの 3D モデルなどを構築してロボットモデルを更新する必要があります．
ヘッドカメラの設定¶
Ueye カメラノードの実行¶
まずロボット頭部のカメラが頭部に設置されていることを確認してください． ステレオカメラのノードを実行します．
$ roslaunch nextage_ros_bridge nextage_ueye_stereo.launch
このノードでは ueye_cam パッケージを利用していますので 次のように映像トピックをサブスクライブ（購読）することができます．
$ rostopic echo left/image_raw
$ rostopic echo right/image_raw
映像は rqt_image_view などの GUI ツールを使用して見ることができます．
ステレオカメラのキャリブレーション¶
ステレオカメラを利用する前にはキャリブレーション作業が必要です．
- How to Calibrate a Stereo Camera - ROS Wiki
次のコマンドでステレオカメラのキャリブレーションを行います．
$ rosrun camera_calibration cameracalibrator.py --size 4x6 --square 0.024 --approximate=0.1 --camera_name=stereo_frame right:=/right/image_raw left:=/left/image_raw right_camera:=/right left_camera:=/left
キャリブレーション結果のデータは
ハンドカメラの設定¶
次のコマンドにより左右ハンドに搭載されている合計2つのカメラを動作させます．
$ roslaunch nextage_ros_bridge hands_ueye.launch
ハンドカメラの映像は次のトピックにより購読できます．
$ rostopic echo /left_hand_ueye/image_raw (left)
$ rostopic echo /right_hand_ueye/image_raw (right)
AR マーカを使ったオブジェクト認識と動作計画¶
AR マーカをロボットのカメラ画像から認識します．
AR マーカ認識プログラムの実行¶
新しいターミナルで AR マーカの認識プログラムを起動します． ar.launch では NEXTAGE OPEN ロボットの右手 Ueye カメラの画像から AR マーカの検出をします．
$ roslaunch nextage_ros_bridge ar.launch marker_size:=4.5
ar.launch 内で認識させる AR マーカの大きさなどが書かれています．
<launch> <arg name="marker_size" default="5.5" /> <arg name="max_new_marker_error" default="0.08" /> <arg name="max_track_error" default="0.2" /> <arg name="cam_ns" default="right_hand_ueye" /> <arg name="cam_image_topic" default="/$(arg cam_ns)/image_raw" /> <arg name="cam_info_topic" default="/$(arg cam_ns)/camera_info" /> <arg name="output_frame" default="/camera" /> <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen" args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" /> </launch>AR マーカのサイズは次に示すところに単位センチメートルで記述されています．
<arg name="marker_size" default="5.5" />デフォルトの 5.5 [cm] 以外のサイズの AR マーカを認識させる場合には 次のように ar.launch 実行時に引数でサイズを指定して実行します．
$ roslaunch nextage_ros_bridge ar.launch marker_size:=4.5
- メモ: AR マーカのサイズ = マーカの黒い部分全体の辺の長さ
MoveIt! と AR マーカの表示¶
MoveIt! を起動してください．
$ roslaunch nextage_moveit_config moveit_planning_execution.launch
MoveIt! でロボットのカメラ映像を表示させます．
- [ Add ] ボタンをクリック
- “rviz” → “Image” をクリックして選択 → “Image” の子ウィンドウが表示
- Displays → Image → Image Topic の項目で /right_hand_ueye/image_raw を選択
AR マーカが印刷された紙などを右手のカメラに映るように設置してください． 必要であればロボットの腕も動かしてください． ここでは AR 番号が ar_marker_4 でサイズが 4.5 cm としています．
次にマーカの処理結果を表示させます．
- [ Add ] ボタンをクリック
- ”rviz” → ”Marker” をクリック
- [ Add ] ボタンをクリック
- ”rviz” → ”TF” をクリック
- Displays → TF → Frames → All Enabled のチェックを外す
- Displays → TF → Frames → /ar_marker_4 のチェックを入れる
AR マーカの各番号に対応した色の四角い立体とその tf が MoveIt! / RViz 上で表示されます．
MoveIt! / RViz の設定を保存しておくと上記の追加作業が不要になるので便利です． MoveIt! / RViz の File メニューから Save Config As を選択して， 例えばファイル名 moveit-ar.rviz などの名前で設定を保存します．
MoveIt! / RViz で保存してある設定を利用するには File メニューの Open Config から設定ファイルを読み込みます．
AR マーカ認識に基づく動作計画と実行¶
デモンストレーションプログラム ar_demo.py は AR マーカを認識して得られた座標を基に MoveIt Commander を利用して動作計画・指示・実行などを行います．
ar_demo.py のコードは次のようになっています．
#!/usr/bin/env python
import rospy
import tf
import geometry_msgs.msg
import sys
from moveit_commander import MoveGroupCommander
from tf.transformations import *
import math
def kbhit():
import select
return sys.stdin in select.select([sys.stdin], [], [], 0)[0]
if __name__ == '__main__':
rospy.init_node('ar_pose_commander', anonymous=True)
group = MoveGroupCommander("right_arm")
base_frame_id = '/WAIST'
ar_marker_id = '/ar_marker_4'
pub = rospy.Publisher('target_pose', geometry_msgs.msg.PoseStamped)
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
pose_st_target = None
while not rospy.is_shutdown() and not kbhit():
try:
now = rospy.Time(0)
(trans,quat) = listener.lookupTransform(base_frame_id, ar_marker_id, now)
quat = quaternion_multiply(quat, quaternion_about_axis(math.pi/2, (1,0,0)))
quat = quaternion_multiply(quat, quaternion_about_axis(math.pi/2, (0,0,1)))
pose_st_target = geometry_msgs.msg.PoseStamped()
pose_st_target.pose.position.x = trans[0]
pose_st_target.pose.position.y = trans[1]
pose_st_target.pose.position.z = trans[2]
pose_st_target.pose.orientation.x = quat[0]
pose_st_target.pose.orientation.y = quat[1]
pose_st_target.pose.orientation.z = quat[2]
pose_st_target.pose.orientation.w = quat[3]
pose_st_target.header.frame_id = base_frame_id
pose_st_target.header.stamp = now
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn(e)
if pose_st_target:
pub.publish(pose_st_target)
rospy.loginfo(trans)
rate.sleep()
if raw_input() == 'q':
sys.exit(1)
# move to a point above ar marker
pose_st_target.pose.position.z += 0.3
rospy.loginfo("set target to {}".format(pose_st_target.pose))
group.set_pose_target(pose_st_target.pose)
plan = group.plan()
rospy.loginfo("plan is {}".format(plan))
ret = group.go()
rospy.loginfo("executed ... {}".format(ret))
デモンストレーションコードの中から主要な行について解説します．
右手を動かします．そのために "right_arm" を引数として渡して
MoveGroupCommander をインスタンス化します．
group = MoveGroupCommander("right_arm")
基準となるフレームを /WAIST に
ターゲットとする AR マーカのフレームを /ar_maker_4 として変数に格納します．
AR マーカのターゲットを変えたい場合はここを変更すれば良いことになります．
base_frame_id = '/WAIST'
ar_marker_id = '/ar_marker_4'
サンプリング周波数 10 Hz を rate = rospy.Rate(10.0) で指定して
基準フレームとターゲットの AR マーカのフレーム間の TF を取得します．
ターゲット取得ループは Enter キーが押されると抜けるようにしています．
listener = tf.TransformListener()
rate = rospy.Rate(10.0)
pose_st_target = None
while not rospy.is_shutdown() and not kbhit():
try:
now = rospy.Time(0)
(trans,quat) = listener.lookupTransform(base_frame_id, ar_marker_id, now)
TF で取得された AR マーカフレームの姿勢から右腕エンドエフェクタの目標姿勢になるように クォータニオンの計算をします．
quat = quaternion_multiply(quat, quaternion_about_axis(math.pi/2, (1,0,0)))
quat = quaternion_multiply(quat, quaternion_about_axis(math.pi/2, (0,0,1)))
TF で取得した AR マーカフレームの位置と計算されたターゲットの姿勢を
pose_st_target に格納します．
pose_st_target = geometry_msgs.msg.PoseStamped()
pose_st_target.pose.position.x = trans[0]
pose_st_target.pose.position.y = trans[1]
pose_st_target.pose.position.z = trans[2]
pose_st_target.pose.orientation.x = quat[0]
pose_st_target.pose.orientation.y = quat[1]
pose_st_target.pose.orientation.z = quat[2]
pose_st_target.pose.orientation.w = quat[3]
Enter キーが押されるとターゲット取得ループから抜けて
右腕のエンドエフェクタを AR マーカ /ar_marker_4 の真上に右手を動かします．
まず，エンドエフェクタをターゲットの 0.3 [m] 上方に動くように位置のZ座標を再計算してから， 右腕の Group に対して目標姿勢として設定します．
pose_st_target.pose.position.z += 0.3
group.set_pose_target(pose_st_target.pose)
次に右腕の動作計画を行い，その結果のログを表示します．
plan = group.plan()
rospy.loginfo("plan is {}".format(plan))
そして動作計画を実行して，その結果のログを表示します．
ret = group.go()
rospy.loginfo("executed ... {}".format(ret))
それでは実際に ar_demo.py を実行してみます．
$ rosrun nextage_ros_bridge ar_demo.py
コンソールに黄色い文字でワーニングが表示されている場合は座標の変換が完遂できていない場合です． 白い文字で座標変換ができている旨のメッセージが出ているときに Enter キーを押すことにより AR 認識で得られた座標データから右手の目標姿勢を定めて動作計画を行い AR マーカの上まで移動させます．
Gazebo での AR マーカ認識シミュレーション¶
Gazebo を用いて仮想空間内の AR マーカを認識するシミュレーションを行います． ここでは頭部カメラの画像を AR マーカ認識に使用します．
Gazebo の起動とモデルの読み込み¶
NEXTAGE OPEN の Gazebo シミュレーションを起動します．
- メモ: ROS / Gazebo でのシミュレーションのため rtm_ros_bridge は不要
$ roslaunch nextage_gazebo nextage_world.launch
Gazebo の仮想空間内に次の2つのモデルを読み込みます．
- Cafe table
- MarkerBox-60mm
“cafe_table” モデルを Gazebo のライブラリから読み込みます．
- 左コラムの Insert タブを選択
- “http://gazeboism.org/models/” を選択して “Cafe table” をクリック
- ポインタに Cafe table がついてくるのでロボットの前の床でクリックして設置
- 上の方にある十字の矢印アイコンボタンをクリック後 Cafe table をクリック
- 赤緑青の座標軸をドラッグして Cafe table の位置を調整
- メモ: Gazebo 起動直後は “http://gazeboism.org/models” が表示されずに
“Connecting to model database...” になっているかもしれません．
しばらくするとデータベースにつながってモデルが読めるようになります．
“MarkerBox-60mm” モデルは Gazebo のライブラリに登録されていないので
ユーザコンピュータの ~/.gazebo/model/ ディレクトリにモデルフォルダをコピーします．
ターミナルなどを使用してコピーしてください．
$ cp -r MarkerBox-60mm/ ~/.gazebo/model/
- AR マーカモデルフォルダ
- /opt/ros/indigo/nextage_gazebo/models/MarkerBox-60mm
- もしくは Catkin ワークスペース内 CATKIN_WORKSPACE/src/rtmros_nextage/nextage_gazebo/models/MarkerBox-60mm
- ソースURL: https://github.com/tork-a/rtmros_nextage/blob/indigo-devel/nextage_gazebo/models/MarkerBox-60mm
コピーしたモデル MarkerBox-60mm を Gazebo に読み込みます．
- 左コラムの Insert タブを選択
- “/home/USERNAME/.gazebo/models” を選択して “MarkerBox-60mm” をクリック
- ポインタを動かしてモデルを Cafe table の下の床の上でクリックして設置
- 上の方にある十字の矢印アイコンボタンをクリックモデルをクリック
- 赤緑青の座標軸をドラッグして Cafe table 上で， ロボット頭部カメラを下に向けたときに写りそうな位置に調整
カメラの画角に AR マーカが入るように頭部姿勢を動かします． ここでは rqt の Joint Trajectory Controller プラグインを利用します．
$ rqt
Plugins の Robot Tools 内 Joint trajectory controller を選択します．
Joint trajectory controller 内で下記のように設定します．
- controller manager ns : /controller_manager
- controller : head_controller
Enable/disable ボタンをクリックして緑色になると頭部の各関節を動かせる状態になります． joints 下の各スライダを動かして頭部の姿勢を調整してください．
AR プロセスと MoveIt! の起動¶
新しいターミナルで AR マーカの認識プログラムを起動します．
$ roslaunch nextage_ros_bridge ar_headcamera.launch sim:=true
ar_headcamera.launch
<launch>
<arg name="marker_size" default="4.5" />
<arg name="max_new_marker_error" default="0.08" />
<arg name="max_track_error" default="0.2" />
<arg name="cam_ns" default="CAMERA_HEAD_L" />
<arg name="cam_image_topic" default="/$(arg cam_ns)/image_raw" />
<arg name="cam_info_topic" default="/$(arg cam_ns)/camera_info" />
<arg name="output_frame" default="/CAMERA_HEAD_L_Link" />
<arg name="sim" default="false" />
<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect"
respawn="false" output="screen"
args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
<group if="$(arg sim)">
<node pkg="tf" type="static_transform_publisher" name="camera_head_l_link_to_frame"
args="0 0 0 1.57080 3.14159 1.57080 /CAMERA_HEAD_L_Link /CAMERA_HEAD_L_Link_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_head_r_link_to_frame"
args="0 0 0 1.57080 3.14159 1.57080 /CAMERA_HEAD_R_Link /CAMERA_HEAD_R_Link_frame 100" />
</group>
</launch>
あとはロボット実機で行った場合と同様に MoveIt! と ar_demo.py を実行します．
- MoveIt! を起動 + （オプション）RViz 設定ファイルの読み込み
- ar_demo.py の実行
を行います．
MoveIt! - ar_demo.py 実行前
MoveIt! - ar_demo.py 実行後
Gazebo - ar_demo.py 実行後
Xtion/Kinect カメラの TF キャリブレーション¶
3次元深度カメラの Xtion や Kinect を利用してロボット環境情報を取得する場合において， 得られた環境空間情報とロボットとの相対位置・姿勢が分からなければ意味がありません．
そのためにはカメラがロボットに対してどのような位置・姿勢で設置されているかを知る必要があります． 具体的にはカメラを設置するリンクもしくはワールド空間に対するカメラの TF を取得します．
ロボット実機での Xtion/Kinect TF キャリブレーション¶
ロボットの準備¶
ROS を使用するので rtm_ros_bridge が起動している必要があります．
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME%
ロボット各関節のキャリブレーションを行います． 関節のキャリブレーションが終わったら一旦ロボットの姿勢を終了姿勢にしてサーボを切ります．
ここでは操作に iPython コンソールを使用しています．
$ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py -- --host nextage
:
(many connection messages)
:
[hrpsys.py] initialized successfully
:
In [1]: robot.checkEncoders()
In [2]: robot.goOffPose()
In [3]: robot.servoOff()
チェッカーボードのロボットへの設置¶
カメラとロボットの相対位置・姿勢を定めるためにチェッカーボードをロボットに取り付けます． このときチェッカーボードと設置するロボットのリンクとの相対位置は機械設計上既知である必要があります．
- 注意: ロボット実機への作業ですのでサーボが切れていることを確認してください．
下図のように胸部チェッカーボードもしくは腰部チェッカーボードをロボットに取り付けてください．
胸部チェッカーボード
腰部チェッカーボード
サーボを入れる（胸部チェッカーボードの場合）¶
胸部チェッカーボードを使用する場合は CHEST_JOINT0 の関節角度を 0 deg（正面）で維持する必要がありますので，ロボットのサーボを入れて初期姿勢にします．
In [1]: robot.servoOn()
In [2]: robot.goInitial()
Xtion/Kinect カメラをチェッカーボードに向ける¶
Xtion/Kinect カメラにチェッカーボードが映るようにします． 頭部に Xtion/Kinect カメラを設置した場合には関節を制御して頭部姿勢を変えます．
下に頭部関節を動作・制御するコマンドを記します．
In [1]: robot.setJointAnglesOfGroup( 'head', [ 0.0, 45.0 ], 5.0 )
In [2]: robot.getJointAngles()
チェッカーボード検出プログラムの実行¶
Xtion/Kinect カメラを本作業を行っている UIコントロールボックス（Ubuntu PC/Vision PC） もしくは 開発用コンピュータ（Ubuntu）に接続してください．
チェッカーボードを検出プログラムを実行します．
Kinect カメラを使用している場合は実行時に引数 launch_openni2:=false を指定してください．
Xtion カメラの場合
$ roslaunch nextage_calibration camera_checkerboard_chest.launch #chest
or
$ roslaunch nextage_calibration camera_checkerboard_waist.launch #waist
Kinect カメラの場合
$ roslaunch nextage_calibration camera_checkerboard_chest.launch launch_openni2:=false #chest
or
$ roslaunch nextage_calibration camera_checkerboard_waist.launch launch_openni2:=false #waist
Checkerboard Detector - 胸部チェッカーボード
Checkerboard Detector - 腰部チェッカーボード
RViz の起動¶
各機能の実行状況を確認するために RViz を起動します．
$ rviz -d `rospack find nextage_calibration`/conf/calibration.rviz
RViz - 胸部チェッカーボード
RViz - 腰部チェッカーボード
カメラリンクと親リンクの TF の取得¶
tf_echo コマンドを利用して camera_link から親リンクへの TF を取得します．
ここでは camera_link から HEAD_JOINT1 への TF を表示します．
$ rosrun tf tf_echo HEAD_JOINT1_Link camera_link
tf_echo の結果の例
At time 93.641
- Translation: [0.101, -0.014, 0.199]
- Rotation: in Quaternion [-0.001, 0.704, 0.002, 0.710]
in RPY (radian) [0.126, 1.563, 0.130]
in RPY (degree) [7.223, 89.561, 7.477]
ロボットでの作業の終了¶
カメラリンクと親リンクの TF の取得ができたらロボットを終了姿勢にしてサーボを切ります．
In [1]: robot.goOffPose()
In [2]: robot.servoOff()
全ノードの終了¶
ロボットのサーボが切れたら全てのノードを Ctrl-C で終了します．
カメラリンク TF の適用¶
取得した Xtion/Kinect カメラ TF をロボット上のカメラのパラメータとして適用します． 適用は Xtion/Kinect カメラモジュール xtion_kinect.launch 起動時に引数として渡して行います．
- 単位系
- TF_COORD_XYZ : [m]
- TF_COORD_RPY : [rad]
まず rtm_ros_bridge を起動します．
$ roslaunch nextage_ros_bridge nextage_ros_bridge_real.launch nameserver:=%HOSTNAME%
Xtion カメラの場合
$ roslaunch nextage_calibration xtion_kinect.launch TF_COORD_XYZ:='0.101 -0.014 0.199' TF_COORD_RPY:='0.126 1.563 0.130'
Kinect カメラの場合（launch_openni2:=false のみの違い）
$ roslaunch nextage_calibration xtion_kinect.launch TF_COORD_XYZ:='0.101 -0.014 0.199' TF_COORD_RPY:='0.126 1.563 0.130' launch_openni2:=false
Xtion カメラの画像キャリブレーション（オプション）¶
Xtion カメラにおいては画像のキャリブレーション情報を取得しておくことをお勧めします． 画像キャリブレーションの作業手順は下記のサイトを参照ください．
- ROS Wiki - How to Calibrate a Monocular Camera
Gazebo を用いた Xtion/Kinect TF 取得シミュレーション¶
実機で行う Xtion/Kinect カメラの TF キャリブレーションの作業は Gazebo シミュレータ上でも同様のことを行うことができます．
Gazebo の起動¶
NEXTAGE OPEN ロボットモデルの Gazebo シミュレータを起動します．
$ roslaunch nextage_gazebo nextage_world.launch
- メモ: ROS / Gazebo でのシミュレーションのため rtm_ros_bridge は不要
Gazebo 内での Kinect とチェッカーボードモデルの設置¶
Gazebo 起動後の NEXTAGE OPEN ロボットの初期動作が終了しましたら， Kinect と チェッカーボード のモデルを Gazebo 内に設置します．
$ roslaunch nextage_calibration gazebo_kinect_checkerboard_chest.launch #chest
or
$ roslaunch nextage_calibration gazebo_kinect_checkerboard_waist.launch #waist
Gazebo シミュレータ内の Kinect と胸部チェッカーボードモデルの設置
チェッカーボード検出プログラムの実行¶
実機ロボットの場合と同様に Xtion/Kinect カメラでのチェッカーボード検出プログラムを実行します．
Kinect の場合
$ roslaunch nextage_calibration camera_checkerboard_chest.launch launch_openni2:=false
or
$ roslaunch nextage_calibration camera_checkerboard_waist.launch launch_openni2:=false
RViz の起動¶
実機ロボットの場合と同様に各種チェックのために RViz を起動します．
$ rviz -d `rospack find nextage_calibration`/conf/calibration.rviz
カメラリンクと親リンクの TF の取得¶
実機ロボットの場合と同様に camera_link から親リンクへの TF を取得します．
$ rosrun tf tf_echo HEAD_JOINT1_Link camera_link
tf_echo の結果の例
At time 93.641
- Translation: [0.101, -0.014, 0.199]
- Rotation: in Quaternion [-0.001, 0.704, 0.002, 0.710]
in RPY (radian) [0.126, 1.563, 0.130]
in RPY (degree) [7.223, 89.561, 7.477]