grbl1.1+Arduino CNCシールドV3.5+bCNCを使用中。
BluetoothモジュールおよびbCNCのPendant機能でスマホからもワイヤレス操作可能。
その他、電子工作・プログラミング、機械学習などもやっています。
MacとUbuntuを使用。

CNCマシン全般について:
国内レーザー加工機と中国製レーザー加工機の比較
中国製レーザーダイオードについて
CNCミリングマシンとCNCルーターマシンいろいろ
その他:
利用例や付加機能など:
CNCルーター関係:



*CNCマシンの制作記録は2016/04/10〜の投稿に書いてあります。


2022年2月22日火曜日

ヒルベルト曲線/Hilbert curve(その2):曲線上の任意点と画像縮小

 前回、ヒルベルト曲線をPythonで描きました。今回はヒルベルト曲線のフラクタルな性質についての実験を行おうと思います。

環境: Python 3.8.5、Jupyter notebook


実験内容:

  • ヒルベルト曲線上に任意の点を複数打ち、オーダーを変えたときにそれらの点の位置関係は保たれるかどうか?
  • ヒルベルト曲線のルートで画像をスキャンし、そのスキャンデータをもとにオーダーを下げて画像縮小を行う
  • ヒルベルト曲線のルートで画像を描く(アニメーション)


ヒルベルト曲線のアルゴリズム:

前回との違い:(アルゴリズムの内容については前回参照)
  • 関数化
  • listをnumpy.arrayに変更(直接四則計算可能)
  • arrayの要素を入れ替える部分でcopy()を使用
    • U[1], U[3] = U[3].copy(), U[1].copy()
  • 正規化(norm)とオフセット(offset)を引数に与える
  • 出力されるX, Y=Pがヒルベルト曲線の各点の座標(array)

def hilbert(ORDER, norm=True, offset=0.5):            # argument: order=1, 2, 3 ... n, offset=(x:0.5, y:0.5)
    N = 2**ORDER                                      # the number of rows or columns
    P = zeros((2, N * N))                             # prepare an array for (X, Y) coodinates output
    for i in range(N * N):
        U = array([[0, 0], [0, 1], [1, 1], [1, 0]])   # four points under ORDER=1
        V = array([offset, offset])                   # offset: starting point
        for j in reversed(range(ORDER)):              # ORDER iteration: j=ORDER-1, ORDER-2 ... 0
            ID = i // 4**j % 4                        # quaternary index
            L = 2**j                                  # unit length: L=2**(ORDER-1), 2**(ORDER-2) ... 1 
            if   ID == 0:
                U[1], U[3] = U[3].copy(), U[1].copy() # swap 1st with 3rd when ID is 0
            elif ID == 3:
                U[0], U[2] = U[2].copy(), U[0].copy() # swap 0th with 2nd when ID is 3
            V += U[ID] * L                            # add each step under the ORDER
        P[:, i] = V                                   # preserve each points
    if norm:
        P /= N                                        # X, Y coodinates are nomalized as from 0 to 1
    return P                                          # returns X, Y = P[0], P[1]


ヒルベルト曲線上の任意の点:

ヒルベルト曲線は折れ曲がっていながらも連続した一本の線であるので、全体を一直線に引き延ばして全長を1(正規化)にしておきます。そこに任意の地点を設定するわけですが、例えば0.1の地点は全体の道のりの10%の位置になるというわけです。
線上の任意の点(0~1の値)がオーダーを変えたときにどのような座標になるのかを調べます。

コードは以下:


def Arbitrary_point(ap, X, Y): # ap: arbitrary point between 0 and 1
    tl = len(X) - 1
    n = int(ap * tl)
    m = ap * tl % 1
    P = [X[-1], Y[-1]]
    if n + m < tl:
        dx = X[n+1] - X[n]
        dy = Y[n+1] - Y[n]
        if dx == 0:
            P = [X[n], Y[n] + dy * m]
        elif dy == 0:
            P = [X[n] + dx * m, Y[n]]
    return P

この任意の点は、値によってはヒルベルト曲線上の二点間の間にも打つことができるようにしてあります。オーダー1のヒルベルト曲線上の0.1~0.9(0.1刻み)の地点は、以下の画像のように表現されます。9個の地点を新たな線でつないでいます。




この方法で、複数のオーダーによるヒルベルト曲線を重ね合わせて、任意の点(複数)を打って、それぞれどの程度違うのかを観察してみます。

コードは以下:


ORDERS = [3,4,5,6]            # list of Orders for Hilvert curves
AP = arange(0.1, 1, 0.1)      # list of arbitrary ratios on the lines: [0.1, 0.2 ... 0.9]

plt.figure(figsize=(10,10))
plt.axis([0,1,0,1])
cmap = plt.get_cmap('tab10')  # color table

X, Y = [], []
for i in ORDERS:
    X.append([])
    Y.append([])
    X[-1], Y[-1] = hilbert(i)
    plt.plot(X[-1], Y[-1], alpha=0.2, color=cmap(i))
    AX, AY = array([Arbitrary_point(ap, X[-1], Y[-1]) for ap in AP]).T
    plt.plot(AX, AY, color=cmap(i), alpha=0.7, label=str(i))
    plt.scatter(AX, AY, color=cmap(i))
    if i == ORDERS[0]:
        for j in range(len(AP)):
            plt.text(AX[j], AY[j], s=' ap='+str(round(AP[j], 3)))

plt.legend()
plt.show()


結果画像:



ここでは、オーダー3、4、5、6のヒルベルト曲線を重ね合わせています。そして任意の点は、0.1から0.9まで0.1刻みで9点打ってあります。
結果から分かることは、ほぼ同じような形をしており、フラクタルな性質が伺えます。オーダーが低い3のときはさすがにズレが大きいですが、オーダーが上がるほど誤差が少なくなってきます。


水平走査線との比較:

モニターなどで使われている走査線方式で同じことをやってみるとどうなるか試してみます。走査線は水平かつ平行に左から右、上から下へ走ることとします。

結果画像:



走査線の場合は、横幅のピクセル数が変わってしまうためズレが大きくなってしまいます。0.5の地点だけはきりのいい値なので似たような位置に集中しています。
走査線と比較すれば、ヒルベルト曲線は異なるスケールに対してもズレが少ないという傾向が見てとれます。


浮動小数点の問題(余談):

今回、小数の計算を行っている際に計算式と結果が一致しないことがあり、調べてみると浮動小数点の誤差問題ということらしいです。これはコンピュータ全般における問題でいくつか改善方法などあるようです。

例えば:

0.2 % 0.1 = 0.0

になりますが、

0.3 % 0.1 = 0.09999999999999998 

になってしまいます。
ためしに、以下のように0.1刻みで出力させると、

for i in arange(0, 1, 0.1):
    print(i)

出力結果:

0.0
0.1
0.2
0.30000000000000004
0.4
0.5
0.6000000000000001
0.7000000000000001
0.8
0.9

になり、どうやら0.3、0.6、0.7にこの問題が発生しているようです。
ちなみに、0~0.9までの値を0.1で割りその余りを100倍する計算:

for i in arange(0, 1, 0.1):
    print(i % 0.1 * 100)

答えはすべて0になるはずですが、出力は以下:

0.0
0.0
0.0
2.7755575615628914e-15
0.0
9.999999999999998
5.551115123125783e-15
2.7755575615628914e-15
0.0
9.999999999999998

0.3、0.6、0.7の出力は小さい誤差がありround()で有効桁数まで丸めてしまえば大丈夫かもしれませんが、問題は出力が9.999999999999998になっている部分。

0.5 % 0.1 * 100 = 9.999999999999998
0.9 % 0.1 * 100 = 9.999999999999998

本来0なのに、ここまでずれるとround()を使っても計算結果が大きくずれてしまいます。ちなみに最後に100を掛ける代わりに事前に0.5と0.1に100を掛けておけば問題ありません。

(0.5 * 100) % (0.1 * 100) = 0.0

ということで、厳密に計算するためのDecimalモジュールもありますが、連続で掛算や割算あるいはモデューロを使うときは注意が必要そうです。


ヒルベルト曲線による画像スキャン:

ヒルベルト曲線のルートに沿ってピクセル単位で画像をスキャンし、一本の連続した色データ(RGB)をつくり、各オーダーに応じた密度の違いを利用して画像縮小を行います。


おなじみのレナさんの画像を使って実験してみます。

この画像の特徴:

  • サイズ(縦×横×RGB):512×512×3=262144×3=786432
  • ヒルベルト曲線オーダー数:9 (4^9=262144)
通常、画像を読み込んでmatplotlibで表示させると原点が左上(Y軸が下向き)になることがありますが、その場合は:

plt.imshow(image, origin='lower')

のように'lower'にすると原点が左下になります。
また、画像が上下反対になっている場合は、numpy.flipud(image)や読み込んだarrayをimage[::-1, :, :]にすることでY軸方向のみの反転が可能になります。


オーダー9によるフルスキャンのコード:

このスキャンによって各ピクセルの色情報(RGB値)と位置(x,y)が得られます。読み取る順序はヒルベルト曲線のルート順になります。

DEFAULT_ORDER = int(np.log2(IMG_W))                   # DEFAULT_ORDER=9: 2**9=512
N = 2**DEFAULT_ORDER                                  # the number of rows or columns: 512

HX, HY = hilbert(DEFAULT_ORDER, norm=False, offset=0) # ORDER=9, normalization: turned off, offset=0
HX = HX.astype(int)                                   # dtype: float to int 
HY = HY.astype(int)

HC = np.zeros((N * N, 3), dtype=int) # prepare an empty 3-channel serial array for color pixel data
for i in range(N * N):
    HC[i, :] = IMG[HY[i], HX[i], :]  # scan color pixel data along with the Hilbert curve route

plt.figure(figsize=(15, 2))          # show a part of the serial color data, len(HC)=262144 pixels
start, end = 1000, 1050              # start and end point to sample the serial data 
plt.imshow([HC[start:end, :]], origin='lower', extent=(0,end-start,0,1))
plt.show()

3チャンネルのRGB情報を含んだ一本のデータの一部を出力してみると以下のようになります。


HCというarrayに格納してあり、サイズは(262144, 3)あります。
上の画像はその一部としてインデックス番号1000番から1049番までのピクセル。
各ピクセルの位置はヒルベルト曲線のルートに沿って(HX,HY)の変数に格納されています。


ヒルベルト曲線を利用した画像縮小:

上記のスキャンからHX, HY, HCのデータを得たので、それらをもとにオーダー数を下げたヒルベルト曲線で画像縮小を行います。

  • オーダー1:2×2ピクセル
  • オーダー2:4×4ピクセル
  • オーダー3:8×8ピクセル
  • オーダー4:16×16ピクセル
  • オーダー5:32×32ピクセル
  • オーダー6:64×64ピクセル
  • オーダー7:128×128ピクセル
  • オーダー8:256×256ピクセル
  • オーダー9:512×512ピクセル(元画像と同等)

縮小方法:

  • 各ピクセルの位置はオーダーに応じたヒルベルト曲線の点座標を用いる
  • 色情報については、オーダー9のフルスキャンの一直線データを以下の間隔でスキップして当てはめていく。近傍ピクセルの色調補正なし。
    • オーダー1:65536個ずつスキップ
    • オーダー2:16384個ずつスキップ
    • オーダー3:4096個ずつスキップ
    • オーダー4:1024個ずつスキップ
    • オーダー5:256個ずつスキップ
    • オーダー6:64個ずつスキップ
    • オーダー7:16個ずつスキップ
    • オーダー8:4個ずつスキップ
    • オーダー9:1個ずつ、スキップなし(計:262144ピクセル)

以下がコード:


ORDER = 6                                      # reduction to ORDER=6: 64x64=4096 pixels
N = 2**ORDER

HX, HY = hilbert(ORDER, norm=False, offset=0)  # obtain x, y coodinates of the Hilbert curve
HX = HX.astype(int)                            # convert to dtype as integer
HY = HY.astype(int)

HIM = zeros((N, N, IMG_CH), dtype=int)         # prepare an empty array for ORDER=6
for i in range(N * N):
    HIM[HY[i], HX[i], :] = HC[i * 4**(DEFAULT_ORDER - ORDER), :] # read the every 64th pixels from the serial data 

上記コードはオーダー6の場合。64個飛ばしに色情報を読み込んでいます。全体的なサイズは64×64で画像幅では1/8、面積的には1/64まで縮小したことになります。


結果の画像(オーダー9からオーダー6に縮小した場合):



オーダーを変えれば多少位置情報は異なりますが、オーダー6であればヒルベルト曲線のフラクタルな性質から一応認識できるレベルにあります。


水平走査線で同様に読み込ませた場合:




走査線の場合は、先ほどの実験通り画像が崩れてしまいます。縦と横の両方をスキップさせれば表現可能ですが、ヒルベルト曲線の場合は一つの情報をスキップさせるだけで済みます。



上画像はヒルベルト曲線におけるオーダー3(上の段:64ピクセル)から2(下の段:16ピクセル)へ縮小したときのデータの対応表です。ヒルベルト曲線では単に4個ずつスキップしていけばいいので簡単です。



そしてこちらは、水平走査線の場合です(同じ64ピクセルから16ピクセルへの縮小)。画像の横方向で2個ずつスキップし、さらに縦方向のために16スキップ(4個セットが16スキップ)しなければいけません。計算式は特に難しいことはないのですが、ヒルベルト曲線ほど単純ではありません。


ヒルベルト曲線スキャンのオーダーごとの画像:

画像上部にオーダー、サイズが書いてあります。
尚オーダー9が等倍(512×512ピクセル)で、それ以下のオーダー8からの画像になります。















認識可能なのはオーダー6以上でしょうか。オーダー5も画像そのものを小さく表示すればある程度は認識可能かもしれません。






オーダー6、5、4を極端に小さく表示してみました。
元々、何かのアイコン程度の画素数しかないので、このくらい小さく表示すればあまり違いが目立たなくなります。それでもオーダー4は厳しいかもしれません。



ヒルベルト曲線画像スキャンのアニメーション:

画像のピクセルを読み込むコードができたのでそのアニメーションもつくってみました。

オーダー6であれば4分程度でJupyter notebook上には表示できますが、それでもmatplotlibのデフォルトのメモリー設定を超えてしまうので、メモリー設定を変更しなければなりません。オーダー6のGIFアニメもつくりましたが、4096枚ものGIF画像を使用しているためかGIFファイルだけでも23MBあります。このブログにアップロードする際にエラーが出てしまったので、代わりにmp4の動画をアップロードしました。
また、オーダー7以上になると時間がかかりすぎるので試していません。
コード内容は以下のGistを参照してください。










関連:

2022年2月18日金曜日

ヒルベルト曲線:Hilbert Curve(Python)

今回は、Pythonでヒルベルト曲線を描いてみます。

ヒルベルト曲線の性質上、再帰アルゴリズムで解く方法がありますが、今回は再帰アルゴリズムを使わず、そのまま通常ループで解いていこうと思います(個人的にはそのほうが分かりやすいので)。

追記:再帰アルゴリズムを後半に追加しました。

環境: Python 3.8.5、Jupyter notebook


上画像、ORDER=6の場合(4096個の点をつないだ線)。

ORDERを上げていけば、面を埋め尽くしてしまうという空間充填曲線の一つ。

また拡大縮小しても相似形となるフラクタルな性質も持ち合わせています。


アルゴリズム:

まずヒルベルト曲線は4点からなるコの字を基準として、それの組み合わせで描いていきます。

コの字の組み合わせとして、90回転が4パターン、そして時計回り、反時計回りがあるので、合計4x2=8パターンありますが、実際はそのうちの4パターンしか使いません。

今回は、座標(0,0)を開始点とし移動方向が上右下のコの字を基本形とし、そのほか右上左、左下右、下左上の4パターンを使います。


ORDER=1の場合、上画像のような4点になります。

ルール1:(X座標が右向き、Y座標が上向きの場合)

  • 4点を(0, 1, 2, 3)の順番とする。
  • 原点0:(0, 0)を左下にする。
  • 原点(0, 0)から上(0, 1)、右(1, 1)、下(1, 0)という順番で描く。

*尚、プログラミング環境によってY座標が下向きの場合は、原点が左上となり、下、右、上(上下反転)。



次にORDER=2の場合(赤破線はORDER=1)。
黒番号は全体の通し番号(0〜15)。
青番号はORDER=2におけるローカルな番号(ORDER=1の番号と同じ位置関係)。
1組目の青コの字(通し番号0〜3)は赤0のエリア
2組目の青コの字(通し番号4〜7)は赤1のエリア
3組目の青コの字(通し番号8〜11)は赤2のエリア
4組目の青コの字(通し番号12〜15)は赤3のエリア

ルール2:

  • 点の数は4n個(コの字4点の組数:2n個)、n=ORDER
  • コの字(4点)ごとに(0, 1, 2, 3)の番号をつける。
  • 前回のORDER=1(赤破線)を参照し、青(0,1,2,3)の順番を入れ替える。
    • 赤0のエリア: 青(0,1,2,3)→青(0,3,2,1)、青1と青3を入れ替え
    • 赤1のエリア: 青(0,1,2,3)→青(0,1,2,3)、入れ替えなし
    • 赤2のエリア: 青(0,1,2,3)→青(0,1,2,3)、入れ替えなし
    • 赤3のエリア: 青(0,1,2,3)→青(2,1,0,3)、青0と青2を入れ替え

  • 前回のORDER=1(赤破線)を参照し、その順番で青コの字を配置する。
    • 赤0:(0,0)のエリア内に青1組目を配置する。
      • 赤(0, 0)に赤単位長2を掛けた値(0,0)を青の点に加算する(赤0の場合は結局0)。
    • 赤1:(0,1)のエリア内に青2組目を配置する。
      • 赤(0, 1)に赤単位長2を掛けた値(0,2)を青の点に加算する。
    • 赤2:(1,1)のエリア内に青3組目を配置する。
      • 赤(1, 1)に赤単位長2を掛けた値(2,2)を青の点に加算する。
    • 赤3:(1,0)のエリア内に青4組目を配置する。
      • 赤(1, 0)に赤単位長2を掛けた値(2,0)を青の点に加算する。
    (例)通し番号7の場合:
        赤1(0,1)のエリア内で青3(1,0)であるため
        赤1(0,1)×赤単位長2 + 青(1,0)×青単位長1 = (0,1)×2 + (1,0)×1 = (1,2)
        よって通し番号7は最終的に(1,2)の座標になる。

ORDER=Nの内容を得るには、ORDER=N-1を参照し、ORDER=N-1の内容を得るには、ORDER=N-2を参照するように遡っていくので、たしかに再帰アルゴリズムが向いていることが分かります。しかし、今回はこのまま続行。


上記手続きのコード:

N = 2**ORDER
X,Y = [], []                             
for i in range(N * N):                   
    U = [[0, 0], [0, 1], [1, 1], [1, 0]]
    px, py = 0.5, 0.5                   
    for j in reversed(range(ORDER)):    
        ID = i // 4**j % 4              
        L = 2**j                        
        if   ID == 0:
            U[1], U[3] = U[3], U[1]     
        elif ID == 3:
            U[0], U[2] = U[2], U[0]      
        px += U[ID][0] * L              
        py += U[ID][1] * L
    X.append(px)                        
    Y.append(py)

上記コードの説明(ORDER=3の場合): 
  • ORDER=3のため合計23×23=64点(0〜63)を順番に一つずつforループ処理させる。
  • 基本となるコの字型(ORDER=1)の座標を、U=[[0,0],[0,1],[1,1],[1,0]]という配列にしておく。
  • ORDER=3のとき、縦横に8個ずつ点があるため単位長を1とすれば全体のサイズは8x8となり、2のORDER乗だけ大きくなる(最終的に正規化し1×1のサイズに変更可)。
  • px, pyは原点座標であり、最終的に最小単位長1の半分(0.5)のオフセットがあるため、最初にずらしておく。
  • ネストされたfor文でORDERの数だけループさせる。reversed()で反転させたループとなり、j=2,1,0となる。
  • ID=i//4**j%4は、ORDER=jによる各点の位置関係を0〜3の値で返す。ORDER=3の場合は4進数の3ビットとなる(以下に「4進数」の説明あり)。
  • L=2**jは、ORDER=jに応じた単位長。
    • j=2: L=4 (ORDER=1)
    • j=1: L=2 (ORDER=2)
    • j=0: L=1 (ORDER=3)
  • ID=0の時U[1]とU[3]、ID=3の時U[0]とU[2]の入れ替えてコの字の向きを変える。
  • px+=U[ID][0]*Lは、入れ替えによって得られたUの要素とORDER数に応じた単位長を掛け合わせた値が加算され、最終的な座標が得られる(pyについても同様)。


青がORDER=3の結果。緑がORDER=2、赤がORDER=1。
黒番号は、全体の通し番号(0〜63)。
どのORDERにおいても基本的に左下:0、左上:1、右上:2、右下:3(ORDER=1を基準とする)。
各番号に対する座標:0:(0,0)、1:(0,1)、2:(1,1)、3:(1,0)

例えば、通し番号45の場合(上から3、右から2)、
    ORDER=1(赤)においては2のエリア(1,1)、ORDER=1の単位長L=4
    ORDER=2(緑)においては3のエリア(1,0)、ORDER=2の単位長L=2
    ORDER=3(青)においては1のエリア(0,1)、ORDER=3の単位長L=1
となるため、
    (px, py)= (1,1)×4 + (1,0)×2 + (0,1)×1 = (4,4) + (2,0) + (0,1) = (6,5)
となる。

同様に57の場合(右から4番目、下から2番目)の場合、
    ORDER=1(赤)においては3のエリア(1,0)、ORDER=1の単位長L=4
    ORDER=2(緑)においては0のエリア(0,0)、ORDER=2の単位長L=2
    ORDER=3(青)においては1のエリア(0,1)、ORDER=3の単位長L=1
よって、
    (px, py)= (1,0)×4 + (0,0)×2 + (0,1)×1 = (4,0) + (0,0) + (0,1) = (4,1)
となる。


4進数:

仕組みとしては4進数であり、ORDERが1増えるごとに4進数の桁も一つ増えていくことになります。同時に桁が増えるごとに、その桁に対応した単位長も2nになり、桁の値と単位長を掛け合わせて合算するとその地点の座標が求まります。

ORDER=3の場合は、IDは4進数による3ビット(以下の数値の並びを縦に見る)になり、

ORDER=1(j=2):ID=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1...]
ORDER=2(j=1):ID=[0,0,0,0,1,1,1,1,2,2,2,2,3,3,3,3,0,0,0,0,1,1,1,1...]
ORDER=3(j=0):ID=[0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3,0,1,2,3...]

ORDER=1は16個ごと、ORDER=2は4個ごと、ORDER=3は1個ごとに[0,1,2,3]の繰り返しになっています。ORDER=3の値の並びが最小単位のコの字に対応していますが、例えば13番目(0番目から数えて)の点は、縦に[0][3][1](赤い列の部分)であり、

ORDER=1(j=2): ID=[0] 左下:(0,0)、単位長:L=2**j=4
ORDER=2(j=1): ID=[3] 右下:(1,0)、単位長:L=2**j=2
ORDER=3(j=0): ID=[1] 左上:(0,1)、単位長:L=2**j=1

となり、ORDER=1のスケールでは左下(0,0)、ORDER=2のスケールでは右下(1,0)、ORDER=3のスケールでは左上(0,1)ということであり、各スケールにおける区画から住所のように座標を割り出せます。

ただし、途中で基準となるコの字型U=[[0,0],[0,1],[1,1],[1,0]]の要素の入れ替えがあるので、あくまでIDの番号は上の並びであって、U[0]〜U[3]の中身の並びは少し変わります。
以下は入れ替え後のU[ID]の4進数3ビット。

ORDER=1(j=2):U[ID]=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1...]
ORDER=2(j=1):U[ID]=[0,0,0,0,3,3,3,3,2,2,2,2,1,1,1,1,0,0,0,0,1,1,1,1...]
ORDER=3(j=0):U[ID]=[0,1,2,3,0,3,2,1,0,3,2,1,2,3,0,1,0,3,2,1,0,1,2,3...]

ORDER=1は、[0,1,2,3]の順で各16個ずつ合計64個。
ORDER=2は、[0,3,2,1][0,1,2,3][0,1,2,3][2,1,0,3]の順で各4個ずつ合計64個。
ORDER=3は、[0,1,2,3][0,3,2,1][0,3,2,1][2,3,0,1][0,3,2,1][0,1,2,3]...という並び方になっています。
先ほどの13番目の点(赤い列の部分)は、U[1]とU[3]の入れ替えによって[0][1][3]に変わっています。
この座標を求めると、

ORDER=1(j=2):[0]:(0,0),単位長:L=2**j=4
ORDER=2(j=1):[1]:(0,1),単位長:L=2**j=2
ORDER=3(j=0):[3]:(1,0),単位長:L=2**j=1

ということになり、各座標に単位長を掛け合わせて合算すると、

(0,0)×4 + (0,1)×2 + (1,0)×1 = (0,0)+(0,2)+(1,0) = (1,2)

よって、13番目(0番目から数えて)の点の座標は、(x,y) = (1,2)になります。

この入れ替えを含めたパターンがヒルベルト曲線におけるアルゴリズムの特徴だと思います。フラクタルだけあってこれもまたあるパターンの繰り返しになっています。


再帰アルゴリズム(追加):

せっかくなので再帰アルゴリムも試してみました(単にforループを外しただけです)。
Pythonのシステム上、再帰回数がデフォルト:1000回に設定されており、必要に応じて設定変更しなければいけないようです。
今回の場合:
オーダー4:1024回
オーダー5:4096回
オーダー6:16384回
オーダー7:65536回
オーダー8:262144回
オーダー9:1048576回
になります。
しかしながら、設定を挙げてもオーダー5にするとパンクしてしまいます。colab.googleでもオーダー5が限界だったので、Jupyter自体のメモリー設定を変えるか、メモ化などの工夫が必要かもしれません。面倒なので無理にこの方法を使わなくでもいいかもしれません。
ちなみにfunctools.lru_cacheのメモ化ツールを使う場合は、arrayやlistを含んだ関数はダメなので、それらを全部タプルに変換する必要があります(メモ化の場合、値そのものを辞書機能のキーに割り当てるため、その際にarrayやlistだとキーにならないためタプルに変換する必要がある)。そうやって試してみましたが、それでもダメでした。


import sys
sys.setrecursionlimit(10 ** 4)            # Increase recursive functions memory(default: 1000) 

def hilbert(i, L, U):
    if i == 4**Order:
        return P + 0.5                    # Final output when the iteration finishes, offset(x, y)=(0.5, 0.5)
    if L >= 0:
        ID = i // 4**L % 4                # Quarternary index number
        if   ID == 0:
            U[1], U[3] = U[3].copy(), U[1].copy() # Turn the U-shape in clockwise and steps in anti-clockwise
        elif ID == 3:
            U[0], U[2] = U[2].copy(), U[0].copy() # Turn the U-shape in anti-clockwise and steps in anti-clockwise
        P[:, i] += U[ID] * 2**L           # add each step to the array for the output
        return hilbert(i, L-1, U)         # go to the next L(L=Order-1, Order-2 ... 0, -1)
    
    U = array([[0,0],[0,1],[1,1],[1,0]])  # initialize U and L
    L = Ord - 1
    return hilbert(i+1, L, U)             # go to the next i

Order = 4                                 # Level of Order (fixed value)
i = 0                                     # The number of iteration(i=0, 1 ... 4**Order-1)
L = Ord - 1                               # Length of U-shape(L=Order-1, Order-2 ... 0, -1)
U = array([[0,0],[0,1],[1,1],[1,0]])      # Initial U-shape coodinates
P = zeros((2, 4**Order))                  # Initial empty array for the output

P = hilbert(i, L, U)                      # X, Y = P[0], P[1]
まず設定したOrderによって点の合計数(4**Order)が決まり、その回数だけ再帰ループすることを終了条件(最後に各点座標を含んだarray:Pにオフセット0.5を加えた値を返します)にします。
次に、iに対して参照する各オーダーの座標を積算していきます。その際も再帰ループでオーダーの分ループし、各オーダーにおける単位長Lをもとに座標を求めていきます。
このオーダーのループが終われば次の点(i+1)を代入した最後の再帰ループに移行します。
という二段階の再帰ループになっています。
i+1が終了条件に達すれば、そこで終了し、すべての点座標を含んだPを返します。

上記の二重再帰を少し変更してみました。以下のように最初の再帰をforループに変えてあります(再帰ループを使う意味が本末転倒)。

def hilbert(i):
    if i == 4**Order:
        return P + 0.5                    # Final output when the iteration finishes, offset(x, y)=(0.5, 0.5)
    U = array([[0,0],[0,1],[1,1],[1,0]])  # initialize U and L
    for j in reversed(range(Order)):      # loop for each Order-scale steps: j=Order-1, Order-2 ... 0 
        ID = i // 4**j % 4                # Quarternary index number
        if   ID == 0:
            U[1], U[3] = U[3].copy(), U[1].copy() # Turn the U-shape in clockwise and steps in anti-clockwise
        elif ID == 3:
            U[0], U[2] = U[2].copy(), U[0].copy() # Turn the U-shape in anti-clockwise and steps in anti-clockwise
        P[:, i] += U[ID] * 2**j           # add each step to the output array
    return hilbert(i+1)                   # go to the next i

Order = 5                                 # Level of Order (fixed value)
P = zeros((2, 4**Order))                  # Initial empty array for the output
i = 0                                     # The number of iteration(i=0, 1 ... 4**Order-1)
P = hilbert(i)                            # X, Y = P[0], P[1]

再帰が一つ減った分、オーダー5までは動くようになりましたが、それ以上だとパンクしてしまいます。徐々にもともとのforループだけのコードに近くなってきたので、無理に再帰を使わなくてもいいかもしれません。再帰なしのforループだけのコードならオーダー9でも動きます。

さらに以下は、全体をforループで回し、その内部でオーダーに応じたステップ数を計算する部分だけ再帰にしてみた場合です。この場合もオーダー5でパンクしてしまいます。

def hilbert(n, L, U):
    for i in range(n, 4**Order):
        if L >= 0:
            ID = i // 4**L % 4                        # Quarternary index number
            if   ID == 0:
                U[1], U[3] = U[3].copy(), U[1].copy() # Turn the U-shape in clockwise and steps in anti-clockwise
            elif ID == 3:
                U[0], U[2] = U[2].copy(), U[0].copy() # Turn the U-shape in anti-clockwise and steps in anti-clockwise
            P[:, i] += U[ID] * 2**L           # add each step to the array for the output
            return hilbert(n, L-1, U)         # go to the next L(next Order: 1, 2 ... Order-1)
        U = array([[0,0],[0,1],[1,1],[1,0]])  # initialize U and L
        L = Order - 1
        n += 1
    return P

Order = 4                                 # Level of Order (fixed value)
P = zeros((2, 4**Order))                  # Initial empty array for the output
n = 0                                     # The number of iteration(i=0, 1 ... 4**Order-1)
L = Order - 1                             # Length of U-shape(L=Order-1, Order-2 ... 0)
U = array([[0,0],[0,1],[1,1],[1,0]])      # Initial U-shape coodinates

P = hilbert(n, L, U)                      # X, Y = P[0], P[1]

結果、このアルゴリズムだと再帰は向いていないようです。とりあえず現状では、再帰なしの通常forループで行ったほうがよさげです。


全体的なコード:

アニメーション描画のコードも追加してあります。






関連:

2021年6月3日木曜日

IK(逆運動学):複数の円の交点から求める

 以前、CCD、FABRIK、ヤコビ行列などを用いて逆運動学を求めましたが、今回は複数の円を用いて、それらの交点から2D逆運動学を求めてみます。

環境:Python 3.8.5、Jupyter Notebook


これまでの方法:



今回の特長:

  1. 2D逆運動学
  2. 行列式、ヤコビ行列は使わない
  3. 繰り返し計算で目標値に近似する
  4. ガイドとなる円弧に沿ってアーム全体が配置される
  5. アームが交差しない
  6. ジョイント角が均等になる(各リンク長が等しい場合)
  7. 各リンク長が異なっても計算可能
  8. N個のリンクに対応

上図(アーム先端がターゲットに到達した状態):

  • 座標(0,0)がベース、LV[4]がアーム先端、TV(赤い×)がターゲット。
  • 青破線の円は各リンクの可動域、各ジョイント位置が円の中心。
  • 赤破線の円は各リンクを円弧状に配置するためのガイド、CPが円の中心。
  • 複数のリンクは赤破線の円に沿って配置されるため均等な角度となる(各リンク長が等しい場合)。


上図(初期状態:未到達時):計算手順

  • ベース(0,0)からターゲットTV(赤い×)まで線を引く(|TV|>0)
  • ベクトルTVの中点から垂直方向にCPを配置する(暫定的な配置)
  • (0,0)とTVを通り、CPを中心とした円を用意する(赤破線の円)、半径CR=|CP|
  • 各リンクのジョイント座標を赤破線の円上に配置する(青破線の円と赤破線の円の交点利用)
  • (0,0)、CP、TVの3点からなる角度Aを求める(下図)
  • (0,0)、CP、アーム先端(LV[4])の3点からなる角度Bを求める(下図)
  • 角度A+角度B=360度になるときアーム先端(LV[4])とTVが一致する
  • 角度A+角度Bが360度になるように、その差分Errorに応じてCPの座標を調節する計算式を用意する


Error = Thetas - tau
scaler += Error * 0.1
CP = np.array([-TV[1], TV[0]]) * scaler + TV/2

上記がCPの座標を調節する計算式。
Thetasは角度Aと角度Bの合計角、tauは2π=360度(目標値)、Errorはその差分。
CPの座標が変わることで赤破線の円の半径が変わる。常にCPはTV中点からの垂線上にある。
  • scaler=0のとき、TV/2がCPとなり、(0,0)とターゲット(TV)を直径とする円になる
  • scaler>0のとき、CPの位置はTV/2より上側になる
  • scaler<0のとき、CPの位置はTV/2より下側になる
つまり、目標値360度と角度A+角度Bの合計角の差分によってscaler値を増減させ、繰り返し計算によってアーム先端をターゲットに近似させていきます。

上図:

ターゲット(TV:赤い×)がベース(0,0)に近い場合でもアームが交差しない。ただし、|TV|>0の場合。


上図:

ターゲット(TV:赤い×)が遠く到達不可能な場合、ターゲット(TV)に向けてアームが伸びる。この場合、1000ループで計算終了。


上図:

リンク数10の場合。変数Nにリンク数を代入することで任意のリンク数に対応。


上図:

円の交点と角度の計算だけなので、各リンクが異なる長さの場合も計算可能。


コード:

  • Nはリンク数。TVはターゲットベクトル。LRは各リンク長(デフォルト:1.0)。
  • While文で繰り返し計算を行い、アーム先端(LV[4])とターゲットTVとの差分(Error)が0.0001以下、あるいは1000ループを超えるとループ解除。
  • intersects()関数は2つの円の中心座標と半径を引数にして、2つの交点を返しますが、今回は一方の交点(上側の交点座標)を利用。



2021年5月23日日曜日

IK(逆運動学)3Dアーム/同次変換行列/ヤコビ行列/角度制限

前回までは2Dアームでしたが、今回は3Dアームにおける逆運動学です。

環境:Python3.8.5、Jupyter Notebook


これまでの2Dアーム逆運動学:


今回の特長:
  • 3Dアーム逆運動学
  • 同次変換行列
  • ヤコビ行列(クロス積)
  • 角度制限
  • インタラクティブ操作

上図:

  • 左上:3D表示
  • 右上:真上からの視点(X-Y平面)
  • 右下:正面からの視点(X-Z平面)
  • 赤い×は目標座標
アームの初期設定は、
  • Joint0[0,0,0]はZ軸を中心にLink0(Joint1)を回転させる
  • Joint1[0,0,0.5]はY軸を中心にLink1(Joint2)を回転させる
  • Joint2[1,0,0]はY軸を中心にLink2(Joint3)を回転させる
  • Joint3[1,0,0]はY軸を中心にLink3(End-Effector)を回転させる
要するに、Link0のZ軸を中心した回転によってLink1〜3はX-Z平面上を回転し、アーム先端(EE:End-Effector)は目標値Targetに近似していきます。


同次変換行列:

3Dなので、同次変換行列は以下のように3種類用意しました。引数1の'X'、'Y'、'Z'によって、その軸回りでの回転になります(今回の4リンクアームの例では、'Y'、'Z'だけ使用)。引数2は平行移動ベクトル[x,y,z]、引数3は回転角。

def H(axis, vec, theta):
    if axis == 'X':
        return np.array([[1,          0,           0, vec[0]],
                         [0, cos(theta), -sin(theta), vec[1]],
                         [0, sin(theta),  cos(theta), vec[2]],
                         [0,          0,           0,      1]])
    elif axis == 'Y':
        return np.array([[cos(theta), 0, -sin(theta), vec[0]],
                         [         0, 1,           0, vec[1]],
                         [sin(theta), 0,  cos(theta), vec[2]],
                         [         0, 0,           0,      1]])
    elif axis == 'Z':
        return np.array([[cos(theta), -sin(theta), 0, vec[0]],
                         [sin(theta),  cos(theta), 0, vec[1]],
                         [         0,           0, 1, vec[2]],
                         [         0,           0, 0,      1]])
    else:
        return np.array([[1, 0, 0, vec[0]],
                         [0, 1, 0, vec[0]],
                         [0, 0, 1, vec[0]],
                         [0, 0, 0,      1]])
4つ目の行列は単位行列を返します(今回は不使用)。


FK(運動学):

FKにおいては、上記の同次変換行列を順次掛け合わせることで各ジョイントとEnd-Effectorのベクトルを計算しています。Tは角度変換行列、Vで変換後の各ジョイントのベクトルをその都度取得しnp.arrayに格納。

def FK(L, TH):
    N = len(L)
    T = H('Z', L[0], TH[0])
    V = np.array(T[:3,-1])
    for i in range(1, N-1):
        T = T @ H('Y', L[i], TH[i])
        V = np.c_[V, T[:3,-1]]
    EE = T @ np.array([[1,0,0,1]]).T
    V = np.c_[V, EE[:3, -1]]
    return V

今回の場合は、
T = H('Z', [0,0,0], 0) @ H('X', [0,0,0.5], pi/2) @ H('X', [1,0,0], 0) @ H('X', [1,0,0], 0) @ EE
という順番で掛け合わせています(@はドット積)。
最後のEEはEnd-Effector=np.array([1,0,0,1]).T


IK(逆運動学)とヤコビ行列:

今回のヤコビ行列はクロス積で求めています(クロス積によるヤコビ行列についてはこちらを参照)。
np.cross(回転軸ベクトル, 各ジョイントベクトル)
回転軸がZ軸の場合は[0,0,1]、Y軸の場合は[0,-1,0]で反転させています。


角度制限:

以前の方法と同様、各ジョイントにおいて下限角度と上限角度を設定しておき、角度更新後にその角度を-180〜180度の範囲に変換してから制限値範囲外の場合は抑制し再更新します。
def angleLimit(TH, MinA, MaxA):
    THCOPY = TH.copy()
    for i in range(len(TH)):
        THCOPY[i] = TH[i] % tau
        if THCOPY[i] > pi:
            THCOPY[i] -= tau
        if THCOPY[i] < MinA[i]:
            THCOPY[i] = MinA[i]
        if THCOPY[i] > MaxA[i]:
            THCOPY[i] = MaxA[i]
    return THCOPY

インタラクティブモード:

右2つのグラフ(X-Y平面上かX-Z平面上の任意の座標)をマウスクリックすると、アーム先端(End-Effector)がその座標に移動します(まだ多少バグがあるかも)。左側3D表示内の座標をクリックすることはできませんが、視点の向きを変えることができます。
どのグラフ(X-Y平面上かX-Z平面上)をクリックしたかは、event.inaxesで判定しています。

def click(event):
    global TH, mx, my, mz
    if event.inaxes == A10.axes:
        mx = event.xdata
        my = event.ydata
    elif event.inaxes == A20.axes:
        mx = event.xdata
        mz = event.ydata
    else:
        pass
    # 以下省略


コード:



実践 ロボット制御: 基礎から動力学まで
実践 ロボット制御: 基礎から動力学まで
Posted with Buyer
prime
オーム社
売上ランキング: 77028

2021年5月10日月曜日

IK(逆運動学):同次変換行列/ヤコビ行列(クロス積)/角度制限

以前、同次変換行列とヤコビ行列(クロス積)を用いて逆運動学を求めてみましたが、今回はそのコードに角度制限を追加してみました。

環境:Python 3.8.5、Jupyter Notebook


この逆運動学アルゴリズムの特長:

  • 同次変換行列を用いる
  • ヤコビ行列(クロス積)を用いる
  • 角度制限を設ける

上図(4リンクの場合):

  • 原点(0,0)がベース
  • 赤い×が目標座標
  • リンク1角度制限:-180〜180度
  • リンク2〜4角度制限:-90〜90度

角度制限のため各リンクは手前のリンクに対して90度以上回転しないようにしています。各ジョイントにおいて任意のminAngleとmaxAngleを設定することができます。仕組みとしては前回のFABRIKとCCDの角度制限と同じです。


追加した角度制限のコード(以下):

def angleLimit(TH, MinA, MaxA):
    for i in range(N):
        TH[i] = TH[i] % tau
        if TH[i] < pi:
            TH[i] -= tau
        if TH[i] < MinA[i]:
            TH[i] = MinA[i]
        if TH[i] > MaxA[i]:
            TH[i] = MaxA[i]
    return TH

引数において、THは全ジョイントの角度リスト、MinAは角度制限の最低角度リスト、MaxAは最高角度リスト。角度は-180〜180度で表現しておきます。


ヤコビ行列を用いた逆運動学アルゴリズムの場合、最後に各ジョイントにおける回転角を求めるため、その角度が制限角度以上(あるいは以下)になった場合に、最高角度(あるいは最低角度)に更新するだけなので、それほど大きな変更点はありません。


全体のコード:



関連:
実践 ロボット制御: 基礎から動力学まで
実践 ロボット制御: 基礎から動力学まで
Posted with Buyer
prime
オーム社
売上ランキング: 77028

人気の投稿