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つの交点を返しますが、今回は一方の交点(上側の交点座標)を利用。



AliExpress.com Product - SG90 MG90S 4 DOF Unassembly Acrylic Mechanical Arm Robot Manipulator Claw for Arduino Maker Learning DIY Kit Robot

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

人気の投稿