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

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



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


ラベル ヤコビ行列 の投稿を表示しています。 すべての投稿を表示
ラベル ヤコビ行列 の投稿を表示しています。 すべての投稿を表示

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月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

人気の投稿