|
一覧
検索
最新
ページへ戻る
履歴
FDIII-HC Starter Kit Guide/Chapter10.5
のソース
knowledge
:
FDIII-HC Starter Kit Guide
/
Chapter10.5
のソース
差分
を表示
FDIII-HC Starter Kit Guide/Chapter10.5
へ行く。
« Prev
TITLE:ロボットハンドプログラムの修正 ** プログラムの修正 [#w9305379] IKで求めた角度をモーションに組み込みます。 #html{{ <pre class="brush:c;toolbar:false"> #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); } } </pre> }} このプログラムはSIMPLE TERM(ターミナルプログラム)から座標を指定できるようになっており、プッシュボタンが押されると終了してしまいます。 ** まとめ [#o814be71] 本章ではFREEDOM III libraryに用意された関数を使用してモーションでロボットハンドを動かしてみました。~ 又、指定された座標から角度を算出しました。今回は一つの考え方を説明しましたが、IKによる計算方法は様々な手法が考えられており、ほとんどが専門の知識を必要とします。これを機に調べてみるのも良いかも知れません。 ** 課題 [#xf0a3d27] 指定された座標によってはプログラムの途中で計算が破綻をきたし、不正な角度を設定してしまいます。これを回避するよう、プログラムを修正して下さい。~ ヒントは計算の破綻は決まった関数(acos/atan2)で起こります。~ 又、取り外したハンド部分のAX-12+を分解してアーム部分を延長することで、産業用のロボットアームと同様の構成にすることができます。~ #ref(agenda_arm.png) [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る
« Prev
FDIII-HC Starter Kit Guide/Chapter10.5 のバックアップ一覧
FDIII-HC Starter Kit Guide/Chapter10.5 のバックアップソース(No. All)
1: 2013-08-02 (金) 17:24:32
yoshida
2: 2013-08-02 (金) 19:00:59
yoshida
3: 2013-08-02 (金) 20:27:27
yoshida
4: 2013-08-05 (月) 13:10:51
yoshida
5: 2013-08-06 (火) 20:21:18
yoshida
現: 2013-08-07 (水) 20:32:08
yoshida
Copyright© 1989,2024 BestTechnology Co.,Ltd.
メニュー
トップ
ニュース
お知らせ
新商品情報
更新情報
マニュアル類
技術情報
チュートリアル
FAQ
お問合せ
技術的なお問合せ
その他お問合せ
お見積り
ブログ
会社案内
ショッピング
ご利用案内
検索
カテゴリ 一覧
ロボットキット
Dynamixelシリーズ
マイコンボード
相撲ロボット
センサ
通信機器
お客様ページ