LeapMotion で Choreonoid の制御

このケーススタディでは、LeapMotion センサーを利用して Choreonoid シミュレーターで機動中の GRobo を制御します。まずは準備されたコンポーネント(デモシステム及びサンプルコンポーネント)で動作確認をします。それから、自作コンポーネントでシミュレーター上のロボットを制御します。

リソースと事前準備

以下のアーカイブをダウンロードし解凍してください。

robomec2015_openrtm_tutorial_part3.zip 2015/05/20更新

アーカイブの内容にある主なるファイルは以下のとおりです。

  • Demo/: デモシステムのコンポーネント及び Choreonoid
    • rtc_handle.bat: デモシステムを機動するためのバッチファイル
  • src/: 準備されたコンポーネントのソース
    • LeapMotion_RobotControl_Sample: サンプルコンポーネントのソース
  • LeapMotionGRoboControlSampleComp.exe: サンプルコンポーネントのバイナリー(x64、Visual Studio 2013用)
  • online_tutorial/:この Webページ

【必須】以下のページから「Windows Download」をクリックして、LeapMotion のドライバーをダウンロードしてインストールしてください。

https://www.leapmotion.com/setup/windows

デモシステム動作確認

ここでは、デモシステムを機動し動作を確認します。以下の手順にしたがってデモシステムを起動させてください。

  1. スタートメニューの「Start Naming Service」でネームサーバーを機動する(OpenRTMのドキュメントに参照してください)
  2. スタートメニューから「RTSystemEditorRCP」または「OpenRTP 1.1.1」を起動する
  3. Demo/rtc_handle.batを実行してデモシステムのランチャーを起動する
    start_launcher.png
    rtchandle_launcher.png
  4. RtcHandle で以下のボタンをクリックしてコンポーネントを起動する
    1. CNoid_G
    2. RobotDemo
    3. LeapRTC
    4. LeapMotion
      launch_demo_components.png
      rtchandle_launched_components.png
  5. 「getRtcList」をクリックする
    rtchandle_getrtclist_button.png
    rtchandle_getrtclist_result.png
  6. 「Load」をクリックして「LeapDemo.data」を選択する
    rtchandle_load_button.png
    rtchandle_load_button_after.png
  7. 「ConnectAll」ボタンをクリックしてコンポーネントを接続するとコンポーネントは自動的に以下のように接続される。
    rtchandle_connectall_button.png
    rtsysed_demo_connected.png
  8. 「ActivateAll」をクリックしてデモを起動してシステムを起動する
    rtchandle_activateall_button.png
    rtsysed_demo_activated.png
  9. LeapRTCCompのウィンドーが選択された状態で LeapMotionの上で手を動かすと、「LeapMotion」ウィンドーでデータが現れ、コマンドによってシミュレーター上のロボットが動く
    demo_moving_robot.png

サンプルコンポーネントの動作確認

ここでは、デモシステムの中でロボットを制御するコンポーネント「GRobotDemo」の代わりにサンプルコンポーネントを使って、シミュレーター上のロボットを制御します。

以下の手順を始める前に、まずはデモシステムを上述の手順で起動してください。

  1. 「LeapMotionGRoboControlSampleComp」をコンパイルする
    1. CMakeを起動する
    2. CMake上で、サンプルコンポーネントのソースダイレクトリー(解凍された場所のsrc/LeapMotion_RobotControl_Sample)をソースコードとして指定する
      cmake_select_source.png
    3. CMake上で、buildディレクトリーを、ソースディレクトリーの中の「build」ディレクトリーに指定する
      cmake_select_build.png
    4. 「Configure」ボタンをクリックする
      cmake_configure_button.png
    5. Generatorを選択する。以下のは、Visual Studio 2013でx64用のバイナリーの場合である。自分がインストールした OpenRTM バージョンに合わせた Generator を選択する。
      cmake_select_generator.png
    6. 「Generate」ボタンをクリックしてプロジェクトを生成する
      cmake_generate_button.png
    7. 「src/LeapMotion_RobotControl_Sample/build」にある「LeapMotionGRoboControlSample.sln」をVisual Studioで開いて、「ALL_BUILD」を右クリックメニューし、コンパイルする
      sample_component_project.png
      sample_component_compile.png
    8. 以下の用にコンポーネントのバイナリーができた
      sample_component_binary.png
  2. 「LeapMotionGRoboControlSampleComp」を起動してロボットを制御する
    1. LeapMotionGRoboControlSampleComp.exeを実行する
    2. RTSystemEditorでGRobotDemoコンポーネントのコネクションを削除する
      grobotdemo_delete_connections.png
    3. RTSystemEditorでLeapMotionGRoboControlSampleCompを以下の用につなげて、コンポーネントを activate する
      • LeapMotionのeSEAT0.hands_outポートからLeapMotionGRoboControlSample0の.hand_positionsへ
      • LeapMotionGRoboControlSample0.hand_positionsからRobotMotion0の.targetAngleへ
        sample_component_connect.png
    4. LeapRTCCompのウィンドーが選択された状態でLeapMotionの上で手を動かすとシミュレーター上のロボットの手が自分の手に合って動く
      sample_component_follow_hands.png

自作コンポーネントを作る

ここでは、ロボット制御コンポーネントを作成する手順を説明します。まずは RTCBuilder を利用してソースのスタブを生成します。それから自由にソースを編集してLeapMotionからのデータに対してシミュレータ上のロボットを制御するアルゴリズムを実装します。

  1. RTCBuilderを起動する
  2. 新しい RTCBuilder プロジェクトを作る(プロジェクト名、コンポーネント名などを自由に選択してください)
    rtcbuilder_new_project.png
    rtcbuilder_project_type.png
  3. アクティビティでonActivatedonDeactivated及びonExecuteを選択する
  4. 以下のデータポートを作成する
種類 ポート名 データ型
入力ポート hand_positions RTC::TimedFloatSeq
出力ポート command RTC::TimedString
出力ポート target_angles RTC::TimedShortSeq
rtcbuilder_create_ports.png
  1. サービスポートはないのでタブをスキップする
  2. コンフィグレーションはないのでタブをスキップする
  3. ソースコードの言語をC++に設定する
    rtcbuilder_select_language.png
  4. コンポーネントソースのスタブを生成する
    rtcbuilder_generate_source.png

以上の準備ができたら、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 出力ポートは以下のコマンドの一つを出力してロボットのポーズを制御します。

  • b_step
  • balance
  • bothdown
  • bothup
  • bow
  • bye
  • f01
  • f1
  • f_step
  • go_ahead
  • go_back
  • gymnastics
  • init
  • kick
  • l1
  • left_step
  • leftdown1
  • leftdown2
  • leftup1
  • leftup2
  • look_left
  • look_right
  • motion
  • muri
  • r1
  • rest
  • right_step
  • rightdown1
  • rightdown2
  • rightup1
  • rightup2
  • turn_l
  • turn_r

自作コンポーネントの target_angles 出力ポートは、軸の位置を出力します。フォーマットは

 [軸番号, 位置, 軸番号, 位置, ...]

です。例えば、両腕の軸の位置を設定するために以下のデータを出力します。

 [14, <位置>, 15, <位置>, 16, <位置>, 17, <位置>, 18, <位置>, 19, <位置>]

別パソコンのコンポーネントを利用する場合

別のパソコンで機動中のコンポーネントを利用することはあります。このセクションでネットワーク越えコンポーネントの接続方法を説明します。

以下の手順を行う前に、必ず Windows Firewall を無効にしてください。

ネームサーバー接続確認

まずは両パソコンの間でネットワークが接続されて、相手側のネームサーバーが見えるかどうかを確認します。

  • 相手側のパソコンのIPを確認する。相手側のパソコンのコントロールパネルのネットワーク設定からコネクション情報を開く(以下はWindows 8の場合)
    win8_network_settings.png
  • 詳細バトンでIPアドレス情報を表示する
    win8_adapter_status.png
    win8_adapter_address.png
  • コマンドプロンプトからも確認できる(相手側のパソコンに実行する)
    find_ip_address_windows.png
  • RTSystemEditorで相手側のネームサーバーに接続する。「ネームサーバを追加」バトンをクリックし、相手側のIPアドレスを入力する
    rtsysed_add_nameserver_button.png
    rtsysed_add_nameserver_dialog.png
  • 成功の場合、以下の用にネームサーバリストに相手側のネームサーバーが追加される
    rtsysed_added_nameserver.png

コンポーネント接続確認

ネームサーバーが見えるようになったら、コンポーネントが通信できるかどうかを確認します。

  • こちら側のパソコンで、スタートメニューから「ConsoleInComp.exe」を実行する
  • 相手側のパソコンで、スタートメニューから「ConsoleOutComp.exe」を実行する
  • こちら側のパソコンの RTSystemEditor で ConsoleIn0(こちら側のネームサーバーから)と ConsoleOut0(相手側のネームサーバーから)を System Diagram に置き、ポートを接続する
    rtsysed_connect_consolein_consoleout.png
  • 成功の場合、こちら側の ConsoleInComp.exe ウィンドウで数字を入力すると、相手側の ConsoleOutComp.exe で表示される。

相手側のコンポーネントが操作できない場合はあります。この場合は以下の手順を両パソコンに行ってください。CORBA の設定を変更します。

  • 「C:/Program Files/OpenRTM-aist/1.1/examples/C++」(64 bitの場合)または 「C:/Program Files (x86)/OpenRTM-aist/1.1/examples/C++」(32 bitの場合)を Explorer で開く
  • 「rtc.conf」をノートパッドで編集する
  • 以下の行を追加する
      corba.endpoint: <IPアドレス>
  • 注意:こちら側のパソコンでこちら側のIPアドレスを入力し、相手側のパソコンで相手側のIPアドレスを入力してください。
  • 全コンポーネントを再起動すると RTSystemEditor で操作できるようになる

相手側の LeapMotion コンポーネントを利用する

以上の手順にしたがって相手側のコンポーネントとの接続ができたら、同じ手順で相手側の「LeapRTC0」コンポーネントをこちら側の RTSystemEditor で利用できます。

RTSystemEditor上 で System Diagram に相手側の LeapRTC0 を置き、ポートを接続すると直接センサーをこちら側のパソコンにつながっているのと同じ様に動きます。

RTSystemEditor でコンポーネントが操作できない場合は、前セクションの手順を行ってください。しかし、rtc.conf の場所は「robomec2015_openrtm_tutorial_part3/Demo/LeapMotion」と「robomec2015_openrtm_tutorial_part3/Demo/eSEAT」です。

ダウンロード

最新バージョン : 2.0.1-RELESE

統計

Webサイト統計
ユーザ数:2159
プロジェクト統計
RTコンポーネント307
RTミドルウエア35
ツール22
文書・仕様書2

Choreonoid

モーションエディタ/シミュレータ

OpenHRP3

動力学シミュレータ

OpenRTP

統合開発プラットフォーム

産総研RTC集

産総研が提供するRTC集

TORK

東京オープンソースロボティクス協会

DAQ-Middleware

ネットワーク分散環境でデータ収集用ソフトウェアを容易に構築するためのソフトウェア・フレームワーク