3DCG

Python3で「OpenGL」を学ぶ 「PyOpenGL」PyCG lesson5

  /

lesson5を学んだ。順にそれぞれ自分の理解をメモしながらまとめる。

逆運動学（Inverse Kinematics）を学ぶ。その具体例にはロボットアームがある。アームの先端が正確な位置に来るための、各関節（セグメント）の制御の仕方を考える。ある物体の動きを、別のデータから復元する数学的なプロセスと言い換えることができる。本稿では、腕の先端が設定されている場合に、腕のセグメントを正しく配置する方法を考える。

前方運動学

数学的に解くために、ロボットアームのモデルを使う。初めに一本の腕のセグメントを考える。一本の腕のセグメントにはピッチとヨーの2つの自由度（オイラー角）がある。この腕のセグメントの長さ、ピッチ、ヨーをそれぞれ l1, p1, y1 とする。その基礎部を原点とすると、先端位置は次の通りだ。


x1=l1 cos(p1) cos(y1)

y1=l1sin(p1)

z1=l1cos(p1)sin(y1)

次に2つのセグメントがある場合を考える。重要な点は、2番目のセグメントが1番目のセグメントのピッチとヨーの値を継承することである。（第2のセグメントの実際のピッチとヨー値は第1のセグメントに依存している）同様の表記法を用いて、2番目のセグメントの基礎部（1番目のセグメントの先端）に対する先端位置は次の式で得られる。

x2=l2cos(p1+p2)cos(y1+y2)

y2=l2sin(p1+p2)

z2=l2cos(p1+p2)sin(y1+y2)

この考え方を一般化させて式にして実装する。

逆問題の分析

n個のアームセグメントに対して2n個のパラメータがある。3次元空間における先端位置しか情報がないため、この問題を閉じた形で解くことは困難である。実際のロボットアームは2つの位置の間をスムーズに移動する傾向があるため、2つの状態間の遷移もスムーズになる。つまり、反復的な解が望ましい。

このモデルのすべてのパラメータを 1 つの長いベクトル u に連結すると以下のように表現できる。

u=(p1,p2,…,pn,y1,y2,…,yn)T

=(u1,u2,…,un,un+1,un+2,…,u2n)T

ヤコビアン行列

ヤコビアン行列 Jf(u) は，点 u 付近の f の最良の線形近似を提供する。uの周りの線形近似は次のようになる。

f(u+Δ)≈f(u)+Jf(u)Δ

逆運動学の問題を解く時、ロボットアームは初期値uが必要である。つまり、f(u+Δ)、f(u)、Jf(u)が何であるかがわかっているため、以下のように、Δを求めたい。

Jf(u)Δ=f(u+Δ)−f(u)

Jf(u) は正方行列ではないため、この方程式は閉じた形で解くことができない。よって、線形最小二乗法を用いて近似を行うしかない。Δについて、|| Jf(u)Δ-[f(u+Δ)-f(u)] ||を最小化する。Δの最小二乗解は、ムーアペンローズ逆行列を用いて計算できる(QR分解も可能)。こうして、新しいパラメータを u+Δで更新できる。

デモ

滑らかなアニメーション（均一な間隔を持つ）を生成するために、パラメータシーケンス全体に線形補間を適用している。

コードはGitHubを参照。動きが早すぎる場合は、numFramePerLoopの値を調整すると良さそう（私は3000にした）。

  • “W, A, S, D” と、マウスを使ってシーンを移動する
  • “ESC” を押下で、終了
  • “p “を押下で、スクリーンショットを撮る

また、自分の環境では、そのままでは動かなかったため、以下の2つの対応を行った。scipyが足りない場合は以下のコマンドを実行。

pip install scipy

以下のエラーが出ている場合は、

Traceback (most recent call last):
  File "main.py", line 2, in <module>
    from gl_lib.transmat import *
ModuleNotFoundError: No module named 'gl_lib'

main.py の先頭に以下を追記。(Tutorial_4のmain.pyなどを参考)

import sys, os

lastFolder = os.path.split(os.getcwd())[0]
sys.path.append(lastFolder)

RELATED POST