Page Top

Dynamixel Configuratorで動かす anchor.png

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

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

物を掴む anchor.png

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

DXCONF.png

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

Page Top

プログラムする anchor.png

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

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

Page Top

モーション anchor.png

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

Page Top

概要 anchor.png

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

outline.png
Page Top
初期化 anchor.png

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

fd_SetSpec (&HomePos, Spec, fd_SpecSize (Spec));
Page Top
fd_SetSpec anchor.png

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

Page Top
THomePosition構造体 anchor.png

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

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

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

Page Top
TSpec構造体 anchor.png

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

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で指定するオフセットと対応していればどのような順番であっても問題ありません。

Page Top
モーション再生 anchor.png

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

fd_PlayMotion (GoToHomePosition, 1, sizeof(GoToHomePosition)/sizeof(TPose), 100, &HandParts);
Page Top
fd_PlayMotion anchor.png

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

Page Top
TPose構造体 anchor.png

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

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

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

Page Top
TApplyPart構造体 anchor.png

アクチュエータを取りまとめた部位の設定を行います。
アクチュエータの指定はIDではなく、TSpecで宣言したオフセット値を使用します。
又、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の順になります。

motion2_1.png

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

motion2_2.png

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

motion2_3.png

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

motion3_1.png

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

motion3_2.png

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

motion3_3.png
Page Top

モーションプログラム anchor.png

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

#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);
}

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

次のチャプターへ

FDIII-HC Starter Kit Guideへ戻る

Attach file: fileDXCONF.png 99DL [Info] filemotion3_3.png 149DL [Info] filemotion3_2.png 108DL [Info] filemotion3_1.png 121DL [Info] filemotion2_3.png 102DL [Info] filemotion2_2.png 108DL [Info] filemotion2_1.png 97DL [Info] fileoutline.png 111DL [Info]
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:22:54 (JST) (4271d)
Links: BTE068 Dynamixelコンフィギュレータ Chapter10.4 Chapter10.2 FDIII-HC Starter Kit Guide FREEDOM III library