背景
人が人形になって動くことができるシステムを作っています。イメージとしては、下図のように人間がVRのヘッドマウントディスプレイを被ると、それに応じて人形の内部に仕込んだモータが稼働し、同じ姿勢を取るように動作します。また、今回は割愛しますが、人形の眼球周辺にカメラを設置し、ヘッドマウントディスプレイ上に視界を投影することで没入感を演出する実装を行うための技術調査の真っ只中です。今回は姿勢を模倣する際の課題を取り扱います。
今回課題となるのが、表題にもあるグローバル座標系からローカル座標系への変換です。Unityではアプリケーションの起動時にヘッドマウントディスプレイの位置が原点に来て、人形もその姿勢をベースに動きます。しかし、下図のように動いているうちに元の姿勢が分からず、人形がずっと前のめりになったままになってしまうといった問題が懸念されます。というか発生しました。
そこで、リセットボタンを押すと姿勢を原点に戻す実装を考えたのですが、これを実現するには、リセットボタン押下時の座標を記録し、その座標を原点としてその後の現在位置を計算する必要があります。これが表題の意味になります。また、動作を真似するという特性上、頭部の3自由度の回転軸において、Pitch（頷く方向）、Roll（首をかしげる方向）はリセットする必要がありません。Yaw（振り向く方向）及び体の前後(Unity上ではz、簡単のためyとする)及び左右（x）の3自由度において姿勢をリセットする必要があります。
図を書いていく
問題としては、下図の真ん中にある点(x1,y1,θ1)を原点として右上にある点(x2,y2,θ2)をローカル座標(x',y',θ')に変換していきます。
ここで、x',y',θ'は下図の部分を表します。
しかし、これを求める為に少し工夫が必要です。θ'=θ2 - θ1なのですが、x'及びy'は傾いているので、傾きがどのくらいか求める必要がありそうですね。というわけでやっていきましょう。
ローカル座標系を求める為に下図のように補助線を引いていきます。(x1,y1)と(x2,y2)の距離をrとすると、x' = r * cos(θ3+θ4), y' = r * sin(θ3+θ4)です。先ほどθ'は求めているので、後はθ3, θ4の角度が分かればx', y'が求まります。
まず、ぐっと睨むとθ1=θ4であることが分かります。y軸及びx軸に平行な線を引いているのでこれは自明です。次に、θ3は少し遠回りをします。直角三角形なので、tan(θ3) = ( (y2 - y1)/(x2 - x1) )です。要するに、tan-1( (y2 - y1)/(x2 - x1) ) = θ3を計算すれば角度が求まります。これで必要な変数がそろったので実装に移りましょう。
実装
上の図をそのまま式にしていきます。(xd, yd, thetad)が今回求めたい値です。実際に紙にプロットしてみると計算通りの結果が得られます（θの回転方向を先の説明とは逆方向に取っています）。
説明用にPythonで簡単な実装を書いてみましたが、実際VRアプリで動かしたときのC#コードは以下ですが、諸々の都合でPoseModelクラスを定義してそこにDegrees型及びCentiMeter型のValueObjectを定義して変数を良しなに扱っています。
余談、及び数年分の振り返り
プログラミングの経験自体は早いもので7年目に突入しました。最初の二年くらいは授業でC, Javaを扱いながらポインタ含めた計算機の挙動やオブジェクト指向について学習しており、その頃はプログラミングが何も分からない感じでした。その後は研究用途でロボットの上位レイヤのアルゴリズムや簡単な対話システムのコードを書いたり、ちょっとしたスマホアプリ実装で今のVRアプリのように加速度センサを使って遊んだりしていました。
そして、就職してからの直近一年間についてはクラウドを活用しつつWebアプリケーションを開発するのが主なタスクでした。Webアプリケーションは今までのアルゴリズムを記載したり実験コードを書くものとはかなり趣向が異なり、3層アーキ等を意識しつつ、MVC+RESTful APIみたいな構成でリクエストに対して適切なレスポンスを返す構造であったり、都度バッチ処理を組み込んであげたり、製品として成り立たせるためにセキュリティやパフォーマンスといった非機能要件を考慮したりといった点に注力していました。研修でアプリケーションを作ったりしました。
そんなこんなで、久しぶりにアルゴリズム的な処理を書いたわけですが、初心に戻った感覚というか、アーキを意識した開発よりももっとミクロな視点で、プログラミング的な楽しさがあり、こういった気持ちを忘れないでいきたいなと思いました。エンジニアとしてはビジネス面も含めて全体の流れを見ながら抽象-具体のレベルを自由に行き来できると良いかなと思いつつ、アーキテクチャの勉強をしたりクラウド技術によってローコードで様々な機能が活用できてしまうとその組み合わせで実現できるシステムを思いついてしまうのですが、こういった細部の業務ロジックがコアになる部分もあるんだな…というのは、常に意識していきたいですね。
更に余談
作業のクリティカルパスとしては今回のようなVRアプリ開発ではなく最初に載せたモデルを自前の3Dプリンタで印刷するためのパーツ分割なのですが、最近3Dプリンタの部品が故障してしまい、作業がストップしてしまったので並行でやる予定だったアプリケーション側の実装をしていた、という経緯があります。やっとこさ部品が届き再稼働できそうという状況になったところで、今度は人形のモデル及びパーツ分割を見直したくなり、プロジェクトマネジメントが多頭飼育崩壊状態で気持ちがかなり辛くなっています。
こんなことになるなら3Dプリンタも複数台購入して冗長構成を組めばよかったですね。トホホ…
