この文書の現在のバージョンと選択したバージョンの差分を表示します。
両方とも前のリビジョン 前のリビジョン 次のリビジョン | 前のリビジョン | ||
ja:manual:additional_function [2014/08/08 18:37] sugiura [13.3力・トルク指令] |
ja:manual:additional_function [2018/04/24 14:25] (現在) doripoyo [トルク指令の制御プログラム(DLL)] |
||
---|---|---|---|
ライン 51: | ライン 51: | ||
関節のタイプがヒンジの場合はトルク指令、スライダの場合は力指令になります。 | 関節のタイプがヒンジの場合はトルク指令、スライダの場合は力指令になります。 | ||
- | 本章では、チュートリアル形式で、力・トルク指令を使ったシミュレーションについて解説します。デフォルトのロボットをベースに、右肩をトルク指令、左肩をスライダにして力指令タイプにしてみます。 | + | 本章では、チュートリアル形式で、力・トルク指令を使ったシミュレーションについて解説します。デフォルトのロボットをベースに、右肩をトルク指令、左肩をスライダにして力指令タイプにしてみます。\\ |
- | ==== トルク指令の設定令==== | + | ==== トルク指令に設定==== |
- | 新規にロボットを作成してください。そして、本チュートリアルでは左肩(#18)をスライダ指令にしたいので、設計エディタで図13-5のように変更してください。 | + | 新規にロボットを作成してください。\\ |
+ | そして、本チュートリアルでは左肩(#18)をスライダ指令にしたいので、設計エディタで図13-5のように変更してください。\\ | ||
- | {{:ja:manual:add_trq1.png|}}\\ | + | {{:ja:manual:add_trq1.png?400|}}\\ |
- | {{:ja:manual:add_trq2.png|}}\\ | + | {{:ja:manual:add_trq2.png?400|}}\\ |
図 13-5 右肩のジョイントをスライダに変更\\ | 図 13-5 右肩のジョイントをスライダに変更\\ | ||
+ | \\ | ||
+ | |||
+ | サーボモータをトルク指令にするために、サーボモータ設定エディタで図13-6のように#17と#18をトルク指令に変更してください。\\ | ||
+ | {{:ja:manual:add_trq3.png?500|}}\\ | ||
+ | 図 13-6 サーボモータをトルク指令に変更\\ | ||
+ | \\ | ||
+ | ==== トルク指令の制御プログラム(DLL)==== | ||
+ | トルク制御のモデルを作成でたので、制御プログラムの作り方について解説します。本サンプルではDLL(C++)でプログラムする方法を示します。 | ||
+ | |||
+ | DLLのサンプルプロジェクト一式を掲示板にアップしました(http://techno-road.com/board/view-postlist/forum-9-go-simulation-/topic-141-cdll.html)。ダウンロードしてください。 | ||
+ | |||
+ | main_11TestDll4_basic.cppの、Dll4_Run関数の中を以下のように書き換えてください(testTorqueRef(sim)が実行されるようにする)。 | ||
+ | <code cpp> | ||
+ | void __stdcall Dll4_Run( DLL_SIM_ROBOT4 *sim) { | ||
+ | //Time count up | ||
+ | int idt = (int)(sim->dt/0.001); | ||
+ | gImsec+=idt; | ||
+ | |||
+ | //select a test function | ||
+ | // testAnalogJoypad(sim); | ||
+ | // testMotionDSensor(sim); | ||
+ | // testCamera(sim); | ||
+ | testTorqueRef(sim); | ||
+ | return; | ||
+ | } | ||
+ | </code> | ||
+ | |||
+ | testTorqueRef()は次のようになっています。\\ | ||
+ | 右肩(17番関節)と左肩(18番関節)にそれぞれPD制御を施しています。\\ | ||
+ | PF_SetRefMotorTorque(int id, double torque);を使用し、idに関節番号、torqueに指令トルク(力)を設定しています。\\ | ||
+ | |||
+ | <code cpp> | ||
+ | void testTorqueRef(DLL_SIM_ROBOT4 *sim){ | ||
+ | if(sim->Axis<=18){ | ||
+ | return; | ||
+ | } | ||
+ | int axis; | ||
+ | |||
+ | //Sholder Pitch Right Joint(ShoP_R) is set as "Hinge Joint" and "Torque Control Motor" | ||
+ | axis = 17; //ShoP_R | ||
+ | if(sim->ServoMode[axis]==3) | ||
+ | { | ||
+ | //トルク制御モードなら | ||
+ | static double bx=0; | ||
+ | static double ref=0; | ||
+ | |||
+ | |||
+ | //angle ref 0 to 120[deg] | ||
+ | ref+=0.1; | ||
+ | if(ref>120){ | ||
+ | ref=120; | ||
+ | } | ||
+ | |||
+ | double x = sim->ServoNowDeg[axis];//angle | ||
+ | double v = (x-bx)/sim->dt;//vel | ||
+ | |||
+ | //PD Control | ||
+ | double kp = 0.04; | ||
+ | double kd = 0.001; | ||
+ | double f = kp*(ref-x) - kd*v; | ||
+ | |||
+ | //output torque | ||
+ | SetRefMotorTorque(axis,f); | ||
+ | |||
+ | //message | ||
+ | wchar_t s[128]; | ||
+ | swprintf(s,L"ang ref=%lf x=%lf\n",ref,x); | ||
+ | sim->PrintCommandWindow(s); | ||
+ | |||
+ | bx = x;//back up | ||
+ | } | ||
+ | |||
+ | //Sholder Pitch Left Joint(ShoP_L) is set as "Slider Joint" and "Force Control Motor" | ||
+ | axis = 18; //ShoP_L | ||
+ | if(sim->ServoMode[axis]==3) | ||
+ | { | ||
+ | static double bx=0; | ||
+ | static double ref=0; | ||
+ | |||
+ | //position ref 0 to 0.3[m] | ||
+ | ref+=0.001; | ||
+ | if(ref>0.3){ | ||
+ | ref=0.3; | ||
+ | } | ||
+ | |||
+ | //double f = 0.1; | ||
+ | |||
+ | double x = sim->ServoNowDeg[axis];//position | ||
+ | double v = (x-bx)/sim->dt;//vel | ||
+ | |||
+ | //PD Control | ||
+ | double kp = 0.8; | ||
+ | double kd = 0.5; | ||
+ | double f = kp*(ref-x) - kd*v; | ||
+ | |||
+ | //output torque | ||
+ | SetRefMotorTorque(axis,f); | ||
+ | |||
+ | //message | ||
+ | wchar_t s[128]; | ||
+ | swprintf(s,L"pos ref=%lf x=%lf\n",ref,x); | ||
+ | sim->PrintCommandWindow(s); | ||
+ | |||
+ | bx = x;//back up | ||
+ | } | ||
+ | |||
+ | } | ||
+ | </code> | ||
+ | |||
+ | |||
+ | |||
+ | それではこのサンプルの動かし方を解説します。 | ||
+ | |||
+ | ビルドして、Releaseフォルダ内に出来た、DllCont4.dllをコピーし、先程つくったロボットプロジェクトのPROGRAMフォルダ下に入れてください。 | ||
+ | |||
+ | そして、コマンドウィンドウから | ||
+ | <code> | ||
+ | dll_run(DllCont4) | ||
+ | </code> | ||
+ | と入力すると先ほどのDLLが実行されトルク制御が行われるようになります。 | ||
+ | |||
+ | この例では最初に説明したように、\\ | ||
+ | 「右肩=ヒンジ=トルク指令」\\ | ||
+ | 「左肩=スライダ=力指令」\\ | ||
+ | になっているので、下図のような結果になり、サンプルでは右肩120[deg]上方、左肩0.3[m]前方 の位置に制御しています。 | ||
+ | |||
+ | {{:ja:manual:add_trq4.png?400|}} | ||
ライン 73: | ライン 201: | ||
border:solid #ccc 1px;"> | border:solid #ccc 1px;"> | ||
株式会社テクノロード<br /> | 株式会社テクノロード<br /> | ||
- | 川崎市幸区新川崎7-7 KBIC 222<br /> | ||
<a href="http://techno-road.com/">http:// techno-road.com/</a><br /> | <a href="http://techno-road.com/">http:// techno-road.com/</a><br /> | ||
<img src="http://go-simulation.com/wiki/_media/ja/mail.png" /> | <img src="http://go-simulation.com/wiki/_media/ja/mail.png" /> | ||
</div> | </div> | ||
</html> | </html> | ||
- | |||
- |