FDIII-HC Starter Kit Guide​/Chapter10.3 のバックアップソース(No.13) :: Besttechnology

• 差分^[1] を表示
• 現在との差分^[2] を表示
• FDIII-HC Starter Kit Guide/Chapter10.3^[3] へ行く。

TITLE:ロボットハンドを動かす
*ロボットハンドを動かす [#u69e0124]

**Dynamixel Configuratorで動かす [#i7e648cf]
まずは確認のため、[[Dynamixel Configurator>BTE068]]でAX-12+を作動させてみましょう。トルク設定をしていませんので、ポジション移動の際は十二分に注意して下さい。~
ID1からID6まで以下の手順を繰り返します。
+「Read All」で状態を読み込む。
+ダイヤルでポジションを移動する。

**物を掴む [#a91d93dc]
ID1~ID3でハンド部分の位置が決まり、ID5、6でハンド部分が開閉します。~
空のペットボトル等を掴んで持ち上げてみましょう。
#ref(Clip.png)
如何でしょうか。持ち上げることができたでしょうか。~
Dynamixel Configuratorでは、個々のアクチュエータにそれぞれ手動で指令を行わなければならず、随分と手間がかかるのではないでしょうか。~
ID1のゴールポジションを変更することで右へ旋回します。旋回後、腕を下して掴み上げたものを下してください。~
では、Dynamixel Configuratorでの操作をプログラムで再現してみましょう。

**プログラムする [#bc44f46d]
ロボットハンドの左につかむ物を設置して下さい。腕の先へ掴めるものが来るように配置しましょう。
#html{{
<pre class="brush:c;toolbar:false">
#include &lt;fd.h&gt;
// メイン関数
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_Wait (1000);

  // 左へターン
  fd_DXSetPosition (1, 605);
  fd_Wait (500);
  // 開く
  fd_DXSetPosition (5, 362);
  fd_DXSetPosition (6, 662);
  fd_Wait (1000);
  // 腕を下げる
  fd_DXSetPosition (2, 818);
  fd_Wait (1000);
  // 挟む
  fd_DXSetPosition (5, 542);
  fd_DXSetPosition (6, 482);
  fd_Wait (1000);
  // 持ち上げる
  fd_DXSetPosition (2, 512);
  fd_Wait (1000);
  // 右へ移動
  fd_DXSetPosition (1, 419);
  fd_Wait (1000);
  // 腕を下げる
  fd_DXSetPosition (2, 818);
  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_Wait (1000);
}
</pre>
}}
今回はDynamixel Configuratorでの操作をそのままプログラムしました。指定位置は掴むものに合った値に変更して下さい。~
まだ動きが随分とぎこちないですね。もう少し滑らかに動くように修正します。

**モーション [#q1da695a]
[[FREEDOM Ⅲライブラリ>FDIIILIB]]には、モーションに関するAPIが用意されていますので、動作をモーションに置き換えてみましょう。~
***概要 [#v0d80198]
モーションを使用するには必要なidの宣言と初期設定値等を取りまとめた[[TSpec構造体>FDIIILIB#w18bc93a]]を初期処理で使用します。~
又、ホームポジションを設定するため、[[THomePosition構造体>FDIIILIB#fa1e627a]]を合わせて使用します。ホームポジションは各アクチュエータの基準になり、モーションの指定はホームポジションに対する角度の指定となります。~
モーションデータを再生するには、どの部分を稼働させるかを指示する[[TApplyPart構造体>FDIIILIB#q3b8aabf]]と、実際のモーションデータである[[TPose構造体>FDIIILIB#y2b4d82e]]を使用します。
#ref(outline.png)

****初期化 [#w63f4eb4]
#html{{
<pre class="brush:c;toolbar:false">
fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec));
</pre>
}}
*****[[fd_SetSpec>FDIIILIB#r0f0534c]] [#a6a7bd34]
指定された値を使用して、初期設定を行います。~
''第1引数''は、ホームポジションを指定したTHomePosition構造体変数のアドレスです。~
''第2引数''は、アクチュエータの情報を指定したTSpec構造体変数のアドレスです。~
''第3引数''は、TSpec配列数です。

*****THomePosition構造体 [#e87812da]
基準となる位置を指定します。
#html{{
<pre class="brush:c;toolbar:false">
const THomePosition HomePos = { 512, 512, 512, 512, 512 , 512 };
</pre>
}}
6軸のアクチュエータに対し、512(出力軸は150°)を指定しています。

*****TSpec構造体 [#lbf8e4eb]
各初期情報を指定します。
#html{{
<pre class="brush:c;toolbar:false">
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} },
};
</pre>
}}
AX-12+のIDを1~6と設定しているので、それらをオフセット0~2と10~12へ割り当てます。オフセットは60の配列の為、59まで使用可能となっており、TApplyPartで指定するオフセットと対応していればどのような順番であっても問題ありません。

****モーション再生 [#u5c51edb]
#html{{
<pre class="brush:c;toolbar:false">

</pre>
}}
*****[[fd_PlayMotion>FDIIILIB#xc988ccc]] [#h4574808]

*****TApplyPart構造体 [#fdec1af9]
モーションはTApplyPartで設定された部位単位に動きます。1つのアクチュエータを複数の部位で使用しても問題ありません。~
但し、その場合はPriorityが有効になります。
#html{{
<pre class="brush:c;toolbar:false">
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) }
};
</pre>
}}
Priorityは小さい方が優先となりますので、AllPart>HandPart>ArmPartの順になります。
#ref(motion2_1.png)
HandPartを使用したモーションが稼働中にAllPartを使用したモーションを稼働させると、HandPartを使用したモーションは無効となりAllPartを使用したモーションのみが稼働します。~
AllPartとHandPartのPriorityを逆転させてみましょう。
#ref(motion2_2.png)
AllPartのモーション稼働時にHandPartのモーションが稼働すると、ID1~3はAllPartのモーションがそのまま稼働し、ID4~6はHandPartのモーションが稼働します。

*****TPose構造体 [#if4363ee]
モーションデータになります。部位と合わせて使用します。
#html{{
<pre class="brush:c;toolbar:false">
const TPose HomePos[] = {
  { Structure:{ 0, 0, 0, 0, 0, 0 },  Adj:ADJ_SACC_SDECEL, Div:1000, },
};
</pre>
}}
モーションデータは配列で宣言して下さい。又、TSpecで宣言したアクチュエータ分、作成して下さい。

***モーションプログラム [#s04f1e9e]
物を掴んで移動する一連の動作をプログラムにします。
#html{{
<pre class="brush:c">
#include &lt;fd.h&gt;
#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, {  390,  818},   0, 1023, { 1, 1, 32,  32} },
  { 3, DEV_AX12, fd_AxisOfs(2), +331, {   64,  512},   0, 1023, { 1, 1, 32,  32} },
  { 4, DEV_AX12, fd_AxisOfs(3), +331, {    0, 1023},   0, 1023, { 1, 1, 32,  32} },
  { 5, DEV_AX12, fd_AxisOfs(4), +331, {    0,  592},   0, 1023, { 1, 1, 32,  32} },
  { 6, DEV_AX12, fd_AxisOfs(5), +331, {  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);
}
</pre>
}}

***解説 [#j28efc99]
少しは滑らかに動いたのではないでしょうか。では、モーションの使用について説明します。

• FDIII-HC Starter Kit Guide/Chapter10.3 のバックアップ一覧^[6]
• FDIII-HC Starter Kit Guide/Chapter10.3 のバックアップソース(No. All)
  • 1: 2011-07-20 (水) 19:53:28^[7] yoshida^[8]
  • 2: 2011-07-20 (水) 20:30:04^[9] yoshida^[8]
  • 3: 2011-07-22 (金) 13:13:29^[10] yoshida^[8]
  • 4: 2011-07-22 (金) 15:06:39^[11] yoshida^[8]
  • 5: 2011-07-22 (金) 17:58:50^[12] yoshida^[8]
  • 6: 2011-07-22 (金) 20:07:06^[13] yoshida^[8]
  • 7: 2011-07-25 (月) 16:27:39^[14] yoshida^[8]
  • 8: 2011-07-28 (木) 19:33:34^[15] yoshida^[8]
  • 9: 2011-07-29 (金) 13:28:53^[16] yoshida^[8]
  • 10: 2011-08-01 (月) 16:18:24^[17] yoshida^[8]
  • 11: 2011-08-01 (月) 20:55:16^[18] yoshida^[8]
  • 12: 2011-08-02 (火) 18:34:36^[19] yoshida^[8]
  • 13: 2011-08-02 (火) 20:33:08 yoshida^[8]
  • 14: 2011-08-03 (水) 14:41:34^[20] yoshida^[8]
  • 15: 2011-08-03 (水) 16:40:07^[21] yoshida^[8]
  • 16: 2011-08-04 (木) 18:20:52^[22] yoshida^[8]
  • 17: 2011-08-05 (金) 16:22:21^[23] yoshida^[8]
  • 18: 2011-08-29 (月) 13:17:40^[24] yoshida^[8]
  • 19: 2013-07-03 (水) 17:56:35^[25] yoshida^[8]
  • 20: 2013-08-01 (木) 16:16:27^[26] yoshida^[8]
  • 21: 2013-08-05 (月) 13:01:56^[27] yoshida^[8]
  • 22: 2013-08-05 (月) 13:13:34^[28] yoshida^[8]
  • 23: 2013-08-07 (水) 15:40:11^[29] yoshida^[8]
  • 現: 2013-08-07 (水) 20:22:54^[30] yoshida^[8]