こちらはフィジカルAIアドベントカレンダー13日目の記事です．

こんにちは．
近年，機械学習技術がNLPやCVタスクを高精度で実行できるようになり，改めてAIと実世界の相互作用の重要性が見直されています．その文脈でロボティクスとAIの融合領域であるPhysical AIやEmbodied AIに対する関心が高まっています．これらの概念を簡単に整理すると以下のようになるかと思います．

生成モデル / AI：新しいコンテンツを生成するAI
基盤モデル：大規模なデータで事前学習され，特定機能を持つモデルの基盤となるもの
Embodied AI：身体性を持って環境に作用できるAI．その手段の１つとしてのロボット基盤モデル
Physical AI：現実世界で知的・自律的に振る舞うシステム全般

特に，技術的なトレンドとしては以下の３つのアプローチが中心的です．

強化学習 (RL)：ヒューマノイドロボットの歩行やダンスでよく使われる
模倣学習 (IL)：ALOHAを始めとする双腕ロボットの制御によく使われる
VLA/ロボット基盤モデル：GoogleのGemini RoboticsやPhysical Intelligenceのπ0などを始めとして世界で開発競争が進む

私が所属する京都大学でも，Physical AI に関心を持つ学生・研究者が増えており，その潮流の中で今年の7月にはKyoto University Physical AI Community (KUPAC)という学生コミュニティが発足しました．KUPACでは，Physical AIに関する話題を幅広くテーマとして，勉強会やハンズオンイベントを行っています．

本記事では，KUPACの内部プロジェクト (KVT) として開発を進めている，自作双腕マニピュレータにおけるデータ収集パイプライン構築の事例について紹介します．

そもそも，なぜ双腕マニピュレータを自作する必要があるのかという話ですが，模倣学習やVLAの研究においてデファクトスタンダードとなりつつあるALOHAのようなシステムを構築しようとすると，マスタースレイブ制御のためにアームが4本必要になり，コストや場所の問題がハードルとなります．
実際，ALOHAを購入するためには数百万円が必要であり，学生としてはなかなか手を出しづらい価格となっています．

そこで私たちはVRヘッドセットを仮想的なマスターアームとして利用し，安価かつ省スペースに学習データを収集するというアプローチで開発を行いました．

実際に，今年の11月に行われた京都大学の学祭では，今回構築したシステムを用いてデータを収集・学習し，ロボットが自動的にお菓子を収集するデモを展示しました．

以下に，ハードウェア・ソフトウェアそれぞれの構成と，開発中に直面した課題について詳しく解説していきます．

ハードウェア構成

ハードウェアにおけるテーマは以下の２つです．

可能な限り低コスト化する
見た目や，自由度構成を標準機 (WidowX) に寄せる

1. 低コスト化へのアプローチ

ALOHAの価格が高くなっている主な要因として，採用されているアーム (WidowX) が高価なサーボモータ (Dynamixel) を多数使用している点が挙げられます．特に，負荷のかかる根元付近の関節にはダブルモータ構成が採用されており，コスト増の大きな要因となっています．
そこで本プロジェクトでは，根元付近の関節を安価かつ高トルクなQDDモータ (Robstride) に置き換えることで，低コスト化を図りました．

2. 標準機への準拠とVLAへの対応

近年，機械学習を用いた双腕マニピュレータ制御において，トレンドとなっている模倣学習やVLA (Vision-Language-Action) モデルですが，特にVLAにおいては，大規模なロボットデータによる事前学習が前提となります．
たとえデプロイ時にファインチューニングを行うとしても，学習済みモデルの事前学習データ分布と，運用するロボットから取得されるデータの分布差は小さい方が望ましいです．また，大規模に事後学習を行う場合でも，そのロボットのオープンデータセットが存在している事が重要となります．
そのため，将来的なVLAの活用も見据え，見た目や自由度構成を標準的なロボットに寄せる必要がありました．

トレードオフの解消（ハイブリッド構成）

低コスト化のためのアクチュエータ変更と，見た目を標準機に寄せる事はトレードオフの関係にあります．
そこで今回は，以下のようなハイブリッド構成を採用することでこの問題を解決しました．

アーム先端部分：観測画像への映り込みによる影響が大きいため，オリジナル版と同様にDynamixelを採用
アーム根元部分：画像への影響が小さく，かつコスト削減効果が大きいため，QDDモータ (Robstride) に変更

ソフトウェア構成

ソフトウェアにおけるテーマは以下の３つです．

直感的なUI
低遅延
シンプルなデータフロー

以下のパートでは失敗談も含めつつ，Quest 3側，PC側の実装に分けて解説します．

初期モデルの失敗と現在の構成

現在のシステム構成にたどり着く前に，一度失敗を経験しています．

失敗1：過度な低遅延の追求

初期モデルでは，低遅延性を極限まで追求するため，Quest 3からUDP通信で直接ロボット制御用のマイコン (ESP32) に関節角度情報を送信していました．しかし，これでは収集したデータとカメラ画像を同期させて保存するという，データ収集パイプラインとの統合が非常に困難でした．
また，Questアプリ，マイコン，PCの３つを連携して動作させる必要があり，起動操作やデバッグが煩雑化してしまいました．

失敗2：ROSによる点群転送の無駄

また，当初はロボット視点での直感的な制御を可能とするために，RealSenseから取得したRGB点群データをROS経由でQuestに送信し，MR空間内に表示していました．しかし，帯域制限により点群の更新頻度が低く，また解像度も荒いため，単に視認性が悪いだけで操作性の向上には寄与しないという結果になりました．
さらに，通信系統が映像用のROSと，制御信号用のUDPの２系統に分かれることで，システム構成が無駄に複雑化していました．

※初期モデルの操作デモ

これらの反省から，現在はPCを中心としたシステム構成とし，Quest 3は入力デバイス，制御やデータの保存はPCという役割分担を明確にしました．

大まかな処理の流れとしては，Quest 3で取得したコントローラのPose（位置・姿勢）から逆運動学（IK）を解いて目標関節角度を算出し，それをPC経由でロボットに指令する構成となっています．

Quest 3側の実装

Quest 3側のソフトウェアはUnity上に構築されており，以下の画像のような構成になっています．

それぞれのコンポーネントの役割は以下の通りです．

UIManager
ユーザーのボタン操作（接続・リセット・モード切替）を受け取り，GlobalVariablesの状態を変更します
Communicator
GlobalVariablesの状態変化を監視し，UI操作に応じてPCにコマンドを送信します．また，トラッキング中はArmControllerから関節角度を取得し，PCへ送信します
ArmController
GlobalVariablesの状態や入力に応じて，アームのIK計算・動作を制御します
GlobalVariables
全体の状態（接続・リセット・モード・トラッキング）を静的に管理し，各クラスが参照・操作します

全体的な流れとしては，Quest 3のコントローラのPoseと，仮想的なアームのエンドエフェクタのPoseが一致するように逆運動学を解き，その角度をPCにUDP通信で送信するようになっています．

こちらはロボット操作時の，Quest 3内部から見た映像です．

逆運動学 (Inverse Kinematics)

今回の実装において，コントローラのPose（手先の目標位置・姿勢）から各関節の角度を求める逆運動学（IK）は最も重要な要素の一つです．

解析解 vs 数値解

IKを解くアプローチには大きく分けて数値的解法（Jacobianの逆行列などを用いる）と解析的解法（幾何学的な数式で直接解く）の2通りがあります．
今回作成したアームは一般的な6軸構成であり，冗長自由度がありません．そのため，幾何学的に一意（あるいは少数の解の候補）に関節角度を決定することが可能です．
Quest 3はスタンドアローン機であり，PCに比べて計算リソースに限りがあります．Unity上で毎フレーム数値計算のループを回すコストを避けるため，今回は解析解を採用しました．具体的には，手首位置（=コントローラ位置）をQuest 3の開発SDKから取得し，そこから各関節角度を三角関数を用いて導出しています．

特異姿勢の回避

解析解の採用により計算は高速化しましたが，実機テストにおいてジンバルロック（特異姿勢）の問題が顕在化しました．特に，腕周辺の関節軸が一直線に並ぶ特異姿勢では，計算上の解が不安定になり，シミュレーション上のエンドエフェクタが激しく回転する現象が発生します．
そのような動きをそのままロボットに反映させてしまうと非常に危険ですので，これを回避するために特異点回避アルゴリズムを実装しました．
具体的には，手先の目標姿勢と，アームの主要な関節（Joint 2）の方向ベクトルの内積を監視します．内積の値が0に近づいたら特異点に近づいたと判定し，目標姿勢を強制的に回転させ，紡錘状に軌道を修正することで特異点を回避しています．

以下は、その回避ロジックの抜粋です．

// 目標回転方向とjoint[2]の方向の内積を計算
float dotProduct = Quaternion.Dot(q, joint2Rotation);
float absDot = Mathf.Abs(dotProduct);
// 内積が閾値(targetDiff)を超えた場合、特異点に近いと判断
if (absDot > targetDiff)
{
    // 回転軸を算出し、姿勢qを強制的に補正する
    Vector3 rotationAxis = Vector3.Cross(forwardJoint2, forwardQ).normalized;
    // 現在の角度と目標の回避角度の差分を計算
    float angleToRotate = targetAngleDeg - currentAngleDeg;
    // 補正回転を適用
    Quaternion correctionRotation = Quaternion.AngleAxis(angleToRotate, rotationAxis);
    q = correctionRotation * q;
}

この処理により，ユーザーが意図せず特異姿勢になるような操作をしても，シミュレータ側で滑らかにその姿勢を回避し，動作の破綻を防ぐことが可能になりました．

通信

Quest 3 (Unity) とロボット制御PC (Python) 間の通信は，データの性質に合わせて WebSocket (TCP) と UDP を併用する構成にしました．

WebSocket

ロボットの接続管理，リセット，録画開始などの確実性が求められるコマンド送信には，TCPベースのWebSocketを使用しています．

UDP

一方で、可能な限り遅延を避けたい関節角度の送信にはUDPを採用しました．WebSocketのオーバーヘッドを避け、高い更新レートでロボットを滑らかに追従させるためです．
データパケットは合計57バイトのバイナリデータとして構成しています．

ヘッダー (1byte): 動作モードフラグ（動作中/停止中）
左手データ (28bytes): Float (4byte) × 7（関節角度6軸 + グリッパー）
右手データ (28bytes): Float (4byte) × 7

UnityのC#とPC側のPythonでエンディアンを合わせる必要がありますが，これにより最小限の通信量で高速なテレオペレーションを実現しています．

private void SendJointAngles()
{
    // 14個の関節データ + 1byteのモード
    int dataCount = 14;
    byte[] sendData = new byte[dataCount * 4 + 1]; 
    sendData[0] = 1; // Mode: 1
    // 左手の角度データをバイト列に変換してコピー
    for (int i = 0; i < 7; i++) {
        byte[] angleBytes = BitConverter.GetBytes(leftAngles[i]);
        Array.Copy(angleBytes, 0, sendData, 1 + i * 4, 4);
    }
    // 右手のデータも同様にコピー...
    udpJointSender.Send(sendData, sendData.Length);
}

PC側の実装

PC側は，システム全体を統括し，データセットを収集する役割を担います．
主な機能は以下の3点です．

通信の中継: Quest 3からの関節角度指令の受信と，ロボットへの送信
データセットの作成: カメラ画像とロボットの状態を同期して保存
安全性確保: 急激な動作を防ぐフィルタリング処理

ここでは、開発中に直面した通信の問題と，LeRobotライブラリを活用した実装の工夫について解説します．

通信の安定化

開発当初，Quest 3とPC間の通信には手軽なモバイルルーターを使用していました．しかし，ロボットを動かしてみると，動作がカクついたり，時折意図しない急動作を起こしたりする事象が頻発しました．
原因を調査するために，PC側で受信した関節角度の時系列データをプロットしてみたところ，以下のグラフのようにモバイルルーター使用時にパケットロスが頻発していることが判明しました．

モバイルルータを使った際の，PCが受信した関節角度の時系列プロット（パケットロスにより階段状になっている）

このように制御信号として飛び飛びの値が送られると，モータに予期せぬ負荷がかかり，ロボットが暴走する危険性があります．

これに対し，高性能なWi-Fi 6対応ルーター（tp-link Archer AX3000）に変更したところ，パケットロスはほぼ解消され，非常に滑らかな制御が可能になりました．

tp-link Archer AX3000を使った際のプロット（滑らかに追従している）

リアルタイムなテレオペレーションにおいて，ネットワークインフラの品質はソフトウェア同様に重要であることを痛感しました．

LeRobotとの連携

本プロジェクトの最大のこだわりは，Hugging Faceが提供するロボット学習ライブラリLeRobotのエコシステムに準拠している点です．
独自形式でデータを保存すると，学習時に変換スクリプトを書く手間が発生しますが，最初からLeRobotのフォーマットに合わせておくことで，データ収集後すぐに学習を開始できます．
また，最新の模倣学習モデルやVLAモデルがLeRobot形式に準拠している事も少なくないため，いち早く自分のロボットで最新のアルゴリズムを試すことができるようになります．

1. Robotクラスの実装

LeRobotは，多様なロボットを統一的に扱うためのインターフェースを提供しています．今回はその形式に合わせてMyAlohaというクラスを作成し，以下のような仕様で実装しました．

observation_features / action_features:
ロボットが持つカメラ画像（480x640x3）や関節角度（14自由度）のデータ構造を定義します
send_action(action):
Policyやテレオペから受け取ったアクションを，実際のモーター指令値（DynamixelやRobstrideのプロトコル）に変換します
get_observation():
現在のカメラ画像と関節角度を取得し，辞書形式で返します

特に工夫した点は，安全性のためのフィルタリング処理です．VRコントローラのトラッキングが飛んだ際にロボットが暴れないよう，send_action内で以下の処理を行っています．

# 1. アンラップ処理: π→-πなど，角度の不連続性を解消
unwrapped_action = self._unwrap_angle_target(action, self.old_action)
# 2. 相対移動量の制限: 1フレームあたりの最大移動量を制限
limited_action = self._limit_relative_target(unwrapped_action, self.old_action)
# 3. 指数平滑化フィルタ: 動きを滑らかにする
self.filtered_joint_angles = (
    self.filter_alpha * limited_action +
    (1 - self.filter_alpha) * self.filtered_joint_angles
)

2. データセットの保存 (LeRobotDataset)

データの保存には LeRobotDataset クラスを使用しています．これにより，画像データの動画圧縮（H.264/AV1）や，Hugging Face Hubへのアップロードが自動的に行われます．

以下は実装コードの抜粋です．

# データセットの初期化
self.current_dataset = LeRobotDataset.create(
    repo_id=repo_id,
    fps=30,
    root=dataset_path,
    robot_type="aloha",
    features=dataset_features,
    use_videos=True, # 画像を動画として保存し容量を節約
    video_backend="pyav", 
)

# 非同期ループ内でのフレーム追加
while self.is_recording:
    # 観測とアクションを取得
    obs = self.robot.get_observation()
    
    # データセットに追加
    frame = {**observation_frame, **action_frame, "task": "do something"}
    self.current_dataset.add_frame(frame)

また，PC側のメインループは asyncio を用いて非同期で記述しており，WebSocket（Unityとの通信），UDP（関節角度受信），ロボット制御，画像保存のそれぞれの処理がブロッキングしないように設計されています．

公開リソース

ドライバ
https://github.com/KyotoVLATech/kvt_aloha_python_controller
PC側ソフトウェア
https://github.com/KyotoVLATech/lerobot-KVT
Unity側ソフトウェア
https://github.com/KyotoVLATech/AlohaController

今回開発したハードウェアのBOMやCADデータは同GitHub Organizationにて順次公開予定です．

まとめ

本記事では，低コストかつ標準機に準拠した双腕マニピュレータのデータ収集システムの構築について紹介しました．

Hardware: QDDモータとDynamixelのハイブリッド構成で，コストを抑えつつ実用的な性能を確保
Software (Unity): Quest 3のMR機能を活かし，IKによる直感的な操作を実現
Software (PC): LeRobotライブラリに完全準拠し，データ収集から学習までのパイプラインをスムーズに接続

このシステムを用いて収集したデータセットを使い，実際にACT (Action Chunking with Transformer) を学習させたところ，お菓子を掴んで箱に入れるタスクを高確率で成功させることができるようになりました．

Physical AIの分野は，ハードウェアとソフトウェアが密接に絡み合う非常にエキサイティングな領域です．従来のロボットのように，ハードウェアに合わせてソフトウェアを構築するだけでなく，ソフトウェアに合わせてハードウェアを構築する事が重要になってきていると感じます．
本記事を読んで，Physical AIに興味を持ってくれる方がいたらとても嬉しいです．