QUMARIONを使ってPepperを操縦する

タイトルの通りです。ソース公開を含みます。

概要と動画の紹介
再現手順について
まとめとか

概要と動画の紹介

QUMARIONというのは姿勢や動きを取れる人型入力デバイスです。普通はCGモデルの操作に使うのですが、今回は「人型」かつ「関節のデータを取れる」という特長を生かしてPepperの操縦をやってみました。その動画がコチラです。

ニコニコ動画のアカウント持ってない方のために動画付きツイートも貼っておきます。

https://twitter.com/baku_dreameater/status/678347488730812416

再現手順

用意するもの

以下の通りです。

お金がかかりそうなもの

Windows PC(7以上を推奨)
QUMARION
Pepper(私はアトリエ秋葉原のを予約して使ってるのでお金は払ってません)

無償で入手可能なもの

Visual Studio 2015(無償のCommunity版でOK)
Python 2.7系
Python NAOqi SDK(インストール方法は下記参照)
QUMARION SDK(こちらのページで規約に同意の上ダウンロードしてください)
QUMARION SDK .NETラッパー(前回の記事で紹介しています)

Python NAOqi SDKについてはこちらの開発者向けサイトでアカウント作成を行ってからログインし、ソフトウェアダウンロードのページでWindows用の32bit SDK インストーラ("Python 2.7 SDK 2.4.2 Win 32 Setup"みたいな名前のやつ)を拾ってきて実行すればインストールできます。当然ですがPythonのダウンロードを先に行ってください。

またPythonのインストール後にはシステム環境変数のPath変数にPython インタープリターへのパス("C:\Python27"とかそういうの)を通しておく方が無難です。

実行手順

まず前回の記事を参考に"QumarionDotNet"というレポジトリをローカル環境に持ってきてビルドやらなんやらを行い、"QumarionDataViewer.exe"というGUIを実行できるところまで済ませて下さい。

次にPython スクリプトとして以下の2ファイルを同じディレクトリ(デスクトップとか)に配置します。スクリプトの文字コードはどちらもUTF-8にします。一つ目の"quma2pepper.py"についてはスクリプト内にある"pepperip"という変数を実際に操縦するPepperのIPアドレスに書き換えてください。

quma2pepper.py

# -*- coding:utf-8 -*-

import struct
import socket
from contextlib import closing

import quma2pepper_sender

#動かしたいPepperのIPアドレス
pepperip = "192.168.xxx.xxx"
#動かしたいPepperのポート(普通9559でOK)
pepperport = 9559

#QumarionクライアントがエンドポイントにしているIPアドレスとポート
udpip = '127.0.0.1'
udpport = 13000

def main():
    bufsize = 4096

    #Qumarionクライアントが送って来るデータ(単精度小数)の個数
    datasize = 32
    unpacker = struct.Struct("f" * datasize)

    quma2pepper_sender.initialize(pepperip, pepperport)

    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    with closing(sock):
        sock.bind((udpip, udpport))
        while True:
            msg = sock.recv(bufsize)
            try:
                data = unpacker.unpack(msg)
                print("received binary")
                quma2pepper_sender.setAngles(data)
            except struct.error:
                print("failed:not binary data")
                print(msg.decode('utf-8'))
                break

if __name__ == '__main__':
    main()

quma2pepper_sender.py

# -*- coding:utf-8 -*-

import math
from threading import Thread, Lock

from naoqi import ALProxy

_deg2rad = math.pi / 180.0

#ALMotionを入れるための変数
_motion = None

#同時に主処理を行うスレッドが最大1つになるように絞るためのフラグ
_lockBusy = Lock()
_isBusy = False
def _getIsBusy():
    with _lockBusy:
        return _isBusy

def _setIsBusy(isBusy):
    global __isBusy
    with _lockBusy:
        _isBusy = isBusy

#Qumarionクライアントから飛んでくるセンサ値に順に対応づけられてるセンサー名
def _getQumaSensorNames():
    return (
        "WaistV_MY",
        "WaistV_MZ",
        "L_Thigh_X",
        "L_Thigh_MZ",
        "L_Thigh_MY",
        "L_Leg_MX",
        "L_Foot_MZ",
        "L_Foot_X",
        "R_Thigh_MX",
        "R_Thigh_MZ",
        "R_Thigh_MY",
        "R_Leg_X",
        "R_Foot_MZ",
        "R_Foot_MX",
        "WaistH_MX",
        "WaistH_Z",
        "Neck_MZ",
        "Neck_MX",
        "Head_X",
        "Head_Y",
        "L_Shoulder_X1",
        "L_Shoulder_MZ",
        "L_Shoulder_X2",
        "L_Elbow_MY",
        "L_Hand_X",
        "L_Hand_MZ",
        "R_Shoulder_MX1",
        "R_Shoulder_MZ",
        "R_Shoulder_MX2",
        "R_Elbow_MY",
        "R_Hand_MX",
        "R_Hand_MZ"
        )

#Pepper側の操縦対象になってる角度一覧
def _getPepperAngleNames():
    return (
        "HeadYaw",
        "HeadPitch",
        "RShoulderPitch",
        "RShoulderRoll",
        "RElbowYaw",
        "RElbowRoll",
        "RWristYaw",
        "RHand",
        "LShoulderPitch",
        "LShoulderRoll",
        "LElbowYaw",
        "LElbowRoll",
        "LWristYaw",
        "LHand"
        )

#Qumarionのセンサデータを使ってPepperの角度を指定する
def _setAngles(*angles): 
    if _motion == None:
        return

    qumaSensorNames = _getQumaSensorNames()
    pepperAngleNames = _getPepperAngleNames()

    #読み取り時に順番を意識しないで済むように辞書化
    qumaAngles = dict(zip(qumaSensorNames, angles))

    pepperAngles = (
        #正面0, 左向き+
        -qumaAngles["Head_Y"] * _deg2rad, 
        #正面0, 下向き+
        -(qumaAngles["Head_X"] - qumaAngles["Neck_MX"] - 20.0) * _deg2rad,
        #正面0, 腕下げて+
        (qumaAngles["R_Shoulder_MX1"] + 100) * _deg2rad,
        #正面0, 開いたとき-のつもり
        -(qumaAngles["R_Shoulder_MZ"] + 75) * _deg2rad,
        #肘内側が内側向きで0, 上を向く方向が+のつもり
        (-qumaAngles["R_Shoulder_MX2"] + 90) * _deg2rad,
        #肘のばして0, まげて+のつもり
        qumaAngles["R_Elbow_MY"] * _deg2rad,
        #手のひら下向きで0, 内転して+のつもり
        qumaAngles["R_Hand_MX"] * _deg2rad,
        #Qumarionの手を手の甲側に曲げると手を開き、逆に曲げると閉じる
        (qumaAngles["R_Hand_MZ"] + 30) / 90.0,
        #正面0, 腕下げて+のつもり
        (-qumaAngles["L_Shoulder_X1"] + 100) * _deg2rad,
        #正面0, 開いたとき+のつもり
        (-qumaAngles["L_Shoulder_MZ"] + 75) * _deg2rad,
        #肘内側が内側向きで0, 上を向く方向が-
        -(qumaAngles["L_Shoulder_X2"] + 80) * _deg2rad,
        #肘伸ばして0, まげて-   
        qumaAngles["L_Elbow_MY"] * _deg2rad,
        #手のひら下向きで0, 内転して+のつもり
        -qumaAngles["L_Hand_X"] * _deg2rad,
        #Qumarionの手を手の甲側に曲げると手を開き、逆に曲げると閉じる
        (-qumaAngles["L_Hand_MZ"] + 30) / 90.0
        )

    #TODO1: pepperAnglesには関節ごとに動かせる値の上限と下限があるので
    #  リミットチェックを行うべき

    #TODO2: 第3引数に0より大きく1以下の値が入ったリスト
    #  (pepperAnglesと同じ長さの)を入れて関節別に反応の良さを調整
    _motion.setAngles(pepperAngleNames, pepperAngles, 0.2)

    _setIsBusy(False)


def initialize(ip, port=9559):
    global _motion
    _motion = ALProxy("ALMotion", ip, port)

def setAngles(angles):
    if _getIsBusy():
        return
    _setIsBusy(True)

    t = Thread(
        target=_setAngles, 
        args=(angles)
        )
    t.start()

スクリプトが準備出来たら"quma2pepper.py"のほうをメインスクリプトとして起動します。コマンドプロンプトでquma2pepper.pyのディレクトリに移動し、下記コマンドで実行します。これでPython スクリプトはGUIプログラムからのデータ受信待ち状態になります。

python quma2pepper.py

最後の準備としてGUI側の"UDP送信設定"を開き、「送信タイミング」を「ボタンを押している間は送信」、「送信内容」を「センサ値バイナリ」とします。これで準備は完了です。

以上の準備が整ったらQUMARIONを手で持ち、適当なポーズを取らせて背面のボタンを押します。ボタンを押してる間はPepperがQUMARIONの動きに追随するので、腕や頭を動かしてみてください。送信タイミングのオプションを変更することでボタンを押さないでも常にデータを送りっぱなしにすることなども可能です。

まとめとか

まとめ書くようなネタじゃない気もしますが、QUMARIONはPepperのような人型ハードウェア用のゴツいコントローラとしても使えますよ、という話でした。質問等あればTwitterとかでお尋ねください。

Bakulog

獏の夢日記的な何か。