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

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



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


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

2021年4月30日金曜日

IK(逆運動学):アーム可動域制限(角度制限)CCDとFABRIKの場合

以前、逆運動学のFABRIKとCCDを実装しましたが、各アームの可動域が無制限だったので今回は角度制限を追加してみました。

環境:Python3.8.5、Jupyter Notebook


小型サーボの場合、通常0〜180度程度の可動域しかないので、同じような条件にしてみました。設定変数によって可動域は変えられるようにしてます。

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

  • Link1は地面に対して鉛直方向を90度、Joint1を回転軸として0〜180度に設定(絶対角度)
  • Link2の可動域はLink1に対してJoint2を回転軸として-90〜90度に設定(相対角度)
  • Link3の可動域はLink2に対してJoint3を回転軸として-90〜90度に設定(相対角度)
それぞれのJointにおいてminAngleとmaxAngleを上記条件で設定しておき、例えば角度計算においてmaxAngle以上の角度が得られた場合は最大角度をmaxAngleになるように抑制します。要はこれまでのFABRIKやCCDの1ループの通常計算のあとに、その都度角度制限の補正を加えるという方法です。

CCDの角度制限の場合:

  • まずLinkの角度を-180〜180度に変換するconvTheta()を用意し角度表現を統一しておく
  • Armクラス内コンストラクタにminAngleとmaxAngleのパラメータを追加(デフォルト値を設定)
  • Armクラス内にangleLimit()メソッドを追加して角度制限する
def convTheta(theta):
    theta = theta % tau
    if theta > pi:
        theta = theta - tau
    return theta
    
class Arm:
    def __init__(self, ax, ay, length, angle, minAngle=-pi/2, maxAngle=pi/2):
        self.ax = ax
        self.ay = ay
        self.length = length
        self.angle = convTheta(angle)
        self.bx = self.ax + self.length * cos(self.angle)
        self.by = self.ay + self.length * sin(self.angle)
        self.minAngle = convTheta(minAngle)
        self.maxAngle = convTheta(maxAngle)
    
    def angleLimit(self, prevTheta, newTheta):
        theta = convTheta(newTheta - prevTheta)
        if theta < self.minAngle:
            theta = self.minAngle
        elif theta > self.maxAngle:
            theta = self.maxAngle
        self.angle = theta + prevTheta
        self.bx = self.ax + self.length * cos(self.angle)
        self.by = self.ay + self.length * sin(self.angle)

メソッドangleLimit()の引数prevThetaは一つ手前のLinkの角度、newThetaは操作後の角度。self.angleの角度を含め、すべて絶対座標上での角度として計算します。
newThetaからprevThetaを差し引いた角度thetaがself.minAngle以下ならself.minAngleのまま(maxAngleも同様に計算)。最終的に補正された角度self.angleによって、self.bxとself.by(各LinkのEnd-Effector寄りの端点)を更新するという手順になってます。
角度制限のデフォルト値は-pi/2〜pi/2(-90〜90度)に設定してあるので、無記入ならデフォルト値が適用されます。



上図:比較(3リンクアームCCDの場合):
CCD(青)が角度制限なしの計算方法、CCD_AL(ピンク)が角度制限ありの計算方法、赤いx印が目標座標。
角度制限なし(青)の方は、Link2とLink3がもう少しで重なりそうになっていますが、角度制限あり(ピンク)の方では、一つ手前のLinkに対して90度以上回転しないように設定してあるため、このような状況のときにはそれぞれのLinkは90度を保ったまま動こうとします。
角度制限ありの場合は、制限されている分、目標座標に到達できないこともあります。暫定的な角度制限アルゴリズムなので、まだ不完全な部分もあります。

上図:比較(4リンクアームCCDの場合):
角度制限なし(青)のほうでは、Linkが自在に動くためLink2とLink4が交差していますが、角度制限あり(ピンク)のほうは最大角度90度を保ったまま目標座標に到達しています。単に90度以上回転しないようにしているだけなので、Link数を増やせば角度制限ありのほうでも交差することはあります。しかしながら、角度制限なしに比べれば、より現実的な動きに近づいています。


CCD_AngleLimitのコード:
__init__関数(コンストラクタ)の最後2つのパラメータminAngleとmaxAngleで角度を制限します。最後のmotion()関数がマウスに追従するプログラムとなります。ゆっくりマウスを動かせば制限された角度を保ちながら一応動きます(まだ不完全な部分あり)。




FABRIKの角度制限の場合:

FABRIKの場合は、backward()とforward()の2つのメソッドがあり、それぞれに角度制限の手続きを追加しておきます。先程のCCDと同様にconvTheta()で角度を-180〜180度に変換しておきます。


以下が角度制限なしのbackward()メソッド。
def backward(self, tx, ty):
    theta = np.arctan2(ty - self.ay, tx - self.ax)
    self.bx = tx
    self.by = ty
    self.ax = tx - self.length * cos(theta)
    self.ay = ty - self.length * sin(theta)
    self.angle = convTheta(theta)
  
そして以下が角度制限ありのメソッド。
def backward2(self, tx, ty, prevTheta):
    theta = convTheta(np.arctan2(ty - self.ay, tx - self.ax) - prevTheta)
    if theta < self.minAngle:
        theta = self.minAngle
    elif theta > self.maxAngle:
        theta = self.maxAngle
    self.angle = convTheta(theta + prevTheta)
    self.bx = tx
    self.by = ty
    self.ax = self.bx - self.length * cos(self.angle)
    self.ay = self.by - self.length * sin(self.angle)
backward/backward2の場合は、End-Effector側から各Linkを回転移動していくため、各Linkの回転軸はEnd-Effector寄りの端点(self.bx,self.by)となり、ベース寄りの端点座標(ax,ay)が角度制限によって補正されます。その逆で、forward2の場合は(self.ax,self.ay)が回転軸となり、(self.bx,self.by)の座標が補正されます。backward2の引数prevThetaは一つ手前のLinkの角度であり、角度制限において相対角度を計算するために必要となります。


FABRIK_AngleLimitのコード:
backward/forwardが角度制限なしメソッド、backward2/forward2が角度制限ありメソッド。


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

2021年4月7日水曜日

IK(逆運動学):同次変換行列、クロス積によるヤコビ行列(その2)

今回は、同次変換行列やクロス積を使って、2Dロボットアームの動きを逆運動学で求めてみます(前回はこちら)。

環境:Python3.8.5、Jupyter Notebook

*numpy.matrix()は将来的に削除されるのでnumpy.array()を使用しています。

*尚、コーディングにおいての行列の掛け算(ドット積)は:

A・B = numpy.dot(A, B) = A.dot(B) = A @ B


今回試す内容:

  • 同次変換行列を用いる
  • ヤコビ行列をクロス積(外積)で求める
  • 任意のリンク数に対応させる(以下画像:N=20の場合)


同次変換行列(FK):

各ジョイントに対応した同次変換行列を数珠つなぎに掛け合わせることで各端点のベクトルが簡単に求められます。2Dなので3x3マトリクスでも足りるのですが、後々の3Dのために4x4マトリクスを使うことにします。
前回までは、ジョイント1の回転角をθ1とすればジョイント2の回転角はθ1+θ2としていましたが、今回の場合は単純に相対角度であるθ2だけ入力すればいいので計算しやすくなります。

2Dにおける同次変換行列をHとすれば、
H = [[cos(θ), -sin(θ), 0, x],
     [sin(θ),  cos(θ), 0, y],
     [     0,       0, 1, 0],
     [     0,       0, 0, 1]]
3Dでは左上3x3が回転用、右端3x1が平行移動用ですが、今回のような2Dロボットアームであれば、xにリンク長、y=0を代入して以下のようになります。
H = [[cos(θ), -sin(θ), 0, L],
     [sin(θ),  cos(θ), 0, 0],
     [     0,       0, 1, 0],
     [     0,       0, 0, 1]]
HをLとθを引数とした関数にすれば、3リンクアームの場合、
V = H(0,θ1)・H(L1,θ2)・H(L2,θ3)・[L3,0,0,1].T
によってV=[x,y,0,1].Tが求まり、(x, y)を取り出せばアーム先端(End-Effector)の2Dベクトルになります。
コーディングでは以下のような感じ。
def H(L, TH):
    return np.array([[np.cos(TH), - np.sin(TH), 0, L],
                     [np.sin(TH),   np.cos(TH), 0, 0],
                     [         0,            0, 1, 0],
                     [         0,            0, 0, 1]])

L  = [1, 1, 1]
TH = np.radians([30, 30, 30])
EE = np.array([[L[2], 0, 0, 1]])
V = H(0, TH[0]) @ H(L[0], TH[1]) @ H(L[1], TH[2]) @ EE.T
print(V)
最後に掛けているEE.Tは、End-Effectorのベクトルです。@はドット積。
そうすると、出力は以下。
[[1.3660254]
 [2.3660254]
 [0.       ]
 [1.       ]]
4x1行列の上2行がEnd-Effectorのベクトル(x, y)になります。
前述のコーディングでは、ジョイントの数だけH(L, TH)を掛け合わせましたが、実際はfor文で繰り返し処理します。



クロス積でヤコビ行列を求める:

運動学は同次変換行列によって求められたので、次に逆運動学の下準備としてヤコビ行列を求めます。前回は、運動学によって求まるEnd-Effector座標のxとy成分の計算式を各ジョイントの回転角θ1、θ2、θ3によって偏微分しました(以下)。

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)]]

今回はクロス積によってこのヤコビ行列を求めます。
  • ジョイントの回転軸の単位ベクトル(Z軸ベクトル[0,0,1])を求める。
  • 各回転軸からEnd-Effectorまでのベクトルを求める。
  • この2つのベクトルをクロス積で掛け合わせx、yの変化率(速度)を求める
  • ith_J = [0, 0, 1] × (End_Effector_vector - ith_Joint_vector)
×はクロス積。クロス積は、XY平面上の2つのベクトルによってできる平行四辺形の面積をXY平面に直行するZ軸方向のベクトルとして表します(以下)。
A×B = |A||B|sin(θ)[0,0,1]
ちなみにドット積は(以下)、
A・B = |A||B|cos(θ)


左図:

Joint1を回転軸とする場合、Joint1からEnd-EffectorまでのベクトルをV1、Z軸の単位ベクトル[0, 0, 1]をUV、End-Effectorの回転速度VE1とすると、
UV = [0, 0, 1]
V1 = End_Effector_vector - Joint1_vector
VE1 = UV × V1
になります。
Z軸の単位ベクトル[0, 0, 1]にV1をクロス積で掛け合わせると、「右ねじの法則」によってV1に直行する黄色実線のベクトルが得られます。また、End-Effectorにおける回転速度はV1に対して垂直な黄色破線として表され、これも同様に[0, 0, 1] × V1で表すことができます。
要はEnd-Effectorの速度ベクトルVE1は、V1を90度反時計回りに回転させたベクトルと同じになります。あるいは、End-Effectorにおける接線に沿ったベクトル(V1のx成分とy成分を-1倍したものを入れ替えたベクトル)になります。V1の角度をθv1とすれば以下。
V1  = [ |V1|cos(θv1), |V1|sin(θv1)]
VE1 = [-|V1|sin(θv1), |V1|cos(θv1)]

右図:

同様に、Joint2からEnd-EffectorまでのベクトルをV2、Joint3からEnd-EffectorまでのベクトルをV3とすれば、それぞれの回転軸で回転させたときのEnd-Effectorの接線方向の速度ベクトルが求まります。これで、V1、V2、V3の速度の割合が求まります。
V1、V2、V3は速度ベクトルなので、プログラム上では任意にスケールダウンして、1ループあたりの移動変化量を調整できます。

クロス積を用いた場合は、偏微分も各ジョイントにおける角度も必要なく、単なるベクトルだけの計算になるためシンプルです。
UV  = [0, 0, 1]
VE1 = UV × V1 = [- |V1|sin(θv1), |V1|cos(θv1), 0]
VE2 = UV × V2 = [- |V2|sin(θv2), |V2|cos(θv2), 0]
VE3 = UV × V3 = [- |V3|sin(θv3), |V3|cos(θv3), 0]
よってヤコビ行列Jは、
J = [UV × V1, UV × V2, UV × V3]
実際は1列にx,y,zの3要素が含まれているため、3xNの行列になります(Nはリンク数)。
コードでは以下。
def Jacobian(V):
    UV = np.array([0, 0, 1])
    J = []
    for i in range(N):
        J.append(np.cross(UV, V[:, -1] - V[:, i]))
    return np.array(J).T
Vは各ジョイントのベクトルで、V[:, -1]はEnd-Effectorのベクトル、V[:, i]は回転軸となるジョイントのベクトル。



IK(逆運動学):

今回は任意のリンク数に対応できるように、同次変換行列Hをfor文で繰り返しFK()という運動学の関数に組み込んでおきます。
def H(L, TH):
    return np.array([[np.cos(TH), - np.sin(TH), 0, L],
                     [np.sin(TH),   np.cos(TH), 0, 0],
                     [         0,            0, 1, 0],
                     [         0,            0, 0, 1]])

def FK(L, TH):
    N = len(L)
    T = H(0, TH[0])
    V = np.zeros(3)
    for i in range(N-1):
        T = T @ H(L[i], TH[i+1])
        V = np.c_[V, T[:3, -1]]
    EE = T @ np.array([[L[-1], 0, 0, 1]]).T
    V = np.c_[V, EE[:3, -1]]
    return V
ヤコビ行列の疑似逆行列は前回同様numpy.linalg.pinv(J)で求めることにします。
ちなみに疑似逆行列は、pinv(J)=J.T ・(J・J.T)-1
while True:
    V = FK(L, TH)
    J = Jacobian(V)
    Error = Target - V[:, -1]
    if norm(Error) < 1e-4:
        break
    dTheta = pinv(J) @ Error * scaler
    TH += dTheta
4行目のV[:, -1]はEnd-Effectorのベクトルで、Target=[x,y,z]との差分をErrorとして疑似逆行列と掛け合わせています(@はドット積)。scalerは刻み幅を細かくするための係数(scaler=0.1〜0.01程度)。
結果的にΔθ:dThetaが求まり、dThetaを現在の角度THに加算してTargetに近づいていきます。誤差が0.0001未満になったらループ終了。


コード:

  • 変数Nでリンク数を増やすことができます。
  • インタラクティブモード(アームをマウスに追従させる)があるためbackendは%matplotlib notebookにしてあります。


関連:

AliExpress.com Product - Metal mechanical arm Multi-degree of freedom manipulator Industrial robot model Six-axis robot 202

2021年3月31日水曜日

Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学(その1)

前回は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を使用しています。



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

AliExpress.com Product - Metal mechanical arm Multi-degree of freedom manipulator Industrial robot model Six-axis robot 202

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:

この方法は、2つのベクトル(回転軸から目標座標までのベクトル、回転軸から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」を使用しています。



3つの比較:

どれも比較的簡単なアルゴリズムなので軽快に動きますが、Forward & Backward系とCCDでは少し動きに違いが出てきます。
以下は、マウス(赤x印)の動きに追従するインタラクティブなプログラム。3つの方法を重ねて同時に動かしています。
BackwardとFABRIKは同じアルゴリズムを使っているので似たような動きになります。
FABRIKは全体的に張りのある動きをしています。Backwardのほうはやや柔らかめ。
CCDはやや反応が遅く、先端に近いほうの動きとBaseに近いほうの動きに差が出て蛇行することがあります。
リンク数を多くすると、その動き方の違いも顕著になってきます。
リンク数20の場合。
マウスの動かし方にもよりますが、CCDは2つに比べるとやや不自然な形になっています。
結果、BackwardとFABRIKが反応も早く、動きも自然という感じです。それぞれの回転移動方法やコードも比較的簡単。

3つ同時のコード:

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

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

人気の投稿