M5GOの加速度・角速度センサーでBlenderの3Dモデルを動かす

#概要

M5GO 内蔵の加速度・角速度センサーをコントローラーとして Blender の3Dモデルを回転させるサンプルです。そのうち、物理的に動くもののコントローラーとして使いたいところですが、とりあえず、センサーからの取得値の扱い、回転角度の算出のお試しです。

• M5GO内蔵の加速度・角速度センサー（MPU6500）を使って回転角度を算出
• M5GO付属の角度センサーも利用
• 算出した回転角度をMQTTで送信
• Mac上のBlenderでPython APIを使用して3Dモデルを回転させる

##環境

• M5GO (Firmware 0.15)
• iMac (macOS High Sierra)
• MQTT Broker (mosquitto version 1.4.15)
• Blender 2.79

こんな感じで動きます。

M5GOの加速度・角速度センサーでBlenderの3Dモデルを動かす。サンプルの実装を公開しました。https://t.co/5oDYoVy73X#M5GO #M5Stack pic.twitter.com/usFxMz5MAZ

— 稲澤祐一 (@inasawa) 2018年9月4日

M5GO回転角度送信

以下のプログラムを M5GO Cloud から動かす、あるいは boot.py/main.py を置き換えて M5GO 上で実行することで、加速度・角速度センサーから算出した回転角度の値を MQTT Broker に対して送信します。
boot.py/main.py の置き換えについては M5GO(M5Stack)で気温・湿度・気圧グラフ表示参照

回転角度の算出については、いくつか解説されているサイトを読んで参考にさせて頂いたものの、理解不十分、計算式の違いによる影響を確認出来ない等で、今回は以下の M5Bala の mpu6050.py の中のロジックをそのまま流用しています。
https://github.com/m5stack/M5Bala/blob/master/mpy/mpu6050.py

100ms間隔で送信していますが、MQTTでの送信に30ms程度かかっているため、ブン回しても50ms間隔程度が限界でした。

from m5stack import lcd, buttonA, buttonB, buttonC
from mpu6500 import MPU6500, ACCEL_FS_SEL_2G, GYRO_FS_SEL_250DPS, SF_M_S2, SF_RAD_S, SF_G, SF_DEG_S
import machine
import i2c_bus
import math
import time
import network
import gc
import units


MQTT_NAME   = 'iMac Mosquitto'
MQTT_BROKER = 'mqtt://MQTTBroker' # MQTT Broker のアドレスを設定
MQTT_TOPIC  = 'M5GO/MPU6500'

def conncb(task):
    print('MQTT Broker [{}] Connected'.format(task))

def disconncb(task):
    print('MQTT Broker [{}] Disconnected'.format(task))

class MyMPU6500(MPU6500):
    def __init__(
        self, i2c, address=0x68,
        accel_fs=ACCEL_FS_SEL_2G, gyro_fs=GYRO_FS_SEL_250DPS,
        accel_sf=SF_G, gyro_sf=SF_DEG_S
    ):
        super().__init__(i2c, address, accel_fs, gyro_fs, accel_sf, gyro_sf)
        
        self.preInterval = time.ticks_us()
        self.accCoef  = 0.02
        self.gyroCoef = 0.98
        self.angleGyroZ = 0
        self.angleGyroX = 0
        self.angleGyroY = 0
        self.angleGyroZ = 0
        self.angleX = 0
        self.angleZ = 0
        self.angleY = 0
        self.gyroXoffset = 0
        self.gyroYoffset = 0
        self.gyroZoffset = 0
        
    def setGyroOffsets(self, x, y, z):
        self.gyroXoffset = x
        self.gyroYoffset = y
        self.gyroZoffset = z
    
    def reset_angle_by_gyro(self):
        # 角速度から積算している回転角をリセット
        self.angleGyroX = 0
        self.angleGyroY = 0
        self.angleGyroZ = 0
        self.angleX = 0
        self.angleY = 0
        self.angleZ = 0
        
    @property
    def angles(self):
        # 加速度から回転角を算出
        accX, accY, accZ = self.acceleration
        angleAccX = math.atan2(accY, accZ + abs(accX)) * SF_RAD_S
        angleAccY = math.atan2(accX, accZ + abs(accY)) * (-SF_RAD_S)
        
        # 角速度取得値からオフセット値を減算
        gyroX, gyroY, gyroZ = self.gyro
        gyroX -= self.gyroXoffset
        gyroY -= self.gyroYoffset
        gyroZ -= self.gyroZoffset
        
        # 角速度取得間隔
        ticks_us = time.ticks_us()
        interval = time.ticks_diff(ticks_us, self.preInterval) / 1000000
        self.preInterval = ticks_us
        
        # 角速度を積算して回転角を算出
        self.angleGyroX += gyroX * interval
        self.angleGyroY += gyroY * interval
        self.angleGyroZ += gyroZ * interval
        
        # 角速度と加速度の両方からの算出値を使って回転角を補正
        self.angleX = (self.gyroCoef * (self.angleX + gyroX * interval)) + (self.accCoef * angleAccX)
        self.angleY = (self.gyroCoef * (self.angleY + gyroY * interval)) + (self.accCoef * angleAccY)
        self.angleZ = self.angleGyroZ
        
        return tuple([angleAccY,       angleAccX,
                      self.angleGyroY, self.angleGyroX, -self.angleGyroZ,
                      self.angleY,     self.angleX])

def send_angle_data(timer):
    # ボタンＡが押されたら、角速度から積算している回転角をリセット
    if buttonA.wasPressed():
        mpu6500.reset_angle_by_gyro()
        clear_screen()
    
    # 角度センサーからの取得値（0〜100）を -180〜180 に変換
    angle_sensor_val = (angle_sensor.read() - 50) * 180 / 50 
    
    # 加速度および角速度から算出した回転角を取得
    a_roll, a_pitch, g_roll, g_pitch, g_yaw, ga_roll, ga_pitch = mpu6500.angles
    
    lcd.print(' {:8.3f}  {:8.3f}  {:8.3f} '.format(a_roll, a_pitch, angle_sensor_val),  10, 80)
    lcd.print(' {:8.3f}  {:8.3f}  {:8.3f} '.format(g_roll, g_pitch, g_yaw),  10,  190)
    if mqtt.status()[0] == 2: # (2, 'Connected')
        # 以下の8種類の値を送信
        #   加速度から算出した回転角 (roll, pitch)
        #   角速度から算出した回転角 (roll, pitch, yaw)
        #   角速度と加速度の両方から算出した回転角 (roll, pitch)
        #   角度センサーから取得した回転角  (yaw)
        mqtt.publish(MQTT_TOPIC, '{},{},{},{},{},{},{},{}'.format(
                    a_roll, a_pitch,
                    g_roll, g_pitch, g_yaw,
                    ga_roll, ga_pitch,
                    angle_sensor_val))
    else:
        print(time.strftime('%Y-%m-%d %H:%M:%S', time.localtime()), ' MQTT Status: ', mqtt.status())
    
    gc.collect()

def clear_screen():
    lcd.clear()
    lcd.print('By Acceleration       By Angle', 10,  20)
    lcd.print('Roll       Pitch      Yaw',      40,  50)
    lcd.print('By Gyro and Acceleration',       10, 130)
    lcd.print('Roll       Pitch      Yaw',      40, 160)


lcd.font(lcd.FONT_DejaVu18)

mqtt = network.mqtt(MQTT_NAME, MQTT_BROKER,
                autoreconnect=True,
                connected_cb=conncb, disconnected_cb=disconncb)
mqtt.start()

angle_sensor = units.ANGLE(units.PORTB)

i2c = i2c_bus.get(i2c_bus.M_BUS)
mpu6500 = MyMPU6500(i2c)

# 角速度の初回取得ちをオフセットとして設定
gyroX, gyroY, gyroZ = mpu6500.gyro
mpu6500.setGyroOffsets(gyroX, gyroY, gyroZ)

clear_screen()

# 100ms間隔で実行
t0 = machine.Timer(0)
t0.init(period=100, mode=t0.PERIODIC, callback=send_angle_data)

Mosquitto 立ち上げ

Mosquitto を Blender を動かす Mac上に Docker を使って立ち上げます。

以下から Dockerfile、docker-entrypoint.sh を取得
https://github.com/eclipse/mosquitto/tree/master/docker/1.4.14

Dockerfileを修正し、動作確認のために mosquitto-clients もインストールしています。

Dockerfile

FROM alpine

LABEL Description="Eclipse Mosquitto MQTT Broker"

RUN apk --no-cache add mosquitto mosquitto-clients && \
    mkdir -p /mosquitto/config /mosquitto/data /mosquitto/log && \
    cp /etc/mosquitto/mosquitto.conf /mosquitto/config && \
    chown -R mosquitto:mosquitto /mosquitto

COPY docker-entrypoint.sh /
ENTRYPOINT ["/docker-entrypoint.sh"]
CMD ["/usr/sbin/mosquitto", "-c", "/mosquitto/config/mosquitto.conf"]

Dockerイメージの作成および起動

$ docker build -t mosquitto .
$ docker run -d --name mosquitto -p 1883:1883 mosquitto

Blender セットアップ

以下から Blender ダウンロード・インストールし、Blender 内の Python に MQTT 通信モジュールをインストールします。
https://www.blender.org/download/

$ # pip install スクリプト取得
$ curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
$
$ # Blener 内の Python へ pip をインストール
$ /Applications/Blender/blender.app/Contents/Resources/2.79/python/bin/python3.5m get-pip.py
$
$ # MQTT 通信モジュール paho-mqtt をインストール
$ /Applications/Blender/blender.app/Contents/Resources/2.79/python/bin/pip install paho-mqtt

Blender 3Dモデル回転プログラム

$ /Applications/Blender/blender.app/Contents/MacOS/blender -P blender_3DModel.py

操作するオブジェクトは三角錐（なんとなく飛行体っぽく）を３つ作成し,
それぞれ以下のように操作し、動きを比べています。

• オブジェクト１は加速度から算出した roll, pitch と角度センサーから取得した yaw で操作
• オブジェクト２は角速度と加速度の両方から算出した roll, pitch と角速度から算出した yaw で操作
• オブジェクト３は角速度から算出した roll, pitch, yaw で操作

blender_3DModel.py

import bpy
import math
import paho.mqtt.client as mqtt
from mathutils import Vector


MQTT_BROKER = 'MQTTBroker' # MQTTBroker のアドレスを設定
MQTT_TOPIC  = 'M5GO/MPU6500'


# コネクト時コールバック
def on_connect(client, userdata, flags, respons_code):
    print('MQTT Broker Connected. Status {}'.format(respons_code))
    client.subscribe(MQTT_TOPIC)

# メッセージ受信時コールバック
def on_message(client, userdata, msg):
    # 以下の8種類の値を取得
    #   加速度から算出した回転角 (roll, pitch)
    #   角速度から算出した回転角 (roll, pitch, yaw)
    #   角速度と加速度の両方から算出した回転角 (roll, pitch)
    #   角度センサーから取得した回転角  (yaw)
    try:
        a_roll, a_pitch, \
        g_roll, g_pitch, g_yaw, \
        ga_roll, ga_pitch, \
        angle_sensor_val = \
                [float(n) for n in msg.payload.decode('utf-8').split(',')]
    except Exception as e:
        print('Exception: ', e)
        return
        
    # オブジェクト１は加速度から算出した roll, pitch と角度センサーから取得した yaw で操作
    obj1.rotation_euler[0] = -math.radians(a_roll)
    obj1.rotation_euler[1] =  math.radians(a_pitch)
    obj1.rotation_euler[2] = -math.radians(angle_sensor_val)
    
    # オブジェクト２は角速度と加速度の両方から算出した roll, pitch と角速度から算出した yaw で操作
    obj2.rotation_euler[0] = -math.radians(ga_roll)
    obj2.rotation_euler[1] =  math.radians(ga_pitch)
    obj2.rotation_euler[2] = -math.radians(g_yaw)
    
    # オブジェクト３は角速度から算出した roll, pitch, yaw で操作
    obj3.rotation_euler[0] = -math.radians(g_roll)
    obj3.rotation_euler[1] =  math.radians(g_pitch)
    obj3.rotation_euler[2] = -math.radians(g_yaw)


# Blender 起動時に Splash Screen を表示しない
bpy.context.user_preferences.view.show_splash = False

# Clear Objects
bpy.ops.object.select_all(action='SELECT')
bpy.ops.object.delete(True)

# ３角錐の頂点
verts = []
verts.append(Vector((-2.5, 0, 0)))
verts.append(Vector(( 2.5,-2, 0)))
verts.append(Vector(( 2.5, 2, 0)))
verts.append(Vector(( 2.5, 0, 1)))

# 面を構成する頂点を verts のインデックスで指定
faces = [[0, 1, 2], [0, 1, 3], [0, 2, 3], [1, 2, 3]]
edges = []

# オブジェクト１
mesh = bpy.data.meshes.new(name='Mesh1')
mesh.from_pydata(verts, edges, faces)
obj1 = bpy.data.objects.new('Ship1', mesh)
obj1.location = (0, -5, 0)
bpy.context.scene.objects.link(obj1)

# オブジェクト２
mesh = bpy.data.meshes.new(name='Mesh2')
mesh.from_pydata(verts, edges, faces)
obj2 = bpy.data.objects.new('Ship2', mesh)
obj2.location = (0,  0, 0)
bpy.context.scene.objects.link(obj2)

# オブジェクト３
mesh = bpy.data.meshes.new(name='Mesh3')
mesh.from_pydata(verts, edges, faces)
obj3 = bpy.data.objects.new('Ship3', mesh)
obj3.location = (0,  5, 0)
bpy.context.scene.objects.link(obj3)

# MQTT Client セットアップ
client = mqtt.Client(protocol=mqtt.MQTTv311)

# コネクト時・メッセージ受信時のコールバックを指定
client.on_connect = on_connect
client.on_message = on_message

# MQTT Broker に接続しメッセージ受信待ち
client.connect(MQTT_BROKER)
client.loop_start()

お好みの３Ｄモデルを動かす

もっと凝った３Ｄモデルを自分で作りたいところですが、今回初めて Blender を使ってみたところなので、フリーの３Ｄモデルのサイト https://free3d.com から Blender 用の３Ｄモデルをダウンロードして試してみました。例えば、以下の X-WING を動かしてみます。
https://free3d.com/3d-model/x-wing-star-wars-24442.html

ファイル（X-WING.blend）をダウンロードし、プログラムを実行します。

$ /Applications/Blender/blender.app/Contents/MacOS/blender -P blender_X-WING.py

起動後 Screen Layout を「3D View Full」とし、向きを調整後 M5GO と連動させて X-WING を動かした例が以下の動画です。

M5GOの加速度センサーをコントローラとして何か動かしたいけど動かすものが無いので、MQTT経由でMac上のBlenderをPythonで制御して3Dモデルを動かしてみた。#M5GO #M5Stack pic.twitter.com/rgtbdKX4Fg

— 稲澤祐一 (@inasawa) 2018年8月8日

blender_X-WING.py

import bpy
import math
import paho.mqtt.client as mqtt
from mathutils import Vector


MQTT_BROKER = 'MQTTBroker' # MQTTBroker のアドレスを設定
MQTT_TOPIC  = 'M5GO/MPU6500'


# コネクト時コールバック
def on_connect(client, userdata, flags, respons_code):
    print('MQTT Server Connected. Status {}'.format(respons_code))
    client.subscribe(MQTT_TOPIC)

# メッセージ受信時コールバック
def on_message(client, userdata, msg):
    # 以下の8種類の値を取得
    #   加速度から算出した回転角 (roll, pitch)
    #   角速度から算出した回転角 (roll, pitch, yaw)
    #   角速度と加速度の両方から算出した回転角 (roll, pitch)
    #   角度センサーから取得した回転角  (yaw)
    try:
        a_roll, a_pitch, \
        g_roll, g_pitch, g_yaw, \
        ga_roll, ga_pitch, \
        angle_sensor_val = \
                [float(n) for n in msg.payload.decode('utf-8').split(',')]
    except Exception as e:
        print('Exception: ', e)
        return
        
    bpy.data.objects['Cylinder.002'].rotation_euler[0] = -math.radians(g_yaw)
    bpy.data.objects['Cylinder.002'].rotation_euler[1] =  math.radians(ga_pitch)
    bpy.data.objects['Cylinder.002'].rotation_euler[2] =  math.radians(ga_roll)


# Blender 起動時に Splash Screen を表示しない
bpy.context.user_preferences.view.show_splash = False

bpy.ops.wm.open_mainfile(filepath='X-WING.blend')

# MQTT Client セットアップ
client = mqtt.Client(protocol=mqtt.MQTTv311)

# コネクト時・メッセージ受信時のコールバックを指定
client.on_connect = on_connect
client.on_message = on_message

# MQTT Broker に接続しメッセージ受診待ち
client.connect(MQTT_BROKER)
client.loop_start()