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/:このウェブページ

【必須】以下のページから「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」です。

最新バージョン

初めての方へ

Windows msi(インストーラ) パッケージ (サンプルの実行ができます。)

C++,Python,Java,
Toolsを含む
1.1.2-RELEASE

RTコンポーネントを開発するためには開発環境のインストールが必要です。詳細はダウンロードページ

統計

Webサイト統計
ユーザ数:1618
プロジェクト統計
RTコンポーネント286
RTミドルウエア21
ツール20
文書・仕様書1

Join our slack

Enter email address for slack invite.

旧Webサイト

OpenRTM.org旧Webサイト

OpenHRP3

動力学シミュレータ

Choreonoid

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

OpenHRI

対話制御コンポーネント群

OpenRTP

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

産総研RTC集

産総研が提供するRTC集

TORK

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

DAQ-Middleware

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

VirCA

遠隔空間同士を接続し、実験を行うことが可能な仮想空間プラットホーム