このケーススタディでは、LeapMotion センサーを利用して Choreonoid シミュレーターで機動中の GRobo を制御します。まずは準備されたコンポーネント(デモシステム及びサンプルコンポーネント)で動作確認をします。それから、自作コンポーネントでシミュレーター上のロボットを制御します。
以下のアーカイブをダウンロードし解凍してください。
robomec2015_openrtm_tutorial_part3.zip 2015/05/20更新
アーカイブの内容にある主なるファイルは以下のとおりです。
【必須】以下のページから「Windows Download」をクリックして、LeapMotion のドライバーをダウンロードしてインストールしてください。
https://www.leapmotion.com/setup/windows
ここでは、デモシステムを機動し動作を確認します。以下の手順にしたがってデモシステムを起動させてください。
ここでは、デモシステムの中でロボットを制御するコンポーネント「GRobotDemo」の代わりにサンプルコンポーネントを使って、シミュレーター上のロボットを制御します。
以下の手順を始める前に、まずはデモシステムを上述の手順で起動してください。
ここでは、ロボット制御コンポーネントを作成する手順を説明します。まずは RTCBuilder を利用してソースのスタブを生成します。それから自由にソースを編集してLeapMotionからのデータに対してシミュレータ上のロボットを制御するアルゴリズムを実装します。
種類 | ポート名 | データ型 |
入力ポート | hand_positions | RTC::TimedFloatSeq |
出力ポート | command | RTC::TimedString |
出力ポート | target_angles | RTC::TimedShortSeq |
以上の準備ができたら、CMake と Visual Studio を利用してコンポーネントのソースを編集してコンパイルします。生成されたソースの中に「include/<コンポーネント名>/<コンポーネントト名>.h」「src/<コンポーネント名>.cpp」を開いて、以下の編集を行ってください。
ヘッダーファイルのクラス定義で以下のプライベート変数を追加してください。
bool m_rightUp; bool m_leftUp;
.cppファイルで、12行目で以下の関数を追加してください。
double minmax(double a, double max, double min) { if (a > max) { return max; } else if (min > a) { return min; } return a; }
.cppファイルで、コンストラクタで以下の行を追加してください。
m_rightUp = false; m_leftUp = false;
.cppファイルで、onExecute メソッドで以下のソースをペストしてください。
if (m_hand_positionsIn.isNew()) { m_hand_positionsIn.read(); // Hand position data is received as an array of floats: // [right_hand_x, right_hand_y, right_hand_z, left_hand_x, left_hand_y, left_hand_z] // Where the x axis runs left to right across the LeapMotion sensor, the y axis is // verticle, and the z axis is front to back across the LeapMotion sensor. // So if you move your hand up and down above the sensor, the y axis will change. // If you move your hand towards the screen, the z axis will change. if (m_hand_positions.data.length() == 6) { // Calculate a command based on the hand positions // Right hand if (m_hand_positions.data[1] > 15 && !m_rightUp) { // Raise the hand if (m_leftUp) { m_command.data = CORBA::string_dup("rightup2"); } else { m_command.data = CORBA::string_dup("rightup1"); } m_rightUp = true; std::cout << "Right hand up\n"; // Write the output port m_commandOut.write(); } else if (m_hand_positions.data[1] < -15 && m_rightUp) { // Lower the hand if (m_leftUp) { m_command.data = CORBA::string_dup("rightdown2"); } else { m_command.data = CORBA::string_dup("rightdown1"); } m_rightUp = false; std::cout << "Right hand down\n"; // Write the output port m_commandOut.write(); } // Left hand if (m_hand_positions.data[4] > 15 && !m_leftUp) { // Raise the hand if (m_rightUp) { m_command.data = CORBA::string_dup("leftup2"); } else { m_command.data = CORBA::string_dup("leftup1"); } m_leftUp = true; std::cout << "Left hand up\n"; // Write the output port m_commandOut.write(); } else if (m_hand_positions.data[4] < -15 && m_leftUp) { // Lower the hand if (m_rightUp) { m_command.data = CORBA::string_dup("leftdown2"); } else { m_command.data = CORBA::string_dup("leftdown1"); } m_leftUp = false; std::cout << "Left hand down\n"; // Write the output port m_commandOut.write(); } // Calculate joint angles for direct control based on the hand positions // Output target positions are published as an array of 6 pairs of short integers. Each pair is [joint index, angle]: // [14, <joint 14 angle>, 15, <joint 15 angle>, ...] m_target_angles.data.length(12); // Set joint numbers to be manipulated m_target_angles.data[0] = 14; m_target_angles.data[2] = 15; m_target_angles.data[4] = 16; m_target_angles.data[6] = 17; m_target_angles.data[8] = 18; m_target_angles.data[10] = 19; // Right hand position m_target_angles.data[1] = minmax(m_hand_positions.data[1] * -10, 1500, -1500); m_target_angles.data[3] = minmax(m_hand_positions.data[0] * -10, 200, -1500); m_target_angles.data[5] = minmax((m_hand_positions.data[2] - 50) * 10, 200, -1300); // Left hand position m_target_angles.data[7] = minmax(m_hand_positions.data[4] * 10, 1500, -1500); m_target_angles.data[9] = minmax(m_hand_positions.data[3] * -10, 1500, -200); m_target_angles.data[11] = minmax((m_hand_positions.data[5] - 50) * -10, 1300, -200); // Write the output port m_target_anglesOut.write(); //std::cout << "From hand positions:\nRight: (" << m_hand_positions.data[0] << ", " << m_hand_positions.data[1] << // ", " << m_hand_positions.data[2] << ")\t Left: (" << m_hand_positions.data[3] << ", " << m_hand_positions.data[4] << // ", " << m_hand_positions.data[5] << ")\nSetting joint positions:\n14: " << m_target_angles.data[1] << // ", 15: " << m_target_angles.data[3] << ", 16: " << m_target_angles.data[5] << ", 17: " << m_target_angles.data[7] << // ", 18: " << m_target_angles.data[9] << ", 19: " << m_target_angles.data[11] << "\n\n"; } } return RTC::RTC_OK;
サンプルコンポーネントと同じ手順でコンパイルしてください。
コンパイルされたコンポーネントを LeapMotionGRoboControlSample0 と同じように利用できます。
サンプルコンポーネントと同じ振る舞いができたら、以下の入出力データフォーマットを参照しながら、自作コンポーネントのソースを編集して振る舞いを変更してください。
LeapMotionのeSEAT0.hands_out 出力ポート(自作コンポーネントのhand_positions入力ポートにくるデータ)のフォーマットは以下のようです。
[右手x,右手y, 右手z, 左手x,左手y,左手z]
自作コンポーネントの command 出力ポートは以下のコマンドの一つを出力してロボットのポーズを制御します。
自作コンポーネントの target_angles 出力ポートは、軸の位置を出力します。フォーマットは
[軸番号, 位置, 軸番号, 位置, ...]
です。例えば、両腕の軸の位置を設定するために以下のデータを出力します。
[14, <位置>, 15, <位置>, 16, <位置>, 17, <位置>, 18, <位置>, 19, <位置>]
別のパソコンで機動中のコンポーネントを利用することはあります。このセクションでネットワーク越えコンポーネントの接続方法を説明します。
以下の手順を行う前に、必ず Windows Firewall を無効にしてください。
まずは両パソコンの間でネットワークが接続されて、相手側のネームサーバーが見えるかどうかを確認します。
ネームサーバーが見えるようになったら、コンポーネントが通信できるかどうかを確認します。
相手側のコンポーネントが操作できない場合はあります。この場合は以下の手順を両パソコンに行ってください。CORBA の設定を変更します。
corba.endpoint: <IPアドレス>
以上の手順にしたがって相手側のコンポーネントとの接続ができたら、同じ手順で相手側の「LeapRTC0」コンポーネントをこちら側の RTSystemEditor で利用できます。
RTSystemEditor上 で System Diagram に相手側の LeapRTC0 を置き、ポートを接続すると直接センサーをこちら側のパソコンにつながっているのと同じ様に動きます。
RTSystemEditor でコンポーネントが操作できない場合は、前セクションの手順を行ってください。しかし、rtc.conf の場所は「robomec2015_openrtm_tutorial_part3/Demo/LeapMotion」と「robomec2015_openrtm_tutorial_part3/Demo/eSEAT」です。