逆運動学ソルバーを実装しようとしていますが、今回はエンド エフェクタの向きも含めます。エンドエフェクタが位置のみを必要とする場合に成功しました。
この場合、ヤコビ行列を次のように構築できることを学びました。ここで、w_i はグローバル空間の i_th 回転軸で、p_i は i_th 軸からターゲット位置へのベクトルです。
問題は、以下の式で x_dot を計算する必要がある場合です。
x_dot が考慮すべき位置のみを持ち、方向がない場合、これは非常に単純でした。しかし、x_dot に 6 つのエントリ (位置、向き) が必要な場合、向きの部分をどうすればよいかわかりません。プログラムで方向を表すためにオイラー角を使用してきました。
現時点で考えているアイデアは、現在のエンド エフェクターのヨー、ピッチ、およびロールをターゲットのヨー、ピッチ、およびロールで減算し、それぞれの結果を 100 で割ることです。しかし、これは少し複雑に思えます。この問題に対処するためのより良い方法はありますか? どんなアイデアでも大歓迎です！
エンド エフェクタの方向を 3 x 3 の回転行列として表す必要があります。現在のジョイント ベクトル (Theta) でエンド エフェクタの方向を計算し、次にジョイント ベクトルの各要素に小さな増分を追加します (この場合、ジョイントが 6 つあるため、6 つの増分)。関節の小さな変化の結果生じる先端位置の変化に関心がある単純なケースでは、X、Y、および Z 位置の変化を計算しました。これは、シータでの位置の単純なベクトル減算であり、それぞれについて摂動シータ。角度について同じことを行うには、3x3@Theta (A) を 3x3@ThetaPrime(B) にする回転行列 R を見つける必要があります。
A*R=B なので
AInv A R=AInv*B
AInve*A = アイデンティティ
R=AInv*B
R から、デルタ ロール、ピッチ、ヨー オイラー角を抽出できます。公式はこちら
https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf
シータ値は、各シータの変化によって生じるヨー、ピッチ、ロールの変化です。