自作CNCマシン・レーザーカッターについて

2021年3月31日水曜日

Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学（その１）

前回はFABRIK法やCCD法でロボットアームの逆運動学を試してみましたが、今回はヤコビ行列を用いた方法を試してみます。

環境：Python3.8.5、Jupyter Notebook6.1.4

運動学：

運動学においては、各Jointの角度を入力すると、アーム先端のEnd-Effectorの座標が求まります。

3リンクアームの場合、Link1の角度θ1の時のx成分、Link2の角度θ1+θ2の時のx成分、Link3の角度θ1+θ2+θ3の時のx成分を単純に足し合わせればEnd-Effectorのx座標が得られます。y座標についても同様に求めると、End-Effectorの座標(x, y)は以下のように求まります。L1、L2、L3は各Link1、2、3の長さ。

x = L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3)
y = L1sin(θ1) + L2sin(θ1+θ2) + L3sin(θ1+θ2+θ3)

変化量 / ヤコビ行列：

運動学の関数をf、出力座標P=[x, y]、入力角度Θ=[θ1, θ2, θ3]とすると、

P = f(Θ)

となります。

逆運動学では、出力と入力が逆になるため、逆運動学の関数をgとすれば、

Θ = g(P)

になります。ちなみにgはfの逆行列でg(Θ)=f^-1(Θ)、最終的にこのような関係式を以下の手順で導いていきます。

まず運動学の式の両辺を時間tで微分し、単位時間における角度の変化量の関係式にすると

dP/dt = df(Θ)/dt

になります。これはプログラム上ではforやwhileループでの1ステップごとの変化量。

そして、df(Θ)をJと置き換えると、

ΔP = J(Θ)・ΔΘ

という角度変化量による位置変化量の関係式になり、この場合のJがヤコビ行列となります。

そしてJの中身は、先ほど運動学で求めたxとyをそれぞれの角度θ1、θ2、θ3で微分（偏微分）し、

J(Θ) = [[dx/dθ1, dx/dθ2, dx/dθ3],
         [dy/dθ1, dy/dθ2, dy/dθ3]]

となります。

それぞれを偏微分した結果は、

dx/dθ1 = - L1sin(θ1) - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3)
dx/dθ2 =           0 - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3)
dx/dθ3 =           0 -            0 - L3sin(θ1+θ2+θ3)

dy/dθ1 = L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3)
dy/dθ2 =         0 + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3)
dy/dθ3 =         0 +            0 + L3cos(θ1+θ2+θ3)

となります（ちなみにcosθの微分は-sinθ、sinθの微分はcosθ、0の部分は微分で消えた定数項）。

よってヤコビ行列Jは、

J(Θ) = [[- L1sin(θ1) - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3), - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3), - L3sin(θ1+θ2+θ3)],
        [  L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3),   L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3),   L3cos(θ1+θ2+θ3)]]

になります。2行はx, y座標（ベクトル）に対応、3列はJoint1、2、3に対応。

Jはその都度更新されるΘによって変化するので、Jを求める関数J(Θ)として表したほうがいいでしょう。

逆運動学 / 疑似逆行列：

逆運動学では目標座標に対してあとどのくらいJointの角度を回転させればいいのかを計算するため、その角度の変化量を求めます。Jの逆行列J^-1を用意して運動学の式の右辺と左辺を入れ替えて、

ΔΘ = J^-1(Θ)・ΔP

という関係式にすれば、目標座標に近づくための角度の補正量が求まります。

しかしながら、Jが正方行列ではないためJの逆行列は存在しません。2リンクであれば正方行列になりますが、Jointの数が増えるほど横長の行列になってしまいます。

ここで疑似逆行列を使うテクニックが必要となるようです。

その疑似逆行列J⁺（Moore-Penrose Pseudo-Inverse Matrix）は、

J⁺ = J.T・(J・J.T)^-1

J.TはJの転置行列。行列式の掛け算なのでドット積。

少し面倒ですがnumpyなら、

J⁺ = np.linalg.pinv(J)

ですぐに求まります（ちなみに正方行列の逆行列はnp.linalg.inv()を使う）。

よって、

ΔΘ = J⁺(Θ)・ΔP

になります。

ΔPは現時点でのEnd-Effectorの座標と目標座標との差分であるため、目標座標をP、現時点でのEnd-Effectorの座標を現時点での角度Θ_iと運動学の関数をfで表せばf(Θ_i)、

ΔΘ = J⁺(Θ_i)・(P - f(Θ_i))

になりΔΘが求まるため、次にどのくらいの角度で動かせばいいかわかります。

実際は、この式に刻み幅をより細かくするためのスケーラー：λ=0.1を掛け合わせて以下のようになります。

ΔΘ = J⁺(Θ_i)・(P - f(Θ_i))λ

つまりプログラム上では、現在の角度Θ_iにΔΘ=[Δθ1, Δθ2, Δθ3]を1ループごとに加算していき、

Θ_i+1 = Θ_i + J⁺(Θ_i)・(P - f(Θ_i))λ

ということになります。

疑似ヤコビ逆行列J⁺は、次のΘ_i+1が代入されることでそのつど更新されていきます。そして徐々にEnd-Effectorの位置が目標座標に近づいていきます。

手順としては：

運動学でEnd-Effectorの座標[x, y]を求める式を用意する
運動学の式を偏微分してヤコビ行列Jを求める
ヤコビ行列Jの疑似逆行列J+を求める
目標座標とEnd-Effector座標の差分とJ⁺を用いて必要な回転量を求める
スケーラーとしてλ=0.1程度を掛け合わせる
以後この操作を繰り返し徐々に目標に近づく

図形的に仕組みを見てみる：

計算式だけではイメージが湧かないので、図形的にヤコビ行列による操作を見ていきます。

ヤコビ行列を求める際にEnd-Effectorの座標(x, y)を各角度で偏微分しましたが（以下）、

dx/dθ1 = - L1sin(θ1) - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3)
dx/dθ2 =           0 - L2sin(θ1+θ2) - L3sin(θ1+θ2+θ3)
dx/dθ3 =           0 -            0 - L3sin(θ1+θ2+θ3)

dy/dθ1 = L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3)
dy/dθ2 =         0 + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3)
dy/dθ3 =         0 +            0 + L3cos(θ1+θ2+θ3)

これらの変化量は以下の図のような感じになっています。

Chart1：

dx/dθ1はJoint1を回転軸とした角度θ1の変化率で、Joint1だけを回転させる時、現状の角度にΔθ1加えるとEnd-EffectorがTargetに最も近くなります。

Chart2：

次にdx/dθ2に関しては、Joint2だけを回転させる時、現状の角度にΔθ2加えるとEnd-EffectorがTargetに最も近くなります。

Chart3：

同様にdx/dθ3に関しては、Joint3だけを回転させる時、現状の角度にΔθ3加えるとEnd-EffectorがTargetに最も近くなります。

Chart4：

これ以降はChart1に戻ってJoint1の回転操作から順に繰り返していきます。

このような手順で各Jointの角度の回転の割合が決まって、徐々にEnd-EffectorがTargetに近づいていきます。

これは以前に試したCCD法に似た手順です。CCD法では図形的に理解しましたが、ヤコビ行列で変化量を求めてそれぞれのJointを個別に回転させることは、同じようなことをやっているのではないでしょうか（証明していないので不明）。

コード：

最初にFKの計算式、そしてヤコビ行列を求めたあと疑似逆行列を求めてIKを計算しています。
numpyにはmatrixクラスもありますが、将来的に廃止となるようなので行列式にはnp.array()を使用しています。
np.array()の場合、行列同士の掛け算はドット積np.dot(A, B)で計算しますが、A@Bのように@でも計算可能です。
最後にインタラクティブにマウス座標を追従するプログラムがあるため、Jupyter Notebookのbackendは%matplotlib notebookを使用しています。

まだ不完全な部分も多いため、引き続き以下の点についても試してみようと思っています。

同次変換行列を用いる方法
特異点における問題点や回避策
回転角範囲の制約と設定
End-Effectorを固定したまま他のジョイントを動かす方法
クロス積を用いたヤコビアンの計算方法
3Dへの拡張

2021年3月25日木曜日

Inverse Kinematics 逆運動学：Backward Shift、FABRIK、CCD

ロボットアーム（マニピュレータ）における逆運動学のアルゴリズム。

今回は以下の方法について試してみます。

Forward + Shift
FABRIK（Foward and Backward Reaching Inverse Kinematics）
CCD（Cyclic　Coordinate　Descent）

環境：

Python3.6
Jupyter Notebook

運動学の場合、ロボットアームの各関節の角度を入力するとアーム先端（End-Effector）の座標値を求めることができます。

逆運動学の場合、End-Effector（アーム先端部）の座標値を入力すると各関節の角度が求められます。

上記の方法以外にヤコビ行列を用いた方法がありますが、それは次回へ。

Backward Shift：

この方法は、解析的に各関節の角度を求めるというより、図形的に回転と移動を繰り返すことで徐々に目標の座標に近似させていきます。Baseに近いほうからLink1、Link2、Link3という順番にしておき、Backwardの名前の通り、Link3から回転移動処理を始め、次にLink2、そしてLink1と操作を続けていきます。

Chart1：

アーム先端（End-Effector）の根元の関節Joint3から目標座標Target3に線を引いて角度θ3を得る。

Joint3を軸にLink3をθ3回転させる。

Chart2：

回転させたのち、End-EffectorとTarget3が一致するようにLink3をd3だけ平行移動する。

Chart3：

Link2とLink3の間に隙間ができるが、これでLink3の操作は一旦終了。

次はLink2の回転移動操作。目標座標はLink3下端（Target2）。

Chart4：

Joint2からTarget2へ向けて線を引き角度θ2を得る。

Joint2を軸にLink2をθ2回転させる。

Chart5：

回転させたのち、Link2上端とTarget2が一致するようにLink2をd2だけ平行移動する。

Chart6：

Link1とLink2の間に隙間ができるが、これでLink2の操作は一旦終了。

次のLink1についても同様の手順で動かす。目標座標はLink2の下端（Target1）。

Chart7：

Joint1からTarget1へ向けて線を引き角度θ1を得る。

Joint1を軸にLink1をθ1回転させる。

Chart8：

回転させたのち、Link1上端とTarget1が一致するようにLink1をd1だけ平行移動する。

Chart9：

Link1下端とBaseの間には隙間d0ができるので、Link1下端がBaseと一致するようにd0だけ全体的に平行移動する。

Chart10：

全体的に平行移動すると、End-Effectorと目標座標Target3との間に隙間ができる。

Joint3からTarget3へ線を引き、これ以降はChart1に戻って同じ処理を繰り返す。

最終的にはEnd-Effectorが目標座標Target3に限りなく近づく。

プログラム上では、この繰り返し処理に回数制限を設けるか、End-Effectorと目標座標との誤差が0.00001以下になったらループ処理を抜け出すかなどの設定にします。

コード：

リンク（Arm）のclassを用意して、長さ、両端座標、角度などを定義しておきます。
ならびに回転移動処理に必要なbackwardとshiftというメソッドも定義しておきます。
角度はnp.arctan2()で二点の座標から求めています。
変数Nを変えれば、Link数を増やすことができます。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
コード後半のmotion()関数で、ロボットアームがマウス座標を追従します（Jupyter上では動きはやや遅い）。

FABRIK：

この方法は、「Forward and Backward Reaching Kinematics」の略で、ForwardとBackwardの回転移動操作を繰り返して徐々に目標座標に近づいていきます。

前述したBackward ShiftのShiftさせた部分をForward（前方からの）の回転移動操作に置き換えたようなものです。

まず、End-Effectorのほうから目標座標に対して回転移動操作（Backward）し、Link1まで操作したら、今度はLink1、Link2、Link3という順番で回転移動操作（Forward）していきます。向きが変わるだけで基本的な手順は前述のChart1〜Chart8までの操作と同じです。

コード：

Linkのclassを用意して、長さ、両端座標、角度などを定義しておきます。
回転移動処理に必要なbackwardとforwardのメソッドも定義しておきます。
角度はnp.arctan2()で二点の座標から求めています。
変数Nを変えれば、Link数を増やすことができます。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。
コード後半のmotion()関数で、ロボットアームがマウス座標を追従します（Jupyter上では動きはやや遅い）。

CCD:

この方法は、２つのベクトル（回転軸から目標座標までのベクトル、回転軸からEnd-Effectorまでのベクトル）の角度の差分だけ回転させますが、Linkを回転させるというよりも、回転軸以降にあるJointの座標を回転移動させると言ったほうがいいでしょう。回転軸はJoint3、Joint2、Joint1という順番で移行し、それを繰り返します。

Chart1：

まずEnd-Effectorに近いJoint3を回転軸とする。

Joint3からTargetまでのベクトル、Joint3からEnd-Effectorまでのベクトル間の角度θ3を求める。

回転軸となるJoint3以降にあるJoint（この場合End-Effectorのみ）をθ3回転させる。

Chart2：

次の回転軸はJoint2。

Joint2からTargetまでのベクトルとJoint2からEnd-Effectorまでのベクトル間の角度θ2を求める。

Joint2以降のJoint（この場合、Joint3とEnd-Effector）をJoint2を中心にθ2回転させる。

Chart3：

次の回転軸はJoint1。

Joint1からTargetまでのベクトルとJoint1からEnd-Effectorまでのベクトルの角度θ1を求める。

Joint1以降のJoint（この場合、Joint2、Joint3、End-Effector）をJoint1を中心にθ1回転させる。

Chart4：

再度、回転軸はJoint3に戻り、Chart1同様の手順で回転移動させる。

あとはこの繰り返しで徐々にEnd-EffectorはTargetに近づいていく。

コード：

各Linkごとに動かすというよりも、回転軸となるJointを中心にそれ以降にあるJointの座標を回転移動させています。
CCD()ファンクション内の二重のforループは、最初のforループが回転軸用、次のforループが回転移動させるJoint座標用になります。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。

３つの比較：

どれも比較的簡単なアルゴリズムなので軽快に動きますが、Forward & Backward系とCCDでは少し動きに違いが出てきます。

以下は、マウス（赤x印）の動きに追従するインタラクティブなプログラム。３つの方法を重ねて同時に動かしています。

BackwardとFABRIKは同じアルゴリズムを使っているので似たような動きになります。

FABRIKは全体的に張りのある動きをしています。Backwardのほうはやや柔らかめ。

CCDはやや反応が遅く、先端に近いほうの動きとBaseに近いほうの動きに差が出て蛇行することがあります。

リンク数を多くすると、その動き方の違いも顕著になってきます。

リンク数20の場合。

マウスの動かし方にもよりますが、CCDは２つに比べるとやや不自然な形になっています。

結果、BackwardとFABRIKが反応も早く、動きも自然という感じです。それぞれの回転移動方法やコードも比較的簡単。

3つ同時のコード：

Backward Shift、　FABRIK、CCDの３つ方法をマウスに追従させることでインタラクティブに試すことができます。
変数Nを変えればリンク数を増やすことができます。
Jupyter Notebook上でインタラクティブ描画するためにバックエンドとして「#matplotlib notebook」を使用しています。

これらのアルゴリズムは、CGのキャラなどを動かすときに向いているようです。角度制限がないため実際のロボットアームに使うアルゴリズムには向いていないようです。

2020年10月14日水曜日

Ubuntu20.04へアップグレード（18.04から）

Ubuntu20.04がリリースされてから約半年が経過したので、そろそろ安定しているかなということで18.04からアップグレードしてみました。アップグレード通知が来ていたので、今回はコマンドを使わずGUIでアップグレード。

アップグレードのボタンを押すと約1時間半ほどでアップグレード完了で、特に難しい部分はありませんでした。途中で数回ほど諸設定を更新するかそのままにするか聞かれるくらいで、すべてボタンクリックで済みました。

Ubuntuの設定などは18.04から引き継いでいるようなので、設定し直す項目もなくシームレスにアップグレードされたという感じです。

ただ、アップグレードすると非対応のアプリケーションなどもあるので慣れ親しんだ18.04とは少し使い勝手が変わってしまう部分もありました。

その他バージョンアップしたもの：

・Anaconda最新版へ

・Python3.6から3.8へ

・cuda9.0から10.1へ

Python関連はAnaconda最新版を入れることでTensorflowやKerasなども最新版へ移行（特に問題なし）。

GPU（nvidia）関連は、nvidia-driver-455、nvidia-cuda-toolkit（11.1）をインストール。nvidia-cuda-toolkitは、Anaconda環境内でもインストールしたため、TensorflowやKerasは10.1で動作しているようです。以前はcuda関連のインストールが大変でしたが、まったく問題なし。今回はconda install pipでpip自体もAnaconda経由でインストールし、pipとcondaが混在しないようにしました。

問題ありの部分：

Tweakで設定できる機能拡張がGnome3.36に未対応のものも多く、それらは使えませんでした。

挙動が変な機能拡張はアンインストールかオフにしたので（上画像）、ほぼデフォルトに近い状態に戻ってしまったという感じです。Argosが便利でしたが使えない。

Tweaksが不安定：

Tweaks>全般>アニメーションをオフにすると左側のサイドバーがちらついてクリックできなくなる。

また、Tweakのメニューバーのメニューが消えてしまうという不具合もあります。ウィンドウ最大化するか、ウィンドウ左端を広げると直ります。

こんな感じ↑でメニューバーには何も表示されない。

そこで、ウィンドウ左端を左側に引き伸ばすとサイドバーが現れてメニューも表示されるようになります（以下）。

ちらついてタブがクリックできないという不具合もウィンドウを左側に広げると一応元に戻ります。どうやら、初期画面のサイズが小さすぎるのかもしれません。

Tweaksちらつき解決方法：

ここに書いてありました。

/usr/lib/python3/dist-packages/gtweak/tweakview.pyの154行目にあるself.listbox.set_size_request(200, -1)を

self.listbox.set_size_request(300, -1)に書き換えれば（要管理者権限）、初期画面が少し横に広がるのでちらつき問題は発生しないようです。実際書き換えて見たら直りました。

Wifiドライバ（接続が途切れる）：

この問題は相変わらずで、18.04のときと同様にドライバソフトを入れ替えました。その方法はこちら。

まとめ：

AnacondaやPython、cuda関連もアップグレードしているため、そのついでに20.04へ移行してしまったという感じです。18.04LTSはまだサポートが続いているので、18.04に慣れ親しんでいるならアップグレードしなくてもよかったかもしれません。おかげでPython環境は良好になったのですが、Ubuntuそのものはまだ使いづらいし、今後も多少のカスタマイズや修正が必要そう。

Ubuntu16.10から18.04へ移行したときはクリーンインストールしたために全て設定し直しましたが、今回はクリーンインストールではないため設定し直す部分も少なくて楽でした。

2019年12月27日金曜日

クォータニオン / Quaternion：立方体の回転操作（その3）

前回までは単位球に対する一点の回転操作でしたが、今回はもう少し具体的にということで立方体の回転操作を試してみました（基本的なクォータニオン回転操作についてはこちらへ）。

立方体の8個の頂点を個別に計算して回転操作しています。

また、scale変数で立方体の縦横高さの比率を調整できるようにし、pos変数で位置の基準点を変えられるようにしています。

手順としては：

・8頂点（ベクトル）のそれぞれのスカラー値を求める

・8頂点（ベクトル）をスカラー値で正規化

・正規化した8頂点（ベクトル）を回転操作

・正規化されている8頂点（ベクトル）をスカラー倍してもとのスケールに戻す

ipywidgetsでインタラクティブに角度を変えられるようになっています。また立方体の縦横高さの比率も変更可。
オンライン上で実行するにはBinderで可能です（方法についてはこちらを参考に）。

以下がコード（Gist）：

・回転角度と立方体の縦横高さ比が変更可能なバージョン
・回転角度と回転軸（単位ベクトル）が変更可能なバージョン
の二つがあります。

四元数

posted with amazlet at 19.12.22

今野紀雄
森北出版
売り上げランキング: 67,351

Amazon.co.jpで詳細を見る

Binder：オンライン上でのJupyter Notebook（ipynbファイル）の実行

ネット上で見かけるipynbファイルをブラウザに表示するにはnbviewerにアクセスして、

ipynbファイルのURLを入力してGo!ボタン。

そうするとJupyter Notebook形式でコードが表示されます。

このプログラムをこのままオンライン上で実行するには、ページ右上の「Execute on Binder」（以下の赤丸のアイコン）をクリック。

そうすると、Binderに移行し実行可能な環境ができあがり、オンライン上でJupyter Notebookが使用可能。

必要なモジュール/requirements.txt：

しかし、このプログラムに必要なモジュール（numpy、matplotlibなど）がこの環境にインストールされていなければエラーがでます。必要なモジュールについては、ipynbファイルとは別にrequirements.txtがアップロードされており、その中に記述されています。

Binder内のディレクトリ。

requirements.txtを開くと必要なモジュール名が書いてあります。requirements.txtがipynbファイルと同じディレクトリ内にあれば自動でモジュールがインストールされますが、もしrequirements.txtがなければ自力で必要そうなモジュールをインストールする必要があります。

ターミナルでモジュールをインストール：

ディレクトリのページに戻り、右上の「New」から「Terminal」を選択。

ターミナル画面が現れたら、pipなどで必要なモジュールをこの環境にインストール。

必要なモジュールが揃えば、プログラム実行可能なります。

直接「Binder」にアクセスして実行：

また、nbviewerを使わずに、直接「Binder」から実行するには、

Binderのトップページ。

プルダウンメニューからipynbファイルの場所の種類（GitHubやGistなど）を選択。ipynbファイルのURLを入力し「launch」ボタンをクリックでファイルが開きます。

requirements.txtがない場合はモジュールが自動インストールされないため、そのままプログラムを実行するとエラーがでるので、先ほど同様オンライン上のターミナルで自力で必要なモジュールをインストール。

PythonユーザのためのJupyter[実践]入門

posted with amazlet at 19.12.27

池内孝啓片柳薫子岩尾エマはるか @driller
技術評論社
売り上げランキング: 37,985

Amazon.co.jpで詳細を見る

登録: 投稿 (Atom)

2021年3月31日水曜日

Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学（その１）

運動学：

変化量 / ヤコビ行列：

逆運動学 / 疑似逆行列：

図形的に仕組みを見てみる：

コード：

2021年3月25日木曜日

Inverse Kinematics 逆運動学：Backward Shift、FABRIK、CCD

Backward Shift：

コード：

FABRIK：

コード：

CCD:

コード：

３つの比較：

3つ同時のコード：

2020年10月14日水曜日

Ubuntu20.04へアップグレード（18.04から）

2019年12月27日金曜日

クォータニオン / Quaternion：立方体の回転操作（その3）

Binder：オンライン上でのJupyter Notebook（ipynbファイル）の実行

人気の投稿