指の動きが、
もう一方の腕を動かすまで

まえがき

MetaPuppetの体験の芯は、ソフトウェアだけでは成立しない。カメラの映像やVRの描画が伝送・合成の記録なら、こちらの記録はその先、実際に参加者の体に力を及ぼす部分についてのものである。参加者は自分の指を動かす。その指の動きをVR側のハンドトラッキングが検出し、UDPで1本、Raspberry Piへ送る。Piはその値を目標位置としてPD制御でサーボを追いかけさせ、サーボの回転がドラムに巻かれたケーブルを引き、ケーブルは参加者自身のもう一方の前腕を持ち上げる。指を動かした本人が、次の瞬間には外力で動かされる側にもなる^[1]。

この記録では、参加者の指先から前腕までの力の経路を、実際に動いているコードに沿って辿る。入力側のハンドトラッキングとUDP送信はUnity(C#)で、受信・PD制御・サーボ駆動はRaspberry Pi上のPythonで実装している。

実機のフレームを正面から見たところ。クロスバー上の2台のサーボからコードが伸び、それぞれ手首のカフに繋がる。手前のノートPCはMotor Piのダッシュボードを開いて較正するためのもの

指のピンチをVR側でどう検出しているか

入力はMetaPuppetPinchUdpSenderというUnity側のコンポーネントが担っている。まずXR Handsのジョイントから親指の先(ThumbTip)と人差し指の先(IndexTip)の位置を取得し、2点の中点を「ピンチ点」、2点間の距離を「ピンチ距離」として毎フレーム計算する。ピンチ距離があらかじめ決めたしきい値(既定で2.5cm)以下になった時点を「ピンチ中」と判定する。ヘッドセットのハンドトラッキングが関節を見失った場合には、Metaのポインティング入力(MetaAim)のピンチ強度をフォールバックとして使う経路も用意してあり、ジョイントの追跡が途切れても入力そのものが即座に消えないようにしている。

ピンチが続いている間、ピンチ点が前フレームからどれだけ鉛直方向に動いたか(verticalDeltaCm)と、ピンチを開始してからの累積移動量(verticalTotalCm)を、cm単位でフレームごとに計算する。前腕を持ち上げる/下ろす動作は、この鉛直成分だけを使っている。ピンチの検出方式(関節トラッキングかMetaAimか)が途中で切り替わっても、蓄積してきたverticalTotalCm自体は引き継ぎ、原点だけを付け替える(Rebase)ようにしてあるのは、切り替わった瞬間にモーターの目標値が飛ばないようにするためである。

UnityからRaspberry PiへのUDPパケット

計算した値は、36バイトの独自パケットに詰めてUDPで送る。TCPではなくUDPを選んでいる理由は、映像伝送(通信インフラの実装で頑張ったことで扱った独自UDPプロトコル)と同じで、多少パケットが欠けても、古い値を後から届けるより「今の指の位置」を継続して送り続けることの方が、力を発生させる用途では重要になるからである。

char[4]  Magic;              // 'M','P','P','H'
byte     Version;            // = 2
byte     Hand;                // 0 = left, 1 = right
byte     MotorId;             // このパケットが駆動するサーボID
uint32   Sequence;
double   TimestampSeconds;
byte     IsPinch;
float    VerticalDeltaCm;     // 前フレームからの差分
float    VerticalTotalCm;     // ピンチ開始からの累積
float    PinchDistanceCm;
float    PinchStrength;

バージョン2でMotorIdをパケット自体に持たせているのは、どの手がどのサーボを駆動するかをPi側の固定マッピングだけに頼らず、Unity側のInspectorで(Left Hand Motor Id / Right Hand Motor Idとして)明示的に指定できるようにするためである。バージョン1のパケット(MotorIdフィールドを持たない、32バイトの旧形式)も後方互換として受け付けており、その場合はPi起動時の引数(--left-hand-servo-id / --right-hand-servo-id)がフォールバックのマッピングになる。

カメラPiとMotor Pi、2台構成にした理由

MetaPuppetの現場では、Raspberry Piを2台使っている。ステレオカメラの撮影・エンコード・独自UDP伝送を担う「カメラPi」と、この記録の主題であるサーボ駆動を担う「Motor Pi」である。カメラPi側の実装は別記事に譲るが、この2台構成にしたのは単純で、カメラのキャプチャ・エンコードと、サーボのシリアル通信・PD制御ループは、どちらも遅延に敏感な処理でありながら性質が全く別で、1台のPiに同居させるとどちらかの処理がもう片方の足を引っ張りかねないからである。

カメラPiの統合ダッシュボード(mp_control.py、ポート7000)は、Motor PiのAPIを/api/motor/*にそのままプロキシしていて、現場ではカメラPiのダッシュボード1画面から、カメラの状態とサーボの状態を両方確認できるようにしている。2台のPiはTailscale^[1]でも繋がっており、本番用の固定配線(Ethernet)と、遠隔デバッグ用のTailscaleを、同じPiの別インターフェースとして共存させている。

STSサーボと、自前で組み立てるシリアルパケット

ドラムを回しているのは、FeetechのSTSシリーズのシリアルバスサーボ2台(ID 5・6)である。ホビー向けのPWMサーボと違い、STSサーボは半二重の非同期シリアル(既定1 Mbps)1本に複数台をIDで区別してぶら下げ、位置・速度・トルクのオン/オフをレジスタ経由で読み書きできる。Motor Piは、このシリーズ向けに配布されているscservo_sdkのうち、シリアルポートを開閉するPortHandlerだけを使い、実際に流すパケットは自前で組み立てている。

0xFF 0xFF  ID  Length  Instruction  [Params...]  Checksum
// Checksum = ~(ID + Length + Instruction + Params...) の下位1バイト
// ID 0xFE はブロードキャストID。複数サーボへの同時書き込み(Sync Write, 0x83)に使う

フレーム上部を見上げたところ。3Dプリント製マウントに固定されたSTSサーボ2台から、コードが手首カフへ伸びる

起動時に各サーボへPingを送って応答を確認したあと、モード切り替えレジスタ(アドレス33)に1を書き込んで「ホイールモード」^[1]にし、トルクオン(アドレス40)を有効にする。以降、現在位置・現在速度はアドレス56から4バイトまとめて読み出し、目標位置(Sync Write、アドレス42)と回転速度(Sync Write、アドレス41)は、2台分のIDと値をまとめて1回のブロードキャストパケットで書き込む。応答を待たない書き込み(write_packet_no_response)にしているのは、位置・速度の指令は毎フレーム続くものであり、書き込みのたびにサーボからの応答を待っていると、その分だけPD制御ループの周期が遅くなるためである。

ピンチ量を目標位置に変換するPD制御

Motor Piの制御モードは、大きく分けて2つある。手動ジョグ用のrelativeモード(その場でサーボを一定量だけ動かす)と、UDPで届く指の位置を追いかけ続けるpdモードで、現場ではUnityからの入力は常にpdモードで受けている。

PDモードでは、パケットに入っている「ピンチ開始からの累積移動量(total_cm)」に、counts_per_cm(較正で決めるcmあたりのエンコーダカウント数、既定290)を掛けたものを目標カウント値とする。この目標値と、内部で保持している推定位置との差を誤差(error)とし、誤差の大きさに比例した項(Kp)と、誤差の変化率に比例した項(Kd)を足し合わせて、サーボへ送る回転速度の指令値を作る^[1]。

target = total_cm * counts_per_cm * direction
error  = target - estimated_position
speed  = clamp(Kp * error + Kd * (error - previous_error) / dt, -speed_limit, speed_limit)
// 誤差が小さいのに速度も小さければ0にする(デッドバンド)
// 0でなければ最低速度(min_command_speed)を下回らないよう底上げする(スティクション対策)
// 前回速度からの変化量を pd_accel_limit * dt でクリップする(急加速の抑制)
estimated_position += speed * dt

ここで注意が要るのは、誤差計算の基準にしている「推定位置」が、サーボから読み返した実際の位置ではなく、直前まで送り続けた速度指令を時間積分しただけの、Pi内部だけの見積もり値だという点である。つまりこのPD制御は、サーボの実位置を毎周期フィードバックする厳密な閉ループではなく、指令速度から自分の現在位置を見積もりながら目標に近づけていく、半分開ループのフォロワーになっている。シリアルバスサーボの位置読み出しは、指令の送信ほど高頻度には回せないため、この設計は精度よりも制御周期の安定を優先した結果である。デッドバンドと最低速度、加速度の上限は、狙った位置の手前でサーボがカクカクと往復し続けたり、参加者の腕に急な衝撃が伝わったりしないようにするための、実運用上の緩衝材にあたる。

現場での較正とマニュアル操作

Motor Piは起動するとFlaskのWebダッシュボードを8080番ポートで公開する。ここには、各サーボの現在位置・目標位置・速度・トルク状態を250msごとに表示するテーブルと、1 mm・10 mm・100 mm刻みでサーボを直接動かせるマニュアルジョグ、現在位置をその場で「初期位置」として登録し直すSet Initialボタン、そしてPDのKp/Kd・counts/cm・加速度上限・最低指令速度をプロセスを再起動せずにその場で書き換えられる調整パネルが並んでいる。

マニュアルジョグは、指定したmm数をcounts_per_cmで換算し、その移動に何ミリ秒かける想定かという最短時間(min-jog-ms、既定120ms)から逆算した速度でサーボを回すという、PDモードとは別の素朴な実装になっている。これは主に、参加者が装着する前にケーブルの張り具合やドラムの巻き取り位置を人手で調整するための機能で、PDモードで使っているのと同じcounts_per_cmの物理較正値を共有している。

安全設計 — 参加者の体に力を及ぼす機構であること

カメラや映像のパイプラインが壊れても、起きることは「映像が乱れる」までである。しかしこの機構は参加者の腕を実際に動かす力を出すため、壊れ方の設計をソフトウェアだけで済ませるわけにはいかない。実装の中には、性質の異なる複数の安全策が積み重なっている。

現場に置かれた物理的な非常停止ボタン。ソフトウェア側の緊急停止(Unity側の2系統)とは独立した、最後の手段

ソフトウェア側の設計とは別に、現場には物理的な非常停止ボタンも置いている。これはUnity側やMotor Pi側のソフトウェアを経由せず、電源系統に直接介入するためのもので、ソフトウェアがどのような状態に陥っていても機構を止められるという意味で、これから説明する2系統のソフトウェア的な緊急停止よりもさらに一段階、独立した最後の手段にあたる。

Unity側にも、参加者・スタッフのどちらからでも独立に止められる2系統の緊急停止を用意している。1つはスタッフ用で、キーボードのEscapeキーを押すと即座に発動する。もう1つは装着している本人用で、両手を同時に5秒間ピンチし続けるというジェスチャーで発動する。この5秒という長さは、プレイ中に偶発的に両手が同時にピンチしてしまっても誤発動しない程度に長く、かつ本人がスタッフの反応を待たずに自分の意思だけで止められる程度に短い、という両立を狙った値である。

どちらの経路で発動しても、シーン内に存在するすべてのMetaPuppetPinchUdpSender(および関連するワイヤー駆動コンポーネント)に対して即座に停止パケットが送られ、以後は駆動用の実パケットではなく安全用の空パケットだけを送り続ける状態に切り替わる。この停止状態はプロセスの生存期間中ラッチされ、シーンをリロードしただけでは解除されない仕組みになっており、HMD内には赤字で警告テキストが表示され続ける。安全のために止めた状態を、うっかり操作で元に戻せてしまっては意味がないという判断による設計である。

PD制御側のデッドバンドと加速度上限(第5章)も、この安全設計の一部として働く。目標値の手前でサーボが微振動し続けることも、目標が急に変わったときに腕へ急な衝撃が伝わることも、どちらも参加者の体に直接触れる機構である以上、映像のコマ落ちとは重みが違う失敗である。

あとがき

指のピンチ量をUDPパケットに詰め、PD制御でサーボの回転速度に変換し、ドラムに巻かれたケーブルで前腕を持ち上げる。文章にすればこれだけの経路だが、そのどの区間にも、参加者の体に直接力が及ぶという前提から逆算された制約が入り込んでいる。目標値の追い方(PD制御であって単純な位置指令ではない理由)、駆動の起動条件(既定オフのUDP)、止め方(2系統の独立した緊急停止とラッチ)は、そのすべてが「動く」ことより先に「安全に止められる」ことを設計の起点に置いている。

このハードウェアが実現しようとしている問い — 自分の指で始めた動きが、自分自身の体の別の部分を外力で動かす経験は何をもたらすのか — についての実験的な枠組みは、別稿のポスター草稿にまとめている。この記録は、その問いを実際に会場で成立させるために書いた、地味な配線とパケットとゲインの記録である。