プログラムの修正 anchor.png

IKで求めた角度をモーションに組み込みます。

#include <math.h>
#include <fd.h>
#define fd_SetSpec(a,b,c)                   fd_SetSpec((PHomePosition)a,(PSpec)b,c)
#define fd_PlayMotion(a,b,c,d,e)            fd_PlayMotion((PPose)a,b,c,d,e)

const THomePosition HomePos = { 512, 512, 512 };
const TSpec Spec[] = {
  { 1, DEV_AX12, fd_AxisOfs(0), +341, { 0, 1023},   0, 1023, { 1, 1, 32,  32} },
  { 2, DEV_AX12, fd_AxisOfs(1), +341, { 0, 1023},   0, 1023, { 1, 1, 32,  32} },
  { 3, DEV_AX12, fd_AxisOfs(2), -341, { 0, 1023},   0, 1023, { 1, 1, 32,  32} },
};
const int L[4] = { 42, 27, 83, 55 };

TApplyPart HandParts = {  // 3DOFロボットハンド
  Priority: 1,
  PartNum:  3,
  Part: { fd_AxisOfs(0), fd_AxisOfs(1), fd_AxisOfs(2) }
};
// * モーションデータ
TPose WorkMotion = { Structure:{ 0, 0, 0 }, Adj:ADJ_SACC_SDECEL, Div:1000, };

double Degree (double rad) {
  return (rad / (atan(1) * 4)) * 180.0;
}

// プッシュボタンが押下されるとプログラムが終了します
void USER_TASK1 (void) {
  while (!fd_GetPB ()) {
    fd_Wait (10);
  }
  fd_SoftReset ();
}

void main (void) {
  double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3;
  double pos[3];
  int in[3], i;

  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);

  fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec));
  fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts);  // ホームポジションへ移動

  fd_Wait (2000);

  act_tsk (TASK1);

  D2D3  = L[2];
  D3POS = L[3];
  while (1) {
    fd_puts ("\nset position (x,y,z) = ");
    fd_scanf ("%d,%d,%d", &in[0], &in[1], &in[2]);  // 座標の指示
    for (i = 0; i < 3; i++) pos[i] = (double)in[i];
    D2POS = sqrt (pos[0] * pos[0] + pos[1] * pos[1] + (pos[2] - (L[0]+L[1])) * (pos[2] - (L[0]+L[1])));
    D2j = pos[2] - (L[0] + L[1]);
    jPOS = sqrt (pos[0] * pos[0] + pos[1] * pos[1]);
    tmpcos1 = (D2D3 * D2D3 + D2POS * D2POS - D3POS * D3POS) / (2 * D2D3 * D2POS);
    tmpcos2 = (D2j * D2j + D2POS * D2POS - jPOS * jPOS) / (2 * D2j * D2POS);
    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);
    WorkMotion.Structure[0] = (int)(Degree(radD1) * 10);  // D1の角度
    WorkMotion.Structure[1] = (int)(Degree(radD2) * 10);  // D2の角度
    WorkMotion.Structure[2] = (int)(Degree(radD3) * 10);  // D3の角度
    fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts);    // モーションの再生
    while (fd_GetMotionStat (&HandParts)) fd_Wait (10);
  }
}

このプログラムはSIMPLE TERM(ターミナルプログラム)から座標を指定できるようになっており、プッシュボタンが押されると終了してしまいます。

Page Top

まとめ anchor.png

本章ではFREEDOM III libraryに用意された関数を使用してモーションでロボットハンドを動かしてみました。
又、指定された座標から角度を算出しました。今回は一つの考え方を説明しましたが、IKによる計算方法は様々な手法が考えられており、ほとんどが専門の知識を必要とします。これを機に調べてみるのも良いかも知れません。

Page Top

課題 anchor.png

指定された座標によってはプログラムの途中で計算が破綻をきたし、不正な角度を設定してしまいます。これを回避するよう、プログラムを修正して下さい。
ヒントは計算の破綻は決まった関数(acos/atan2)で起こります。
又、取り外したハンド部分のAX-12+を分解してアーム部分を延長することで、産業用のロボットアームと同様の構成にすることができます。

agenda_arm.png

FDIII-HC Starter Kit Guideへ戻る


Front page   Diff ReloadPrint View   Page list Search Recent changes   RSS of recent changes (RSS 1.0) RSS of recent changes (RSS 2.0) RSS of recent changes (RSS Atom)
Last-modified: 2013-08-07 (Wed) 20:32:08 (JST) (2671d)