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] = { 40, 28, 68, 40 }; 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 main (void) { double D2D3, D3POS, D2POS, D2j, jPOS, tmpcos1, tmpcos2, cosD3, radD1, radD2, radD3; 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); D2D3 = L[2]; D3POS = L[3]; 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)(Degrees(radD1) * 10); // D1の角度 WorkMotion.Structure[1] = (int)(Degrees(radD2) * 10); // D2の角度 WorkMotion.Structure[2] = (int)(Degrees(radD3) * 10); // D3の角度 fd_PlayMotion (&WorkMotion, 1, 1, 100, &HandParts); // モーションの再生 while (fd_GetMotionStat (&HandParts)) fd_Wait (10); } </pre> }} ** まとめ [#o814be71] Chapter10ではFREEDOM III libraryに用意された関数を使用してモーションを動かしてみました。~ 又、指定された座標から角度を算出しました。今回は一つの考え方を説明しましたが、IKによる計算方法は様々な手法が考えられており、ほとんどが専門の知識を必要とします。これを機に調べてみるのも良いかも知れません。 ** 課題 [#xf0a3d27] 範囲外の座標が指示された場合、プログラムの途中で計算が破綻をきたします。これを回避するよう、プログラムを修正して下さい。~ ヒントは計算の破綻は決まった関数(acos/atan2)で起こります。~ 又、取り外したハンド部分のAX-12+を分解してアーム部分を延長することで、産業用のロボットアームと同様の構成にすることができます。~ #ref(agenda_arm.png) [[FDIII-HC Starter Kit Guide>LEARNINGGUIDE]]へ戻る
(This host) = http://www.besttechnology.co.jp