Choreonoid PA10位置制御パネルを作る
Choreonoid 1.4 では、シミュレーションのサンプルとして、PA10を使った積み木のハンドリングデモがあります。このデモを改良して、PA10を位置制御できるようにし、eSEATで操作パネルを作成してみましょう。
Choreonoidプラグインの改良
Choreonoid 1.4 では、OpenRTMを使ったロボット制御ができるようになっています。OpenRTMで作成した制御コンポーネントの例は、"OpenRTM-PA10Pickup.cnoid"というプロジェクトファイルに入っているのですが、これを実行すると単にPick and Placeを行うだけになっています。
このサンプルプロジェクトとプラグインを改良して、外部のコンポーネントから位置指令を受けて動作するようにしたいと思います。
このPick and Placeのプラグインは、https://github.com/s-nakaoka/choreonoid/tree/master/sample/OpenRTMの中にある
PA10PickupControllerRTC.cpp, PA10PickupControllerRTC.h, PA10Pickup.conf, Interpolator.hから構成されています。
このファイルを編集してもよいのですが、PA10Pickup -> PA10Pos に変更して新たなプラグインを作成してみましょう。
まず、PA10PickupControllerRTC.cpp, PA10PickupControllerRTC.h, PA10Pickup.confをそれぞれPA10PosControllerRTC.cpp, PA10PosControllerRTC.h, PA10Pos.confにコピーします。
そして、CMakeでこのプラグインもコンパイルできるように、CMakeLists.txt に下記を追加します。
# PA10 Pos
set(target PA10PosControllerRTC)
add_cnoid_sample_rtc(${target} PA10PosControllerRTC.cpp)
configure_file(OpenRTM-PA10Pos.cnoid ${CNOID_SOURCE_SHARE_DIR}/project COPYONLY)
configure_file(PA10Pos.conf ${PROJECT_BINARY_DIR}/${CNOID_PLUGIN_SUBDIR}/rtc/PA10Pos.conf COPYONLY)
install(FILES PA10Pos.conf DESTINATION ${CNOID_PLUGIN_SUBDIR}/rtc)
追加する場所は、PA10PickupControlleRTCの定義の下あたりでよいと思います。
編集する前に一応コンパイル可能かどうかを確かめておいた方がよいと思います。コンパイルするには、Choreonoidをソースコードからコンパイルできるようにしておいた方が良いと思います。
現在はすでに最新の環境でもコンパイル可能のようですが、現在公式HPにある1.4では少し古い環境が必要のようですが、コンパイル環境の構築について、こちらを参考にしてください。
入出力ポートの追加
Choreonoidに同梱されているサンプルでは、シミュレーションを開始するとコンポーネントを接続して、PA10が環境中の緑の箱型の物体をつかんで黄色の台の上に乗せるという動作をします。
ここで目標としているのは、外部から位置指令を与えるとPA10がその場所に動く、エンドエフェクタを開けたり閉じたりできるようにするということですので、位置とエンドエフェクタの開閉の指令を受け取るための入力ポートと現在の位置姿勢を外部に出力する出力ポートを追加します。
位置の指令は、手先の位置姿勢にするか、関節角の目標角度にするかにもよるのですが、どちらでも対応できるようにTimeDoubelSeq型にしておきます。本来は、OpenRTMの標準インターフェースを使って実装すべきなのですが、取り敢えず動かすところまでやりたいので、目をつぶっています。
エンドエフェクタの開閉については、コマンドでよいのでTimedStringにしておきます。
また、出力ポートに関しても、位置姿勢、関節角のどちらも扱えるようにTimedDoubleSeqにしておきます。
最終的には、下図のPA10PosControllerRTC0のようにしたいと思います。
コアロジックの実装
Choreonoidに添付されていたサンプルでは、PA10の動作として、手先の位置、姿勢を指定するようになっています。一方、Choreonoidでは、ロボットの動作は目標トルク値か目標関節角を与えれば動作するはずですので、逆運動学の計算自体は、このサンプルに入っているはずです。そこで、実装コードをよく見てみると逆運動学計算も時分割の目標関節角の計算も入っていますので、比較的簡単に目標としているプラグインは実現できそうです。
PA10PickupControllerRTC.cppを見てみると、初期姿勢設定とアプローチ点への移動設定部分は onActivateで行っています。この部分はほぼこのままでよさそうです。ただし、最初のアプローチ点への設定は余計ですので、この部分だけ削除すれば大丈夫そうです。したがって、ここで作りたい機能を実現するには、onInitializeとonExecuteを主に改変すればいいことがわかります。
onInitializeに関しては、データポートの初期化部分を追加するだけですので、通常のRTCの開発の時のように、2つの入力ポートと1つの出力ポートを作成します。あとは、onExecuteの部分なのですが、ここで実現したいコントローラの機能は、この部分で実装していきます。
PA10PickupControllerRTC.cppのonExecute関数の内部では、phaseという変数名で動作の切り替えを行っていることがわかります。このphase 変数をよく見てみると、phase の値が 1の時ハンドを閉じ、3の時は、ハンドを開ける動作をしています。また、phaseの値が3の時には、ハンドを開けた後に次の目標位置姿勢を指定している部分もありますので、このあたりのコードを組み合わせればよいと思います。
また、目標位置姿勢が設定されている時は、wristInterpolator で各時刻の目標位置姿勢を算出できるようですので、これも組み合わせる必要があることがわかります。
ということで、目標位置姿勢を入力する部分は下記のようなコーディングでよいと思います。
VectorXd p0(6);
VectorXd p1(6);
if(m_target_angleIn.isNew()){
m_target_angleIn.read();
if (m_target_angle.data.length() < 7) { break; }
p1.head<3>() = Vector3(m_target_angle.data[0],
m_target_angle.data[1], m_target_angle.data[2]);
p1.tail<3>() = toRadianVector3(m_target_angle.data[3],
m_target_angle.data[4], m_target_angle.data[5]);
wristInterpolator.clear();
wristInterpolator.appendSample(time, p0);
wristInterpolator.appendSample(time+m_target_angle.data[6], p1);
wristInterpolator.update();
}
また、Interpolatorによる時分割の位置姿勢目標は、
p0 = wristInterpolator.interpolate(time);
if(baseToWrist->calcInverseKinematics(Vector3(p0.head<3>()),
wrist->calcRfromAttitude(rotFromRpy(Vector3(p0.tail<3>()))))){
for(i=0; i < baseToWrist->numJoints(); ++i){
Link* joint = baseToWrist->joint(i);
qref[joint->jointId()] = joint->q();
}
}
となります。また、ハンドの開閉に関しては、
if(close_hand == 1){
if(fabs(m_torque_in.data[0]) < 40.0 || fabs(m_torque_in.data[1]) < 40.0){
dq_hand = std::min(dq_hand + 0.00001, 0.0005);
if(qref[rightHand_id] > 0) { qref[rightHand_id] -= radian(dq_hand); }
if(qref[leftHand_id] < 0) { qref[leftHand_id] += radian(dq_hand); }
}
}else{
if(qref[rightHand_id] < 0.028 || qref[leftHand_id] > -0.028){
dq_hand = std::min(dq_hand + 0.00001, 0.002);
qref[rightHand_id] += radian(dq_hand);
qref[leftHand_id] -= radian(dq_hand);
}
}
となると思います。ほぼこれで目標位置姿勢を入力すれば動作するコントローラができると思います。
このページに添付しているソースコードは、目標位置姿勢だけでなく、差分目標位置姿勢、目標関節角が設定できるように拡張しています。
作成したプラグインは、コンパイルし lib/chreonoid-1.4/rtc にインストールすれば使えるようになります。
eSEATによる入力パネルの作成
次に、作成したプラグイン PA10PosControllerRTCに目標位置姿勢、差分目標位置姿勢、目標関節角を入力できるようなGUIパネルをeSEATで実装していきます。
最終的なGUIパネルとしては、下図のように3つのモードで入力可能なGUIパネルとします。
eSEATでは、GUIのパネルは内部状態と対応づけられていますので、ここで作成するSEATMLは、Mode0, Mode1, Mode2 の内部状態を作成します。
また、この操作パネルは前述のプラグインと接続しますので、下記のデータポートを作成します。
| ポート名 | ポートの種類 | データ型 | |
| str_out | Out | TimedString | ハンドの開閉命令、モード以降命令 |
| pos_in | In | TimedDoubleSeq | 現在の位置姿勢の状態 |
| q_in | In | TimedDoubleSeq | 目標位置姿勢、差分目標位置姿勢、目標関節角の指令 |
| q_out | Out | TimedDoubleSeq | 現在の関節角の状態 |
ここまので仕様でSEATMLファイルは下記のようになります。
<?xml version="1.0" encoding="UTF-8"?>
<seatml>
<general name="PA10_Command">
<adaptor name="str_out" type="rtcout" datatype="TimedString" />
<adaptor name="pos_in" type="rtcin" datatype="TimedDoubleSeq" />
<adaptor name="q_in" type="rtcin" datatype="TimedDoubleSeq" />
<adaptor name="q_out" type="rtcout" datatype="TimedDoubleSeq" />
</general>
<state name="Mode0">
</state>
<state name="Mode1">
</state>
<state name="Mode2">
</state>
</seatml>
あとは各状態に対応するGUIパネルをlabel, button, input要素を使って実装していきます。
目標位置姿勢入力パネル(Mode0）
目標位置姿勢入力パネルは、このSEATMLではデフォルトの入力パネルとしていますので、Mode0の状態に作成していきます。必要とするアイテムは、目標位置姿勢の入力用のInput要素、ハンドの開閉命令用のButton要素、現在の位置姿勢、各関節角の状態表示用のInput要素、モード切替用のButton要素となると思います。あとは補助的な表示としてlabel要素を活用して実装してみましょう。
<state name="Mode0"> <label text="Input(Pos, Angle, Time):" /> <input id="lineIn" width="70" colspan="3">0.9, 0, 0.25, 180, 0, 0, 2.0</input> <button label="Send" > </button> <brk /> <label text="Hand:" /> <button label="Open" ></button> <button label="Close" ></button> <brk /> <label text="Pos, Aangle, cTime:" /> <input id="lineOut" width="70" colspan="4">0,0,0,0,0,0,1.0</input> <brk /> <label text="Joints:" /> <input id="JOut" width="70" colspan="4">0,0,0,0,0,0,1.0</input> <brk /> <label text="Mode:" /> <label text="Mode0" bg_color="blue" /> <button label="Mode1" > </button> <button label="Mode2" > </button> </state>
このソースコードでは下記のようがGUIパネルが生成されます。
input要素の幅などは適当に変更すればよいと思います。ただし、レイアウトマネージャーはgridが使われますので、colspan属性などを併用してレイアウトをしてください。
差分位置姿勢入力パネル(Mode1)
次に、差分位置姿勢入力パネルを作成します。このパネルの場合もMode0と同じでも問題なのですが、位置（x, y, z)、姿勢角(roll,pitch,yaw)をそれぞれ "+", "-"のボタンで制御できるようにしてみます。
これだけだと、固定長の増減しかできませんし、そもそも位置と姿勢では単位が違いますので、ここも指定できるようにinput要素を用いて作っていきます。
下のようなソースコードでいかがでしょうか。
<state name="Mode1"> <label text="Pos, Aangle, cTime:" /> <input id="lineOut1" width="70" colspan="6">0,0,0,0,0,0,1.0</input> <rule source="pos_in"></rule> <brk /> <label text="Pos:" /> <input id="valIn1" width="5">0.1</input> <label text="Ang:" /> <input id="angIn1" width="5">10.0</input> <label text="Time:" /> <input id="timeIn1" width="5">2.0</input> <brk /> <label text="Pos, Aangle:" /> <button label="+X" > </button> <button label="+Y" > </button> <button label="+Z" > </button> <button label="+r" > </button> <button label="+p" > </button> <button label="+y" > </button> <brk /> <label text="Pos, Aangle:" /> <button label="-X" > </button> <button label="-Y" > </button> <button label="-Z" > </button> <button label="-r" > </button> <button label="-p" > </button> <button label="-y" > </button> <brk /> <label text="Hand:" /> <button label="Open" ></button> <button label="Close" ></button> <brk /> <label text="Mode:" /> <button label="Mode0" > </button> <label text="Mode1" bg_color="blue" /> <button label="Mode2" > </button> </state>
このソースコードでGUIパネルを生成すると下図のようなGUIパネルが得られます。
目標関節角度入力パネル(Mode2)
最後に目標関節角の角度入力パネルです。これも、Mode0と同じでもほとんど問題ないのですが、各関節角に対応するInput要素を作ってみます。このとき、現在の関節角の角度が表示されていれば目標角度の入力がしやすいと思いますので、同じよなUIでGUIパネルを作成してみます。
そこで、下記のようなSEATMLを書いてみました。
<state name="Mode2"> <label text="J_Ang:" /> <input id="cJ0" width="7" >0.0</input> <input id="cJ1" width="7" >0.0</input> <input id="cJ2" width="7" >0.0</input> <input id="cJ3" width="7" >0.0</input> <input id="cJ4" width="7" >0.0</input> <input id="cJ5" width="7" >0.0</input> <input id="cJ6" width="7" >0.0</input> <brk /> <label text="Target:" /> <input id="tJ0" width="7" >0.0</input> <input id="tJ1" width="7" >0.0</input> <input id="tJ2" width="7" >0.0</input> <input id="tJ3" width="7" >0.0</input> <input id="tJ4" width="7" >0.0</input> <input id="tJ5" width="7" >0.0</input> <input id="tJ6" width="7" >0.0</input> <brk /> <input id="timeIn2" width="4" >2.0</input> <button label="Send" > </button> <label text="" /> <button label="Copy" > </button> <brk /> <label text="Hand:" /> <button label="Open" ></button> <button label="Close" ></button> <brk /> <label text="Mode:" /> <button label="Mode0" > </button> <button label="Mode1" > </button> <label text="Mode2" bg_color="blue"/> </state>
このソースコードからは、下記のようなGUIパネルが生成されます。
これでほぼGUIパネルの実装は終わりです。あとは、各GUIアイテムへ入力や押下などのイベントに対応する処理、入力ポートからのデータを処理するコードを追加していけばいいと思います。
GUIのイベント処理の追加
次にGUIのイベント処理を追加していきます。GUIアイテムに対するイベント処理ですが、ほぼ下記に集約されると思います。
- モード移行ボタンを押下したときの内部状態遷移
- "Send"、ハンドの開閉ボタン等のボタン押下によるPA10への指令送信
- 入力ポートからのデータをGUIアイテムに出力
最初のモード移行ボタンに対するイベントですが、例えば、Mode0のGUIパネルの"Mode12というボタン押下時のイベントは、
<button label="Mode1" >
<statetransition>Mode1</statetransition>
</button>
と書くことができます。ただし、このときPA10のコントローラの内部状態も変わってもらわないといけないので、str_outのコマンド送信用のデータポートからモード名を出力すればよいと思いますので、
<message sendto="str_out">Mode1</message>
を追加して
<button label="Mode1" >
<message sendto="str_out">Mode1</message>
<statetransition>Mode1</statetransition>
</button>
のように書くことができます。他のモード移行ボタンも同じように書いていけばよいことがわかります。
つぎにPA10のコントローラに対する目標位置姿勢、関節角等の送信ボタンに関しては、input要素などに記載されたデータを送信したいのですから、getEntry関数で値を取得してq_outのデータポートへ TimedDoubleSeqのデータ型に変換して送信すればよいでしょう。
eSEATでは、データポートに対するデータの出力はmessage要素で記載することが多いのですが、message要素での送信は、通常固定されたデータまたは可変の文字列データしか送信できませんので、この場合には、script要素を使ったデータ送信を行います。
例えばMode0のパネルでSendボタンでInput要素内に記載されたデータを送信するのですが、この場合には、下記のように書くことができます。
<button label="Send" >
<script sendto="q_out">
instr = seat.getEntry("Mode0:lineIn")
if instr.strip() :
rtc_result=eval("TimedDoubleSeq(Time(0,0),["+instr+"])")
</script>
</button>
その他のボタンへのイベントに関しては、どのようなデータを送信するのかを考えてscript要素にするかmessage要素にするかを決めればよいと思います。
最後に、入力データポートからのデータをGUIアイテムへ出力するイベント処理を作成していきます。
eSEATでは、button要素やinput要素に対するイベント処理は、bind関数で関連付けを行っていますが、外部のコンポーネントからの入力データに対応する処理は、rule要素で記述します。
通常、rule要素は、key要素を子要素としてもち、すべてのデータポートからの文字列データに対応したマッチング処理をおこなっていますが、rule要素にsource属性を付加することで、source属性でしていされたadaptorからの入力データ到着時のイベント処理を書くことができます。
例えば、Mode0のときにpos_inやq_inからのデータを処理する場合には、rule要素とscript要素を組み合わせて下記のように記述すればよいと思います。
<rule source="pos_in">
<script>
data=rtc_in_data.data
l_data=len(data)
msg = ""
for i in range(l_data):
msg = msg + " %1.3f" % (data[i])
if i < (l_data -1) : msg = msg + ","
seat.setEntry("Mode0:lineOut", msg)
</script>
</rule>
<rule source="q_in">
<script>
data=rtc_in_data.data
l_data=len(data)
msg = ""
for i in range(l_data):
msg = msg + " %1.3f" % (data[i])
if i < (l_data -1) : msg = msg + ","
seat.setEntry("Mode0:JOut", msg)
</script>
</rule>
上記の記載からもわかるように、script要素内ではデータポートから入力されたデータは、rtc_in_dataという大域変数に格納されています。また、input要素への出力は、setEntryというメンバ関数で設定することができます。
あとは各状態、パネルに対するイベント処理を追加していけば完成です。
rtc_handleの統合とその他の入力パネル
上記のもので基本的な操作は可能なのですが、もう少し快適に使うためにRTCの状態操作、ポート間の接続などをおこなうことができるライブラリであるrtc_handleを統合したバージョンを作りたいと思います。
rtc_handleは、電通大の末廣先生により開発されたもので1つのPythonスクリプトであるためeSEATでSEATMLの中に取り込むには非常に便利です。ただし、rtcshellよりも処理が少し複雑ですので、rtc_handle_tool.pyというユティリティスクリプトを作成しています。これらを使って、RTC間のコネクション作成、切断、有効化、非有効化を行うためのボタンを追加してみましょう。
このページに添付したSEATMLファイルには、この機能も追加しています。
また、1つの作業の目標関節角列を管理するような機能もあると、PA10の操作が一層楽になると考えられますので、pos_mgr.seatmlというSEATMLも作成してみました。
これらのGUIパネルを使った動作例は、デモビデオとして作成していますので、動作を確認してみてください。
ソースコードは、このページに添付資料として掲載しています。
デモビデオ
最後に、私が作成したPA10の位置制御プラグインとeSEATで作成した操作パネルの実行例として画面のキャプチャ動画を作成してみました。このページの添付資料としています。
この動画ファイル(PA10_Demo.mp4)はMP4になっていますので、videoタグに対応していないブラウザの場合には、ダウンロードしてからでないと見ることができないと思います。
資料