ルービックキューブロボットのソフトウェアをアップデートした 4. 状態認識

この記事はなに？

私は現在2x2x2ルービックキューブを解くロボットを開発中です。これはそのロボットのプログラムの解説記事集です。

かつてこちらの記事に代表される記事集を書きましたが、この時からソフトウェアが大幅にアップデートされたので新しいプログラムについて紹介しようと思います。

該当するコードはこちらで公開しています。

関連する記事集

「ルービックキューブを解くロボットを作ろう！」

1. 概要編
2. アルゴリズム編
3. ソフトウェア編
4. ハードウェア編

ルービックキューブロボットのソフトウェアをアップデートした

1. 基本関数
2. 事前計算
3. 解法探索
4. 状態認識(本記事)
5. 機械操作(Python)
6. 機械操作(Arduino)
7. 主要処理

今回は状態認識編として、detector.pyを紹介します。

使うモジュールのインポート

使うモジュールをインポートします。

import cv2
from time import sleep

from basic_functions import *
from controller import move_actuator

ここで、controllerは次回解説するプログラムですが、この中のmove_actuator関数は、その名の通りアクチュエータ(ここではモーター)を動かすコマンドを送る関数です。

パズル4面の情報から6面の情報を作る

2x2x2ルービックキューブは4面の色を見るだけで全面の色が推測できます。この性質を利用するため、埋まっている色の情報から埋まっていない色を推測する関数が必要です。その関数を紹介します。

先に謝らせてください。こちらの関数、元々手動で色を入力する時代(数ヶ月前)に勢いで作った関数でして、今となってはなにをやっているのかあまりわかりません！そして今は特定の4面の色を見ることで残りの2面の色を推測するので、こんなに汎用性の高い関数である必要はありません。つまりここで紹介する関数は、少し冗長かつ何をやっているのかよくわからないがなんか動いている関数です。ごめんなさい。昔の自分に「せめてコメントをつけろ」とよく注意しておきます。

余談ですが今回こんな記事を書いているのは未来の自分のためでもあります。未来の自分がこのコードを読んで途方に暮れないように、今のうちにコメントを書いて、解説記事も残しておくようにしました。

''' 埋まっていないところで色が確定するところを埋める '''
''' Fill boxes if the color can be decided '''
def fill(colors):
    for i in range(6):
        for j in range(8):
            if (1 < i < 4 or 1 < j < 4) and colors[i][j] == '':
                done = False
                for k in range(8):
                    if [i, j] in parts_place[k]:
                        for strt in range(3):
                            if parts_place[k][strt] == [i, j]:
                                idx = [colors[parts_place[k][l % 3][0]][parts_place[k][l % 3][1]] for l in range(strt + 1, strt + 3)]
                                for strt2 in range(3):
                                    idx1 = strt2
                                    idx2 = (strt2 + 1) % 3
                                    idx3 = (strt2 + 2) % 3
                                    for l in range(8):
                                        if parts_color[l][idx1] == idx[0] and parts_color[l][idx2] == idx[1]:
                                            colors[i][j] = parts_color[l][idx3]
                                            done = True
                                            break
                                    if done:
                                        break
                                break
                    if done:
                        break
    return colors

推測で解説を一応します。

最初の2つのi, jを回すfor文は、単に色の格納された以下のような配列を一つずつ見ています。

colors = [
    ['', '', 'w', 'o', '', '', '', ''],
    ['', '', 'w', 'g', '', '', '', ''],
    ['b', 'o', 'g', 'y', 'r', 'w', 'b', 'r'],
    ['o', 'o', 'g', 'g', 'w', 'r', 'b', 'b'],
    ['', '', 'y', 'r', '', '', '', ''],
    ['', '', 'y', 'y', '', '', '', '']
    ]

ここで、w, y, g, b, o, rはそれぞれ白、黄、緑、青、橙、赤を表します。なおこの配列の持ち方は過去に作ったプログラムの伝統を悪い方向に引き継いでいて、無駄が多いです。いつか直したいです。

その後のif (1 < i < 4 or 1 < j < 4) and colors[i][j] == '':で、色の情報が本来含まれているところかそうでないところかを分けます。

for k in range(8):で考えうる全てのパーツの候補を回し、if [i, j] in parts_place[k]:で2色の色が一致したら、for strt in range(3):でパーツの向き3通りを全部チェックします。

このような流れで処理をしているのだと思います。

カメラを使った状態認識

パズルの状態を認識するにはカメラを使います。今回はOpenCVというライブラリを使って色を認識します。

''' パズルの状態の取得 '''
''' Get colors of stickers '''
def detector():
    colors = [['' for _ in range(8)] for _ in range(6)]
    # パズルを掴む
    for i in range(2):
        move_actuator(i, 0, 1000)
    for i in range(2):
        move_actuator(i, 1, 2000)
    sleep(0.3)
    rpm = 200
    capture = cv2.VideoCapture(0)
    # 各色(HSV)の範囲
    #color: g, b, r, o, y, w
    color_low = [[40, 50, 50],   [90, 50, 70],    [160, 50, 50],   [170, 50, 50],    [20, 50, 30],   [0, 0, 50]]
    color_hgh = [[90, 255, 255], [140, 255, 200], [170, 255, 255], [10, 255, 255], [40, 255, 255], [179, 50, 255]]
    # 各パーツのcolors配列上での位置
    surfacenum = [[[4, 2], [4, 3], [5, 2], [5, 3]], [[2, 2], [2, 3], [3, 2], [3, 3]], [[0, 2], [0, 3], [1, 2], [1, 3]], [[3, 7], [3, 6], [2, 7], [2, 6]]]
    # その他定数
    d = 10
    size_x = 130
    size_y = 100
    center = [size_x // 2, size_y // 2]
    dx = [-1, -1, 1, 1]
    dy = [-1, 1, -1, 1]
    # パズルの4つの面を読み込む
    for idx in range(4):
        # 読み込みがうまくいかない時があるので5回ダミーを読み込ませておく
        for _ in range(5):
            ret, frame = capture.read()
        tmp_colors = [['' for _ in range(8)] for _ in range(6)]
        loopflag = [1 for _ in range(4)]
        # 1面にある4枚のステッカーの色が全て読み込まれるまでwhile
        while sum(loopflag):
            ret, show_frame = capture.read()
            show_frame = cv2.resize(show_frame, (size_x, size_y))
            hsv = cv2.cvtColor(show_frame,cv2.COLOR_BGR2HSV)
            # 4枚のステッカーを順番に調べる
            for i in range(4):
                y = center[0] + dy[i] * d
                x = center[1] + dx[i] * d
                val = hsv[x, y]
                # 6色のうちどれかを調べる
                for j in range(6):
                    flag = True
                    for k in range(3):
                        if not ((color_low[j][k] < color_hgh[j][k] and color_low[j][k] <= val[k] <= color_hgh[j][k]) or (color_low[j][k] > color_hgh[j][k] and (color_low[j][k] <= val[k] or val[k] <= color_hgh[j][k]))):
                            flag = False
                    if flag:
                        tmp_colors[surfacenum[idx][i][0]][surfacenum[idx][i][1]] = j2color[j]
                        loopflag[i] = 0
                        break
        # colors配列に値を格納
        for i in range(4):
            colors[surfacenum[idx][i][0]][surfacenum[idx][i][1]] = tmp_colors[surfacenum[idx][i][0]][surfacenum[idx][i][1]]
        # 次の面を見るためにモーターを回す
        move_actuator(0, 0, -90, rpm)
        move_actuator(1, 0, 90, rpm)
        sleep(0.2)
    capture.release()
    colors = fill(colors)
    return colors

ここで少し問題なのが、色の範囲が決め打ちなことです。これでは外乱によって色の認識がうまくいかなくなる可能性があります。実際にMaker Faire Tokyo 2020の会場では光の環境によってうまく動きませんでした(値を微修正して乗り切りました)。

まとめ

今回はカメラで状態を認識し、見ていない面を復元するパートを紹介しました。次回は実際にロボットを動かすところです。