新しいmc_rtcロボットインタフェースの実装方法

インターフェイスとは、mc_rtcとロボット（またはシミュレータ）を接続するソフトウェアのことです。インターフェイスは反復ごとに次の処理を行います。

ロボットの状態（関節位置、速度、トルク、IMU、力センサーなど）を読み取る
それをmc_rtcに渡し、コントローラーを実行する
結果（関節位置、速度、またはトルク）をロボットに送り返す

このチュートリアルでは、新しいロボット用のインターフェイスの書き方を説明します。方法は2つあります。

推奨: URI（Unified Robot Interface）用の小さなドライバプラグインを書く。ロボット固有ではない部分はすべてURIがすでに提供しています。
従来の方法: mc_control::MCGlobalControllerを組み込んだ専用のインターフェイス実行ファイルを書く。インターフェイスのページに掲載されているインターフェイスの多くはこの方法で書かれています。

このチュートリアルは、mc_rtcのコントローラーとロボットモジュールに慣れており、対象ロボットのRobotModuleがすでにあることを前提としています（こちらのチュートリアルを参照してください）。

URI用ドライバの作成

URIとは

URIは従来のインターフェイスを3つの部分に分割します。

uri manager: すべてのロボットに対してソルバー（mc_rtc）を実行する1つのプロセス。通常のmc_rtc.yamlにRobotsセクションを加えて設定します。
uri interface: ロボットごとに1つ存在する汎用プロセス。ロボット側のPCまたは制御用PCで動作し、起動時にあなたのドライバを読み込みます。
両者の間の通信レイヤー（ZenohとFlatBuffers）。これを直接使う必要はありません。

あなたが書く部分はドライバだけです。ドライバはロボットのSDKをテンプレート化されたドライバでラップする共有ライブラリで、mc_rtcには一切依存しません。さらに、1つのコントローラーで異なる複数のロボットをそれぞれの制御周期で同時に動かすことができ、ロボットは別々のコンピュータ上に置くこともできます。

URIのインストール

URIはmc_rtcの隣にインストールします。mc-rtc-superbuildでmc_rtcをビルドしている場合は、WITH_URIオプションを有効にします。

cd mc-rtc-superbuild
cmake --preset relwithdebinfo -DWITH_URI=ON
cmake --build --preset relwithdebinfo

その他のインストール方法（手動、devcontainer）はURIのドキュメントに記載されています。

使用したインストールプレフィックスを覚えておいてください。uri interfaceは<prefix>/lib/robot_interface内のドライバだけを探すため、ドライバも同じプレフィックスでインストールする必要があります。

既存のドライバの確認

Universal Robots（RTDE）、Kinova Gen3、xArm、ros2_controlのcontroller managerを持つ任意のロボットなどのドライバがすでに存在します。対応ロボットの一覧を参照してください。お使いのロボットが載っている場合は、mc_rtcでドライバを使うに進んでください。

ドライバプロジェクトの作成

このトピックの詳細な情報はURIのウェブサイトにあります（Create a new robot driverを参照してください）。

URIはすぐにビルドできるプロジェクトを生成してくれます。

uri create_new_driver MyRobot <new_driver_path>

これにより、スタンドアロンのCMakeプロジェクトmy_robot_driverと、RobotDriverMyRobotという名前のクラスが作成されます。このプロジェクトはすでにコンパイルできます。空の状態を報告し、コマンドを無視するだけのスケルトンです。

ドライバの実装

src/RobotDriverMyRobot.cppを開いてメソッドを実装します。ほとんどの場合に実装するのは次のメソッドです。

メソッド	実装すべき内容
コンストラクタ	受け取った`ip`、`port`、およびオプションの`config_path`を使ってロボットに接続する。接続に失敗した場合は例外を投げる。
`sync()`	ロボットが新しい状態を公開するまでブロックする。これが制御周期を決めます。`sleep()`ではなく、SDKのブロッキング読み取りを使ってください。
`getActualQ()`	関節位置 [rad]。まだ状態が得られていない場合は空のベクトルを返す。
`getActualQd()`	関節速度 [rad/s]（任意）
`getJointTorques()`	関節トルク [Nm]
`servoJ(q)`	関節位置 [rad] をロボットに送る
`speedJ(qd)`	関節速度 [rad/s] をロボットに送る
`tauJ(tau)`	関節トルク [Nm] をロボットに送る
`getIMUs()`、`getForceSensors()`	センサーの読み取り値（任意）

実装が必要なのは、ロボットが対応する制御モードのコマンドメソッドだけです（位置制御ならservoJなど）。それ以外は例外を投げるか、何もしなくて構いません。

特に重要なルールが3つあります。

関節の順序: ドライバに入出力されるすべてのベクトルは、RobotModuleのref_joint_orderに従います。SDKが異なる順序を使う場合は、ドライバ内で並べ替えてください。
単位: rad、rad/s、Nm、N。クォータニオンは(w, x, y, z)です。
センサー: getIMUs()とgetForceSensors()でキーとして使う名前は、RobotModuleのボディセンサーおよび力センサーと一致している必要があります。

SDKが追加のライブラリを必要とする場合は、生成されたCMakeLists.txtに通常どおりfind_packageとtarget_link_librariesで追加してください。ipとportに収まらない設定（トピック名やゲインなど）は独自のYAMLファイルに記述でき、そのパスはconfig_pathとしてコンストラクタに渡されます。

ビルドとインストール

URI（およびmc_rtc）と同じプレフィックスでドライバをビルドします。

cd my_robot_driver
cmake -S . -B build \
  -DCMAKE_PREFIX_PATH=/path/to/install \
  -DCMAKE_INSTALL_PREFIX=/path/to/install
cmake --build build
cmake --install build

生成されたソースにはROBOT_DRIVER_EXPORT_ABI_VERSION()の行が含まれています。削除しないでください。この行がないドライバはURIに読み込みを拒否されます。URIがドライバのインターフェイスを変更した場合、古いバージョンに対してビルドされたドライバは明確なメッセージとともに読み込みに失敗するため、再ビルドが必要です。

ドライバ単体でのテスト

mc_rtcを使う前に、ドライバが接続でき、状態が正しく見えることを確認します。ロボットのアドレスを書いた小さなファイルを用意します。

robot_interface:
  driver: RobotDriverMyRobot
  ip: 192.168.1.10
  port: 0

次にビューアを実行します。

uri viewer -c viewer.yaml

ビューアはuri interfaceとまったく同じようにドライバを読み込み、関節位置、速度、トルク、およびsync()で費やされた時間を表示します。ロボットを手で動かして（またはシミュレーションで）、関節の順序、符号、単位を確認してください。ビューアはコマンドを一切送信しないため、実機でも安全に使用できます。

mc_rtcでドライバを使う

mc_rtc.yamlのRobotsセクションにロボットを追加します。それ以外の項目（MainRobot、Enabled、オブザーバー、プラグインなど）の意味は従来どおりです。

MainRobot: MyRobot
Enabled: Posture
Timestep: 0.005            # mc_rtcは200 Hzで動作

Robots:
  my_robot:                # ロボット名
    module: MyRobot        # mc_rtcのRobotModule
    controller:
      mode: position       # position | velocity | torque
      time_step: 0.001     # ロボット自身の制御周期（1 kHz）
    network_interface:
      protocol: zenoh
    robot_interface:
      driver: RobotDriverMyRobot
      ip: 192.168.1.10
      port: 0
      autostart: true

いくつかの項目について説明します。

moduleは、ロボットのmc_rtc RobotModuleです。
controller.modeは、ロボットに送るコマンドの種類（position、velocity、またはtorque）です。servoJ、speedJ、tauJのどれが呼ばれるかを決めます。
controller.time_stepは、ロボットの制御周期です。（mc_rtcの）Timestepはこの値の整数倍でなければなりません。この例では、ロボットは1 kHz、mc_rtcは200 Hzで動作し、その間は最後のコマンドが再送されます。
robot_interface.driverは、ドライバのクラス名です。ipとportは、ドライバが使用しない場合でも常に必須です。
autostart: trueを指定すると、uri managerがuri interfaceを自動的に起動します。ロボットが制御用PCから到達可能な場合に使用します。

インターフェイス実行ファイルの代わりに、uri managerでコントローラーを起動します。

uri manager -c mc_rtc.yaml

ドライバが読み込まれ、ロボットが実際の関節位置で初期化され、mc_rtcがコントローラーを開始する様子が確認できるはずです。これまでと同様に、任意のmc_rtc GUIクライアントでコントローラーを操作できます。

コントローラーは、すべてのロボットから最初の状態を受信してから開始されるため、ロボットがデフォルトの姿勢に向かって急に動くことはありません。

別のコンピュータ上のロボット

ロボットが専用のPC（例えばアームの隣にあるリアルタイムPC）に接続されている場合は、autostartを削除し、そのPCにドライバをインストールします。そのPCで、uri managerの前にuri interfaceを起動してください。必要なのはロボットの名前だけです。

# my_robot_interface.yaml（ロボット側PC）
name: my_robot             # Robots:の下のキーと同じ名前
network_interface:
  protocol: zenoh

# ロボット側PCで
uri interface -c my_robot_interface.yaml
# 制御用PCで
uri manager -c mc_rtc.yaml

ドライバ名、IPアドレス、制御モードはマネージャから送られるため、ロボット側のPCに触れずにドライバを切り替えられます。接続が失われた場合、マネージャはコントローラーを停止し、インターフェイスはロボットをその場に保持します。

複数のロボット

Robotsの下にエントリを追加します。各ロボットはそれぞれ独自のドライバと制御周期を持ち、すべて同じmc_rtcコントローラーによって制御されます。baseキーは別のエントリをコピーするため、異なる部分だけを書けば済みます。

さらに詳しく

このチュートリアルでは要点のみを扱いました。URIのドキュメントには次の詳細があります。

新しいロボットドライバの作成: グリッパー、ROS 2ベースのドライバ、チェックリスト
ロボットマネージャ: mc_rtc.yamlの完全なリファレンス、タイミングのルール、接続断の処理
コマンドライン: すべてのuriサブコマンド
シミュレーション: 同じコントローラーをシミュレーション上のロボットで実行する

従来の方法: 専用のインターフェイス実行ファイル

これはロボットをmc_rtcと統合する従来の方法です。現在もサポートされていますが、新しいロボットには推奨されません。

この方法では、mc_rtcとリンクし、mc_control::MCGlobalControllerを組み込んだ実行ファイルを書きます。ロボットとの通信、タイミング、mc_rtcの呼び出しといったリアルタイムループ全体を自分で管理します。各反復は次のようになります。

// 専用インターフェイスのスケッチ
mc_control::MCGlobalController controller("/path/to/mc_rtc.yaml");
auto & robot = controller.robot();

// 1. 実ロボットの状態からコントローラーを初期化する
controller.init(readJointPositions());

// 2. ロボットに同期したリアルタイムループ
while(running)
{
  waitForNextRobotState();

  // 3. mc_rtcにセンサー値を渡す
  controller.setEncoderValues(readJointPositions());
  controller.setJointTorques(readJointTorques());
  controller.setSensorOrientations(Accelerometer);

  // 4. コントローラーを1回実行する
  if(controller.run())
  {
    // 5. 結果をロボットに送る
    sendJointPositions(robot.mbc().q);
  }
}

主に使用する関数は次のとおりです。

mc_control::MCGlobalController::init()は、ロボットの初期状態からコントローラーを初期化します
mc_control::MCGlobalController::setEncoderValues()、mc_control::MCGlobalController::setEncoderVelocities()、mc_control::MCGlobalController::setJointTorques()は、関節の状態を提供します
mc_control::MCGlobalController::setSensorOrientations()、mc_control::MCGlobalController::setSensorAngularVelocities()、mc_control::MCGlobalController::setSensorLinearAccelerations()、mc_control::MCGlobalController::setWrenches()は、その他のセンサーを提供します
mc_control::MCGlobalController::run()は、コントローラーを1回実行します

URIドライバと比べると、専用のインターフェイスにはいくつかの欠点があります。

ロボットごとに、mc_rtcとその依存ライブラリにリンクする専用の実行ファイルが必要で、多くの場合それらを個別にビルドする必要があります。
タイミング、安全性（接続断、ロボットの保持）、マルチレートの処理を毎回実装し直す必要があります。
異なるSDKや制御周期を持つ複数のロボットを1つのコントローラーから制御することは、標準ではサポートされていません。
ロボットのSDKはmc_rtcと同じマシン上で動作する必要があります。

既存のインターフェイス（インターフェイスのページを参照）は、保守や移植が必要な場合に参考になる良い例です。例えば独自のメインループを持つシミュレータを統合するなど、新しいスタンドアロンのインターフェイスがどうしても必要な場合は、それらを出発点にできます。

次のチュートリアル: mc_rtcにおける環境データの作成