モーションセンサーmocopiは、慣性計測装置(IMU)の夢を見るか？

この記事はクラスター Advent Calendar 2023の11日目の記事です。
クラスター株式会社のメタバース研究所でリサーチエンジニアをやっているsatomiです。
昨日は@kako_vailさんのクラスター社で1年と少し働いてみて。でした。
私もクラスター社にJoinして1年半しか経っていないので、とても共感する内容でした。
さて、この記事ではSonyが発売したモバイルモーションキャプチャーmocopiになぜ異常な愛情を寄せ、Unityの中で直方体をクルクルと回してみたのか？について語りたいと思っています。

（注意、本記事の内容は私個人のチャレンジで行った事であり、mocopiの製造元のSonyには一切の関係も無い事を予め表明しておきます。）

記事の想定読者

• mocopiってIMUっぽいですよね、と妄想するエンジニア。

この記事で説明しないこと

• mocopiを使ったモーションキャプチャーの仕方。

準備する物

ハードウェア

1. mocopi
2. MacBook Air/Pro
3. iPhone and/or Android端末

ソフトウェア

1. Unity
2. Python3

mocopiの通信方法は？

mocopiを使っているとiOSアプリと個別に接続している様子や、常識を働かすことによってBLE (Bluetooth Low Energy)を通信方法をして利用していることが分かります。
なので、まずはBLEのお勉強しましょう。このO'ReillyのBLE本がお勧めです。

mocopiのデバイスを特定してみよう！

BLEで通信している事が分かれば、次に6個あるmocopiセンサーがそれぞれ独立したUUIDを持っているはずなので、これを調査してみましょう。
BLEデバイスは、BLEのAdvertiseを接続する前に行っているので、この情報を調べる必要が有ります。
ちょうど使いやすそうなライブラリであるBleakという物があるのでこれを使ってみます。
Bleakを pip install break でインストールした後、次のような簡単なプログラムを実行してみます。

import asyncio

from bleak import discover

async def run():
    devices = await discover()
    for d in devices:
        print(d)

loop = asyncio.get_event_loop()
loop.run_until_complete(run())

するとどうでしょう、あなたの周りに存在するBLEデバイスのUUID一覧が包み隠さず表示されます(個人情報の為、UUIDは秘匿しました)。

(base) satomi@MacBookPro14 mocopi % python discovery.py
/Users/satomi/work/mocopi/discovery.py:6: FutureWarning: The discover function will removed in a future version, use BleakScanner.discover instead.
  devices = await discover()
********-****-****-****-************: None
9FB38B4A-37E1-ABC4-EBFD-F2F4FAD37FCF: QM-SS1 00F22
********-****-****-****-************: None
********-****-****-****-************: iPadPro12.9M1
********-****-****-****-************: None
********-****-****-****-************: Pokemon GO Plus +
********-****-****-****-************: satomiさんのApple Watch
********-****-****-****-************: iPhone6_Plus
********-****-****-****-************: None
********-****-****-****-************: iPhone 7 Plus
********-****-****-****-************: None
********-****-****-****-************: iPad Pro 10.5”
********-****-****-****-************: None
********-****-****-****-************: None
********-****-****-****-************: satomiのAirPods Pro
********-****-****-****-************: iPhone 15 Pro
********-****-****-****-************: None
********-****-****-****-************: None
********-****-****-****-************: None
********-****-****-****-************: None

お手持ちのmocopiセンサーの裏を見ると16進数5桁の固有番号が記載されています。（私のHEADセンサーでは 00F22 でした)
この番号と上記の一覧を突き合わせると当該mocopiセンサーのUUIDを特定する事が可能になります。

mocopiのBLE GATTを見てみよう

UUIDが分かれば接続する事は容易です。
なので、手持ちのiPhone(or Android)端末に徐ろにBLE Inspection用のお好きなアプリをインストールします。私は、iOS用のBLExplrを利用してmocopiを覗いてみました。

すると、こんな感じでBLEのGATT(汎用アトリビュート・プロファイル)を確認することが可能です。

なんとも怪しいもとい複数のGATTが並んでいますが、mocopiがIMUとしての情報を通知しているはず、という推論からNotifyを持ったプロパティを心眼を使って見つけ出します。
そして、そのプロパティに対してNotifyをSubscribeしてあげれば、情報が得られそうな感じがして来ましたよね。

ということで、またBleakを使ってPythonのプログラムを書いてみます。

import numpy as np
import asyncio
from bleak import BleakClient

address = "9fb38b4a-37e1-abc4-ebfd-f2f4fad37fcf"
NUS_RX = "0000ff01-0000-1000-8000-00805f9b34fb"
NUS_TX = "25047e64-657c-4856-afcf-e315048a965b"

def notif_hndlr(_, data):
    print(' '.join('{:02x}'.format(x) for x in data))

async def main(address):
    async with BleakClient(address) as client:
        await client.start_notify(NUS_TX, notif_hndlr)

        await asyncio.sleep(1)
        await client.write_gatt_char(NUS_RX, b".....", True)

        await asyncio.sleep(600)
        await client.stop_notify(NUS_TX)

if __name__ == "__main__":
    asyncio.run(main(address))

これをお手持ちのmocopiセンサーで試すと...

(base) satomi@MacBookPro14 mocopi % python dump.py
30 41 78 38 da 4c 23 0d 72 16 46 e9 ff ff 9d ff 68 16 3d e9 04 00 97 ff 1a 25 7d b2 7e 33 ef a9 fe b6 33 ae
30 6e a9 39 da 4c 23 0d 6b 16 3f e9 00 00 9c ff 68 16 3d e9 ff ff 9c ff 11 24 81 af bc b4 df 20 9a b3 39 b3
30 9b da 3a da 4c 23 0d 6c 16 41 e9 f7 ff a5 ff 5d 16 32 e9 f2 ff aa ff bf 29 16 b3 36 b4 04 2c c8 b3 53 ab
30 c8 0b 3c da 4c 23 0d 5a 16 2f e9 ee ff ae ff 5f 16 33 e9 fd ff 9f ff 9d 06 f9 20 64 b1 f0 af b6 b3 82 b3
30 f5 3c 3d da 4c 23 0d 69 16 3d e9 f6 ff a6 ff 6a 16 3f e9 03 00 98 ff 00 29 a2 b3 19 b4 c2 b0 b1 b5 b8 b3
30 22 6e 3e da 4c 23 0d 67 16 3c e9 f8 ff a4 ff 63 16 37 e9 03 00 99 ff 80 30 ee b3 a2 b2 57 ae aa b5 1f b3
30 4f 9f 3f da 4c 23 0d 67 16 3c e9 00 00 9c ff 6b 16 3f e9 01 00 9b ff 00 27 f7 b0 5a b4 5d 24 9a b4 12 b4
30 7c d0 40 da 4c 23 0d 69 16 3d e9 fb ff a2 ff 67 16 3b e9 ff ff 9e ff c8 2e 84 ad 17 b3 5b aa 6a b3 fb b1
30 a9 01 42 da 4c 23 0d 5d 16 31 e9 f7 ff a6 ff 60 16 35 e9 fe ff 9f ff 1b 32 90 ac b3 13 4a aa 36 a8 07 b4
30 d6 32 43 da 4c 23 0d 67 16 3b e9 fa ff a4 ff 6b 16 3f e9 fd ff a1 ff 0a 27 08 a9 0e b4 f3 a6 7f b2 03 b4
30 03 64 44 da 4c 23 0d 6a 16 3e e9 f9 ff a4 ff 67 16 3c e9 03 00 9a ff 50 9a 25 ab b1 b0 75 ae b7 b5 7f b2
30 30 95 45 da 4c 23 0d 68 16 3d e9 fc ff a1 ff 67 16 3c e9 fe ff a0 ff 28 2a 8d b3 0b b4 f4 a5 d4 b4 8d b3
30 5d c6 46 da 4c 23 0d 65 16 39 e9 fa ff a3 ff 65 16 39 e9 01 00 9c ff 86 29 bc b4 72 b3 2d a9 1d b0 61 b3
30 8a f7 47 da 4c 23 0d 60 16 34 e9 f8 ff a5 ff 53 16 28 e9 f8 ff a6 ff b1 30 45 ac b3 b1 1d 99 fa 2a 1c 93
30 b7 28 49 da 4c 23 0d 5f 16 33 e9 f1 ff ac ff 6c 16 41 e9 ff ff 9f ff dc 2a 02 26 5f b5 a9 af a8 b0 33 b5
30 e4 59 4a da 4c 23 0d 6f 16 43 e9 fe ff a0 ff 72 16 46 e9 00 00 9e ff a2 28 fd b5 d3 b2 45 22 e7 b0 f0 b1
30 11 8b 4b da 4c 23 0d 6d 16 42 e9 02 00 9b ff 73 16 47 e9 01 00 9d ff a7 25 a4 b0 dd ae 77 24 29 b5 d6 b3
30 3e bc 4c da 4c 23 0d 79 16 4d e9 03 00 9b ff 70 16 44 e9 06 00 98 ff c5 23 26 b8 6d b4 7e ac a6 b5 50 af
30 6b ed 4d da 4c 23 0d 66 16 3b e9 02 00 9c ff 68 16 3c e9 00 00 9f ff 27 29 fa b7 ab b0 f5 a2 51 b2 69 b2
30 98 1e 4f da 4c 23 0d 66 16 3a e9 01 00 9d ff 63 16 38 e9 00 00 9f ff be a2 10 b2 11 af 81 32 5d ad cf b5
30 c5 4f 50 da 4c 23 0d 58 16 2c e9 f7 ff a8 ff 58 16 2d e9 fa ff a5 ff 1b 34 76 2e fc 2a 98 2d 48 2a 47 2d
30 f2 80 51 da 4c 23 0d 5c 16 30 e9 fb ff a4 ff 5f 16 34 e9 fd ff a2 ff e6 31 41 ad ef b9 49 b4 46 b7 eb b8
30 1f b2 52 da 4c 23 0d 62 16 37 e9 ff ff a1 ff 66 16 3b e9 08 00 96 ff a9 32 58 92 a4 b9 7e b4 0f b8 c6 b7
30 4c e3 53 da 4c 23 0d 75 16 49 e9 03 00 9c ff 78 16 4d e9 0f 00 91 ff d8 2f 68 b5 76 b6 c0 ad c6 b8 18 b3
30 79 14 55 da 4c 23 0d 6b 16 40 e9 08 00 97 ff 66 16 3a e9 04 00 9b ff 8f 28 97 b6 5b ae 47 b2 e6 b5 d4 af
30 a6 45 56 da 4c 23 0d 63 16 38 e9 02 00 9d ff 60 16 34 e9 01 00 9f ff ff 2f 2e b1 78 b8 b0 2d 73 b4 96 b3
30 d3 76 57 da 4c 23 0d 55 16 2a e9 f6 ff aa ff 54 16 29 e9 f7 ff a9 ff 62 38 e7 37 c7 31 30 20 04 21 8b ae
30 00 a8 58 da 4c 23 0d 58 16 2c e9 f9 ff a7 ff 5b 16 30 e9 fb ff a5 ff 05 32 57 31 32 bc 21 b6 c6 b6 cc b8
30 2d d9 59 da 4c 23 0d 5e 16 33 e9 fd ff a4 ff 6a 16 3e e9 0c 00 94 ff 3d 31 14 30 15 bb 17 b7 46 ba 27 b8
30 5a 0a 5b da 4c 23 0d 7d 16 51 e9 00 00 a1 ff 6f 16 44 e9 0c 00 94 ff 95 31 99 b4 5f b8 c6 b2 27 ba 0e ab
30 87 3b 5c da 4c 23 0d 6a 16 3f e9 05 00 9b ff 67 16 3c e9 03 00 9d ff 8b 31 d8 b4 14 b4 0b b0 6b b5 a1 b2
30 b4 6c 5d da 4c 23 0d 64 16 38 e9 01 00 9f ff 5c 16 31 e9 fd ff a4 ff 66 32 14 b2 94 b2 13 36 61 33 0b 36
30 e1 9d 5e da 4c 23 0d 4c 16 21 e9 fa ff a7 ff 50 16 25 e9 fb ff a5 ff b6 32 16 31 d7 34 b8 30 3e 2d 0c bc
30 0e cf 5f da 4c 23 0d 54 16 29 e9 fe ff a3 ff 59 16 2d e9 00 00 a1 ff 55 b0 ba b3 e9 ba 77 aa f6 b1 30 bb
30 3b 00 61 da 4c 23 0d 5c 16 31 e9 01 00 9f ff 72 16 47 e9 ff ff a3 ff b7 ba 7b bb d0 bc 74 32 21 b4 df bc
30 68 31 62 da 4c 23 0d 75 16 4a e9 05 00 9c ff 71 16 46 e9 02 00 9f ff 8f b8 b0 bb 20 b4 0c 1e d8 b2 eb 2c
30 95 62 63 da 4c 23 0d 6c 16 41 e9 00 00 a1 ff 67 16 3c e9 ff ff a3 ff 7e ae 8b b6 31 28 2d 38 53 33 90 39
30 c2 93 64 da 4c 23 0d 59 16 2d e9 fd ff a5 ff 5e 16 32 e9 fe ff a4 ff 79 35 3d 35 c7 3d 4e 35 3f 31 28 b7
30 ef c4 65 da 4c 23 0d 62 16 36 e9 00 00 a1 ff 67 16 3b e9 02 00 9f ff 17 ac 7a b7 67 b9 3f ae 92 b7 39 b8

といった16進数のバイナリデータが受信出来る事が分かります。

あとは、mocopiを揺らしながらこのデータの変化をひたすらに眺めていると、なんとなくデータフォーマットが頭の中に湧いてくるものと思います。

そこで、データフォーマットが何となく分かってきたら徐ろにデータ部分の解析を始めます。

私の見立てでは、

def notif_hndlr(_, data):
    ts=data[7] << 56 | data[6] << 48 | data[5] << 40 | data[4] << 32 | data[3] << 24 | data[2] << 16 | data[1] << 8 | data[0]
    ts*=1e-9
    v = np.array([np.int16(data[9] << 8 | data[8]), np.int16(data[11] << 8 | data[10]), np.int16(data[13] << 8 | data[12]), np.int16(data[15] << 8 | data[14])])
    nv = v / np.linalg.norm(v)
    print("%f,%f,%f,%f" % (nv[0], nv[1], nv[2], nv[3]))

こんな感じで、時間情報と回転の情報が取れそうな感じでした。（残りの部分は、ちょっと判明出来ず、何方かの解析を待ちたいと思っています）

ここで、回転に関する情報が4つの浮動小数点値で表現されていることに注目します。そう、これはみんな大好き四元数が使われているに違いありません！

実際にmocopiセンサーを振りつつ動作を確認してみると...

(base) satomi@MacBookPro14 mocopi % python ble_sample.py
0.424343,-0.811877,-0.000245,-0.400986
0.425678,-0.810879,-0.001467,-0.401587
0.419076,-0.814306,-0.002690,-0.401589
0.415914,-0.815196,-0.001834,-0.403073
0.415672,-0.814957,-0.000734,-0.403810
0.415429,-0.814837,0.000489,-0.404300
0.417399,-0.813763,0.000245,-0.404435

何となく良さげに四元数表現が取れていそうです。

Unityで直方体を回してみよう！

ここまで来ればもうあと一歩で直方体を回すことが出来ます。

本来は、Unityの方でBLEをハンドリングすべきで有る所なのですが、今回はより簡易に実装するために、Pythonプログラムとの共同作業で実装しました。

大凡のC#スクリプトは、こんな感じになります。

public void HeavyMethod()
    {
        pr = new Process();

        pr.StartInfo.FileName = "/Users/satomi/opt/anaconda3/bin/python";
        pr.StartInfo.Arguments = "-u /Users/satomi/work/mocopi/ble_sample.py";

        // コンソール画面を表示させない
        pr.StartInfo.CreateNoWindow = true;

        // 非同期実行に必要
        pr.StartInfo.UseShellExecute = false;
        pr.StartInfo.RedirectStandardOutput = true;

        // イベントハンドラ登録（標準出力時）
        pr.OutputDataReceived += process_DataReceived;

        // イベントハンドラ登録（プロセス終了時）
        pr.EnableRaisingEvents = true;
        pr.Exited += onExited;

        pr.Start();
        pr.BeginOutputReadLine(); //非同期で標準出力読み取り
    }

    public void process_DataReceived(object sender, DataReceivedEventArgs e)
    {
        string output = e.Data;
        
        MainThread.Post(__ =>
        {
            var stArray = output.Split(',');
            float d0 = float.Parse(stArray[1]);
            float d1 = -float.Parse(stArray[2]);
            float d2 = -float.Parse(stArray[3]);
            float d3 = float.Parse(stArray[0]);
            this.transform.rotation = new Quaternion(d0, d1, d2, d3);
        }, null);
    }

結果です。

おわりに

この記事では、なぜmocopiに異常な愛情を寄せ、慣性計測装置(IMU)として使ってみる（正確には、ジャイロですが）事にチャレンジしたかの軌跡を綴ってみました。

明日は@yooozさんの自分用のブックマークリストを作って育ててみるです。私自身はブックマークをFirefoxに集約させていますが、人それぞれ色々な管理方法が有るんですね。とても興味深いです。