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

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

FABRIKの角度制限の場合：

FABRIKの場合は、backward()とforward()の２つのメソッドがあり、それぞれに角度制限の手続きを追加しておきます。先程の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が角度制限ありメソッド。

2021年4月7日水曜日

IK（逆運動学）：同次変換行列、クロス積によるヤコビ行列（その２）

今回は、同次変換行列やクロス積を使って、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行列の上２行が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までのベクトルを求める。
この２つのベクトルをクロス積で掛け合わせx、yの変化率（速度）を求める
ith_J = [0, 0, 1] × (End_Effector_vector - ith_Joint_vector)

×はクロス積。クロス積は、XY平面上の２つのベクトルによってできる平行四辺形の面積を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列にｘ，ｙ，ｚの３要素が含まれているため、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にしてあります。