FDIII-HC Starter Kit Guide​/Chapter10.3 :: Besttechnology

ロボットハンドを動かす ^[1]

Dynamixel Configuratorで動かす ^[2]

まずは確認のため、Dynamixel Configurator^[3]でAX-12+を作動させてみましょう。トルク設定をしていませんので、ポジション移動の際は十二分に注意して下さい。
ID1からID6まで以下の手順を繰り返します。

1. 「Read All」で状態を読み込む。
2. ダイヤルでポジションを移動する。

物を掴む ^[4]

ID1~ID3でハンド部分の位置が決まり、ID5、6でハンド部分が開閉します。
空のペットボトル等を掴んで持ち上げてみましょう。

^[5]

如何でしょうか。持ち上げることができたでしょうか。
Dynamixel Configuratorでは、個々のアクチュエータにそれぞれ手動で指令を行わなければならず、随分と手間がかかるのではないでしょうか。
ID1のゴールポジションを変更することで右へ旋回します。旋回後、腕を下して掴み上げたものを下してください。
では、Dynamixel Configuratorでの操作をプログラムで再現してみましょう。

プログラムする ^[6]

ロボットハンドの左につかむ物を設置して下さい。腕の先へ掴めるものが来るように配置しましょう。

#include <fd.h>
// メイン関数
void main (void) {
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);
  fd_Wait (2000);
  // 基本姿勢の作成
  fd_DXSetPosition (1, 512);
  fd_DXSetPosition (2, 512);
  fd_DXSetPosition (3, 512);
  fd_DXSetPosition (4, 512);
  fd_DXSetPosition (5, 512);
  fd_DXSetPosition (6, 512);
  fd_Wait (1000);

  // 左へターン
  fd_DXSetPosition (1, 605);
  fd_Wait (500);
  // 開く
  fd_DXSetPosition (5, 362);
  fd_DXSetPosition (6, 662);
  fd_Wait (1000);
  // 腕を下げる
  fd_DXSetPosition (2, 700);
  fd_DXSetPosition (3, 400);
  fd_Wait (1000);
  // 挟む
  fd_DXSetPosition (5, 542);
  fd_DXSetPosition (6, 482);
  fd_Wait (1000);
  // 持ち上げる
  fd_DXSetPosition (2, 512);
  fd_DXSetPosition (3, 512);
  fd_Wait (1000);
  // 右へ移動
  fd_DXSetPosition (1, 419);
  fd_Wait (1000);
  // 腕を下げる
  fd_DXSetPosition (2, 700);
  fd_DXSetPosition (3, 400);
  fd_Wait (1000);
  // 離す
  fd_DXSetPosition (5, 362);
  fd_DXSetPosition (6, 662);
  fd_Wait (1000);
  // 基本姿勢へ戻る
  fd_DXSetPosition (1, 512);
  fd_DXSetPosition (2, 512);
  fd_DXSetPosition (3, 512);
  fd_DXSetPosition (4, 512);
  fd_DXSetPosition (5, 512);
  fd_DXSetPosition (6, 512);
  fd_Wait (1000);
}

今回はDynamixel Configuratorでの操作をそのままプログラムしました。指定位置は掴むものに合った値に変更して下さい。
いきなり動き出したり、このプログラムでは動きが随分とぎこちないですね。もう少し滑らかに動くように修正します。

モーション ^[7]

FREEDOM Ⅲライブラリ^[8]には、モーションに関するAPIが用意されていますので、動作をモーションに置き換えてみましょう。

概要 ^[9]

モーションを使用するには必要なidの宣言と初期設定値等を取りまとめたTSpec構造体^[10]を初期処理で使用します。
又、ホームポジションを設定するため、THomePosition構造体^[11]を合わせて使用します。ホームポジションは各アクチュエータの基準になり、モーションの指定はホームポジションに対する角度の指定となります。
モーションデータを再生するには、どの部分を稼働させるかを指示するTApplyPart構造体^[12]と、実際のモーションデータであるTPose構造体^[13]を使用します。

初期化 ^[14]

初期化には次の様にAPIを使用します。

fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec));

fd_SetSpec^{[15] [16]}

指定された値を使用して、初期設定を行います。
第1引数は、ホームポジションを指定したTHomePosition構造体^[11]変数のアドレスです。
第2引数は、アクチュエータの情報を指定したTSpec構造体^[10]変数のアドレスです。
第3引数は、TSpec配列数です。

THomePosition構造体^{[11] [17]}

基準となる位置を指定します。

const THomePosition HomePos = { 512, 512, 512, 512, 512 , 512 };

6軸のアクチュエータに対し、512(出力軸は150°)を指定しています。

TSpec構造体^{[10] [18]}

各初期情報を指定します。

const TSpec Spec[] = {
  { 1, DEV_AX12, fd_AxisOfs(0),  +341, {  257,  768},   0, 1023, { 1, 1, 32,  32} },
  { 2, DEV_AX12, fd_AxisOfs(1),  +341, {  390,  818},   0, 1023, { 1, 1, 32,  32} },
  { 3, DEV_AX12, fd_AxisOfs(2),  +341, {   64,  512},   0, 1023, { 1, 1, 32,  32} },
  { 4, DEV_AX12, fd_AxisOfs(10), +341, {    0, 1023},   0, 1023, { 1, 1, 32,  32} },
  { 5, DEV_AX12, fd_AxisOfs(11), +341, {    0,  592},   0, 1023, { 1, 1, 32,  32} },
  { 6, DEV_AX12, fd_AxisOfs(12), +341, {  432, 1023},   0, 1023, { 1, 1, 32,  32} },
};

AX-12+のIDを1~6と設定しているので、それらをオフセット0~2と10~12へ割り当てます。オフセットは60の配列の為、59まで使用可能となっており、TApplyPart^[12]で指定するオフセットと対応していればどのような順番であっても問題ありません。

モーション再生 ^[19]

モーションを再生するには次の様にAPIを使用します。

fd_PlayMotion (GoToHomePosition, 1, sizeof(GoToHomePosition)/sizeof(TPose), 100, &HandParts);

fd_PlayMotion^{[20] [21]}

指定されたモーションを指定部位にて再生します。
第1引数は、モーションデータを指定したTPose構造体^[13]変数のアドレスです。
第2引数は、モーションの開始位置を指定します。
第3引数は、モーションの終了位置を指定します。
第4引数は、モーションの再生スピード(%)を指定します。
第5引数は、モーションを再生部位を指定したTApplyPart構造体^[12]変数のアドレスです。

TPose構造体^{[13] [22]}

ホームポジションからの角度情報を指定します。ポーズが1つでも、配列として宣言します。

const TPose GoToHomePosition [] = {
  { Structure:{ 0, 0, 0, 0, 0, 0 },  Adj:ADJ_SACC_SDECEL, Div:2000, },
};

上記は6軸全てを0°としており、全てのAX-12+をホームポジション位置へ移動させます。

TApplyPart構造体^{[12] [23]}

アクチュエータを取りまとめた部位の設定を行います。
アクチュエータの指定はIDではなく、TSpec^[10]で宣言したオフセット値を使用します。
又、1つのアクチュエータを複数の部位で使用しても問題ありませんが、その場合はPriorityが有効になります。

TApplyPart ArmParts = {
  Priority: 10,
  PartNum:  3,
  Part: { fd_AxisOfs(0), fd_AxisOfs(1), fd_AxisOfs(2) }
};
TApplyPart HandParts = {
  Priority: 5,
  PartNum:  3,
  Part: { fd_AxisOfs(10), fd_AxisOfs(11), fd_AxisOfs(12) }
};
TApplyPart AllParts = {
  Priority: 1,
  PartNum:  6,
  Part: { fd_AxisOfs(0), fd_AxisOfs(1), fd_AxisOfs(2), fd_AxisOfs(10), fd_AxisOfs(11), fd_AxisOfs(12) }
};

Priorityは小さい方が優先となりますので、AllPart>HandPart>ArmPartの順になります。

ArmPartを使用したモーションを再生するとID1~3が稼働します。

ArmPartのモーション再生中にAllPartを使用したモーションを再生すると、ArmPartを使用したモーションは無効となりAllPartを使用したモーションで置き換わります。

それではAllPartとHandPartのPriorityを逆転させてみましょう。

AllPartのモーションを再生するとID1~6が稼働します。

AllPartのモーション再生中にArmPartを使用したモーションを再生すると、ID1~3までがArmPartを使用したモーションで置き換わります。ID4~6はHandPartのモーションが稼働します。

モーションプログラム ^[24]

それでは物を掴んで移動する一連の動作をプログラムにします。

#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, 512, 512 , 512 };
const TSpec Spec[] = {
  { 1, DEV_AX12, fd_AxisOfs(0), +341, {  257,  768},   0, 1023, { 1, 1, 32,  32} },
  { 2, DEV_AX12, fd_AxisOfs(1), +341, {  300,  818},   0, 1023, { 1, 1, 32,  32} },
  { 3, DEV_AX12, fd_AxisOfs(2), +341, {  180,  800},   0, 1023, { 1, 1, 32,  32} },
  { 4, DEV_AX12, fd_AxisOfs(3), +341, {    0, 1023},   0, 1023, { 1, 1, 32,  32} },
  { 5, DEV_AX12, fd_AxisOfs(4), +341, {    0,  592},   0, 1023, { 1, 1, 32,  32} },
  { 6, DEV_AX12, fd_AxisOfs(5), +341, {  432, 1023},   0, 1023, { 1, 1, 32,  32} },
};
TApplyPart HandParts = {  // 全軸ロボットハンド
  Priority: 1,
  PartNum:  6,
  Part: { fd_AxisOfs(0), fd_AxisOfs(1), fd_AxisOfs(2), fd_AxisOfs(3), fd_AxisOfs(4), fd_AxisOfs(5) }
};

/*======================================================================*/
// * モーションデータ
const TPose WorkMotion [] = {
  { Structure:{    0,    0,    0,    0,    0,    0 },  Adj:ADJ_SACC_SDECEL, Div:2000, },  // ホームポジション
  { Structure:{  450,    0,    0,    0,    0,    0 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // 左旋回
  { Structure:{  450,    0,    0,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // 指を開く
  { Structure:{  450,  300, -300,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:500,  },  // 腕を下げる
  { Structure:{  450,  450, -450,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:500,  },
  { Structure:{  450,  600, -300,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:500,  },
  { Structure:{  450,  900,    0,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:500,  },
  { Structure:{  450,  900,    0,    0,  300, -300 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // 指を閉じる
  { Structure:{  450,  600,    0,    0,  300, -300 },  Adj:ADJ_SACC_SDECEL, Div:500,  },  // 腕を上げる
  { Structure:{  450,  450, -450,    0,  300, -300 },  Adj:ADJ_SACC_SDECEL, Div:500,  },
  { Structure:{ -900,  450, -450,    0,  300, -300 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // 右旋回
  { Structure:{ -900,  600,    0,    0,  300, -300 },  Adj:ADJ_SACC_SDECEL, Div:500,  },  // 腕を下げる
  { Structure:{ -900,  900,    0,    0,  300, -300 },  Adj:ADJ_SACC_SDECEL, Div:500,  },
  { Structure:{ -900,  900,    0,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // 指を開く
  { Structure:{ -900,    0,    0,    0, -900,  900 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // 腕を上げる
  { Structure:{   0,     0,    0,    0,    0,    0 },  Adj:ADJ_SACC_SDECEL, Div:1000, },  // ホームポジション
};


/*======================================================================*/
/*  メイン関数                                                          */
/*======================================================================*/
void main (void) {
  // 初期化
  fd_SetBeepCondition (FD_BEEP_MMI | FD_BEEP_PACKETERR | FD_BEEP_LOWVOLTAGE | FD_BEEP_BOOTUP);
  fd_SetUVThreshold (7.4);
  DX_ChangeBaudrate (1000000);
  fd_Wait (2000);

  // モーション初期設定
  fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec));
  // モーション実行
  fd_PlayMotion (WorkMotion, 1, sizeof(WorkMotion)/sizeof(TPose), 100, &HandParts);
  // モーション終了待機
  while (fd_GetMotionStat (&HandParts)) fd_Wait (10);
  // トルクOFF
  fd_SetDXEnableControl (false, &HandParts);
}

少しは滑らかに動くようになったのではないでしょうか。
最後に逆運動学で各アクチュエータの角度を算出してみます。

次のチャプターへ^[25]

FDIII-HC Starter Kit Guide^[26]へ戻る