Besttechnology - [FDIII-HC Starter Kit Guide/Chapter10.4 のバックアップソース(No.10)]

差分を表示
現在との差分を表示
FDIII-HC Starter Kit Guide/Chapter10.4 へ行く。

*逆運動で動かす [#o66404ae]

**三角関数を使用する [#zdb959c7]
逆運動(IK)とは指示された座標を元に関節の角度を求めるものです。~
色々な計算手法がありますが、今回のロボットハンドでは三角関数をメインに角度を計算してみたいと思います。~
尚、ハンド部分は角度計算に不要なので取り外します。~
#ref(RobotHand3D.png)
又、各アクチュエータと軸間の長さを次のように呼称することにします。
#ref(id_rename.png)

**2次元での角度算出 [#d90f6f7a]
では三角形を見てみましょう。~
三角形を作るのはD2、D3、posとなっています。~
#ref(triangle2D.png)
D2→D3、D3→posはそれぞれ軸の長さから求められるので、D2→posの長さが分かればこの三角形の各角度が算出可能となります。~
D2→posの長さは三平方の定理から求められます。~
#ref(inclined_D2pos.png)
三辺の長さが決まったので余弦定理を使ってD3の角度を算出してみましょう。~
#ref(cosineD3.png)
コサインの値が取得できたらアークコサインで角度にすることができます。~
ここでの角度はラジアンなので180を掛けてπ(円周率)で割れば度(デグリー)に変換することもできます。~

**3次元での角度計算 [#x04dbfff]
3次元においても角度の求め方は変わりませんので、D3の角度はそのまま使用します。~
#ref(cosineD2.png)
同様にD2の角度を求めますが、ここで求まるのはD2、D3、posを頂点とした三角形の内角です。D2への指令角度とは異なりますので、注意が必要です。
#ref(triangle3D.png)
指令値を出すため、もう一つ三角形を作ります。D2からZ軸に沿ってposの位置まで移動した点をjとし、D2、j、posを頂点とした直角三角形を作成します。D2の指令角度はこの三角形のD2の角度から先ほど求めたD2の角度を差し引きした角度になります。~

最後にD1の角度をアークタンジェントで求めれば角度計算は終了です。

**回転方向について [#b1298fca]
回転方向は軸を正から見て左回りになる方向をプラスとします。
#ref(roll_pitch_yaw.png)

**プログラミング [#bdefef6a]
ではプログラムしてみましょう。~
#html{{
<pre class="brush:c;toolbar:false">
#include <math.h>
#include <fd.h>
double pos[3] = { 80.0, 0, 50.0 };  // 座標(x, y, z)
const int L[4] = { 42, 27, 68, 40 };  // 軸間の長さ

void main (void) {
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  DX_ChangeBaudrate (1000000);
  fd_Wait (2000);
  // 必要な辺の長さを求めます
  double D2D3  = L[2];
  double D3POS = L[3];
  double D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + pos[2] * pos[2]);
  double D2j = pos[2] - (L[0] + L[1]);
  double jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]);
  // コサイン値を求めます
  double tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS);
  double tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS);
  double cosD3 = (D2D3 * D2D3 + D3POS * D3POS - D2POS * D2POS) / (2 * D2D3 * D3POS);
  // 角度を出します
  radD1 = atan2 (pos[1], pos[0]);
  radD2 = acos(tmpcos2) - acos(tmpcos1);
  radD3 = acos(-1) - acos (cosD3);
}
</pre>
}}

« Prev Next »

FDIII-HC Starter Kit Guide/Chapter10.4 のバックアップ一覧
FDIII-HC Starter Kit Guide/Chapter10.4 のバックアップソース(No. All)

メニュー

トップニュースお知らせ新商品情報更新情報マニュアル類技術情報チュートリアル FAQ お問合せ技術的なお問合せその他お問合せお見積りブログ会社案内

ショッピング

ご利用案内検索カテゴリ一覧ロボットキット Dynamixelシリーズマイコンボード相撲ロボットセンサ通信機器お客様ページ

FDIII-HC Starter Kit Guide​/Chapter10.4 のバックアップソース(No.10)

FDIII-HC Starter Kit Guide/Chapter10.4 のバックアップソース(No.10)