PepperのALRedBallDetectionでモーションキャプチャまがいの実験

こんにちは。ゲーム事業の開発支援をしているF.S.です。最近、EC事業の支援も始めました。
先日AppleWatchを購入してWatchネタでも行きたいところではありますが、今回もPepperネタでひとつ行きたいと思います。

ここしばらくはPepperの対話力を向上させようと音声認識エンジンを粛々と取り組んでいたのですが、そんな中、なぜかふと頭をよぎったのが、、、

これ。

今年もきっとやるんだろうなぁ。そしたらPepperにも踊って欲しいよなぁ。。

でも、ポーズとってタイムラインのせて動かすのって、すっごく面倒だよなぁ。。。

なんか、人の動きをまねて簡単に動いてくれないかなぁ、ということで、モーションをキャプチャする方法を悶々と考えていました。

• kinect等のキャプチャ機材
  →　環境揃えるだけで結構ハマりそう。
• スマホの加速度・ジャイロセンサ
  →　精度が期待できない。
• Pepperのdepthカメラ
  →　kinectと同じことができるかも？　だがkinectのように人と認識してスケルトンに落とし込んでくれないと道のりが長すぎるし、重たそう。
• Pepperの2DカメラによるRedBallDetection
  →　カメラに映ったRedBallの座標と大きさを検出してくれるので、手先の動きに変換できるかも。2D画像なので検出も速そう。片腕だけのモーションしか期待できないが、リアルタイムで実機を動かすこともできそう。

ということで、RedBallDetectionを試してみることにました。
検出したRedBallの位置をなぞってPepperの腕を動かしてみようと思います。

前々回（Pepperのタブレットを活用する）はロボットと言っても結局コンテンツ勝負、というようなことを書きましたが、一転してロボティクスな感じがしてまいりました。

この実験のサンプルデータおよび実行コードはGitHubにあげてますので、参考ください。
https://github.com/gmo-satof/pepper-redballdetection

実験

RedBallの検出

まずはRedBallを調達。最初は真面目に買ってこようと思っちゃいましたが、スマホで赤丸画像を検索し、表示して見せればOK。（スマホ画面の映り込みには注意）
ALMemoryからredBallDetectedイベントを取得してログに出力するだけなのですが、下記ダイアグラムフローのように並行してビデオ録画しておくとPepperがどのように見ているかが後で確認できて良いです。今回は10秒記録して処理を停止するようにしています。

RedBallDetectionは額にある2Dカメラが使用されるので、ビデオ録画のカメラも「Top」を選択してください。
なお、デフォルトのRecord Videoボックスにはバグがあってエラーが出るのですが、エラーメッセージに従ってこのように修正すればOKです。

この部分の

    def onUnload(self):
        #~ puts code for box cleanup here
        if( videoRecorder and self.videoRecorder.isRecording() ):
            self.videoRecorder.stopRecording()

videoRecorder を self.videoRecorder に修正

    def onUnload(self):
        #~ puts code for box cleanup here
        if( self.videoRecorder and self.videoRecorder.isRecording() ):
            self.videoRecorder.stopRecording()

それでは検出してみましょう。

こちらがRedBall検出中にPepperが見ている映像です。

検出結果をプロットすると、このような感じです。

1検出あたりのサンプルデータはこのようになっています。

[
  # TimeStamp (Seconds, Microseconds)
  [1430094858, 705281],

  # BallInfo (centerX, centerY, sizeX, sizeY)
  [0.24958209693431854, -0.2528946101665497, 0.04835652932524681, 0.04993459954857826], 

  # CameraPose_InTorsoFrame (Point6D)
  [0.02125110849738121, -0.00018178121536038816, 0.34490036964416504, -5.820766091346741e-11, -0.16260194778442383, -0.0030679700430482626],

  # CameraPose_InRobotFrame (Point6D)
  [0.052272260189056396, -0.002408697037026286, 1.1627302169799805, 0.004445356782525778, -0.10582906007766724, -0.003793689887970686],

  # Camera_id
  0
]

redBallDetectedイベントにより得られるデータフォーマットはこちらのドキュメントを参照ください。

Aldebaran documentation – ALRedBallDetection
http://doc.aldebaran.com/2-1/naoqi/vision/alredballdetection.html#alredballdetection

このドキュメントでは

centerX and centerY are the angular coordinates of the center of the ball in angles (radians).
<中略>
The origin of the angles is the middle of the image. centerX corresponds to the direct (counter-clockwise) rotation along the Z axis, and centerY corresponds to the direct rotation along the Y axis, as in the image below...

と記述されているのですが、必死になって角座標(angular coordinates)計算を試みるもまったくもって難解な結果となり、とんでもなくハマってしまいました。結局、上図座標のように、ただ単にcenterX, centerYをデカルト座標にプロットするだけでそれらしい動き（というかそのまんま？）になっているので、デカルト座標を元に計算を進めます。

データに入っているカメラのポーズについては、検出時に正面を向いていることを前提として今回は無視します。

なお、ログに流れるデータのタイムスタンプを見ると、だいたい15〜20fpsで検出結果が得られているようでした。

モーション基礎知識

Pepperの腕のジョイントはこのようになっています。

• 肩（Roll、Pitch）
• 肘（Roll、Yaw）
• 手首（Yaw）
• 手（開閉）

ジョイントの可動方向によって、角度、回転速度の制約も異なります。
可動角度はロボットビューで確認してもらえば良いと思いますが、回転速度は実行時にエラーが出て初めて制約がわかります。
例えば、下記のように肩のPitchは最大回転速度が 7.3398(rad/s)であることがわかります。

RuntimeError: 	ALMotion::angleInterpolation
		ALMotion::angleInterpolation
	Body Max Velocity is not respected. RShoulderPitch: 
            Max velocity  : +7.33998 rad.s-1
            Given velocity: +9.95285 rad.s-1

実は以前Perfumeの公開モーションデータを使ってPepperを動かそうと試みたのですが、Pepperにとっては動きが速すぎて上記の制約に引っかかりまくってしまいました。AKBなら比較的ゆるめなので大丈夫だと信じています。

ジョイントを時系列にしたがって回転させるには、ALMotionのAPIを使用します。
ジョイントの可動種類、回転角(ラジアン)、経過時間(秒)をリストにして一気に再生する感じです。

例）

motionProxy = ALProxy("ALMotion")

motionProxy.angleInterpolation(
    # ジョイントの可動種類
    ["RShoulderPitch","RShoulderRoll"],

    # 回転角 
    [[0.0, -0.1, -0.2], [0.5, 0.7, 0.6]],

    # 経過時間（ジョイントごとに異なっていてもOK）
    [[1.0, 1.5, 2.0], [1.0, 1.5, 2.0]]
)

ジョイント制御に関するドキュメントはこちらですね。

Aldebaran documentation – Joint control
http://doc.aldebaran.com/2-1/naoqi/motion/control-joint.html#control-joint

ジョイントの動かし方が把握できたところで、実際にジョイントを回転させる角度を算出していきます。

腕を動かす

まずは腕をまっすぐに伸ばした状態で検出したRedBallの方向に腕を向けることを試してみます。
厳密にはカメラ位置から肩に原点を移動する必要があるのですが、簡単にするために原点をそのまま肩の位置とします。

空間座標における原点からRedBallの方向を算出し、肩ジョイントのPitch, Rollを求めます。
z値についてはRedBallの半径であるsizeXかsizeYのどちらかの値に適当な係数を掛けたものを使用します。

上図より、肩のPitch, Rollの算出式を出しました。

これで算出したShoulderPitch, ShoulderRollを使って時系列にしたがって動かした結果がこちらです。

なお、原点からRedBallまでの距離dは下記のように算出できます。この距離dは後ほど使用します。

肘の動きを加える

腕が無事に動いたことはよかったのですが、腕が伸びっぱなしだと動きとしては不自然ですね。
RedBallの位置を手先とすると原点からの距離によっては肘をたたむことになるのですが、肘の位置はキャプチャできていないので、肘の取りうる座標が腕の方向を中心軸とした円上のどこにでも存在できることになります。
したがって、下記の前提条件を設けました。

• 肘の座標は空間座標における原点からRedBallまでの線分を含む垂直平面P上に存在するものとする
• 肘の内角が上方向に開く（肘の内側が上を向く）ようにする

前提条件をもとに、肘の位置を算出していきます。

• 空間座標s1(x1, y1, z1)
• 曲げ角度(ElbowRoll)
• 回転角度(ElbowYaw)

平面Pをデカルト座標PX, PY（便宜上、上下を反転）として表したのがこちらの図です。

p2(px2,py2)の座標はこのように計算できます。

p2の座標が出たので、2関節ロボットアームの逆運動学より肘の曲げ角度、および平面P上の座標を算出します。

このq2が肘の曲げ角度(ElbowRoll)になります。

肩ジョイントの角度を計算するため、P平面上の肘座標を空間座標s1(x1,y1,z1)に変換します。

肘を曲げた場合の ShoulderPitch, ShoulderRoll の計算は、腕が伸びている場合の計算式にてRedBallの座標s(x,y,z)を肘の座標s1(x1,y1,z1)に置き換えて計算するだけですね。

最後に、肘の内側が上を向くように肘の回転角度(Yaw)を調整します。
ElbowYawは肘の内側が体のほうを向いている角度が0になっており、上を向くように下記のように算出します。

これで、肩（Roll、Pitch）肘（Roll、Yaw）の全ての計算式が揃いました。

実際には腕の長さや肘の曲げ角度の限界があるので、RedBallまでの距離dの範囲を定め、範囲外の場合は距離dおよびzを補正します。
腕の曲げ角度は簡単のために90°を上限として算出するとして、補正した d および z は下記のようになります。

では、Pepperがどのように動くか見てみましょう。

ちょっと肩のRollが想定より開いている気がしますが、まずまずな感じです。

実機はどうでしょう？

なんとなくそれらしい動きが再現できていると思います。

考察

単純な動きを見ている限りでは、それなりに使えるかと思います。
実際にこのブログ書いている最中に急遽の応対でモーションの要求があったのですが、（運良く片手の動きで十分だったので）これを使って記録し、Sayと並行して再生することでカスタムAnimation Sayをお手軽に実現しました。

しかし、AKBにはまだ遠いかなぁと思いますので、関連して下記を検証しておきたいと思います。

• リアルタイム動作（トラッキング的な）
  →　タブレットに検出位置をリアルタイム描画できると嬉しい
• RedBallの半径の差がどれくらい検出できるか？
• RedBallが複数あるとどのように検出結果が出てくるか？
• X=0をまたぐ場合は上腕が内側に動かせないため前提としている肘が垂直平面にある限りは動かすことができない。その解決法を検討する
  →　肘の位置を何かしらで特定させる方法はないか？

それではまた。

次世代システム研究室では、アプリケーション開発や設計を行うアーキテクトを募集しています。アプリケーション開発者の方、次世代システム研究室にご興味を持って頂ける方がいらっしゃいましたら、ぜひ 募集職種一覧 からご応募をお願いします。

皆さんのご応募をお待ちしています。