http:// techno-road.com/
begin distance_sensor
#body len[m] LSB posx posy posz[m] rol pit yaw[deg]
0 8.000 0.001 0.05 0.0 0.0 0 0 -45
0 8.000 0.001 0.05 0.0 0.0 0 0 0
0 8.000 0.001 0.05 0.0 0.0 0 0 45
end
この状態でロボットをロードすると、図 13-1 (1)のように腰パーツに距離センサが３つ追加されたことが確認できます。さらに前方にかがむと(2)のように距離センサの反射された場所が●で表示されることが確認できます。\\
このとき、図 13-2 のように、コマンドウィンドウでDSensor0, Desonsr1, DSensor2と入力して、各距離センサの値を確認することができます。もちろんスクリプトから利用することが可能です。C++(DLL)から利用することも可能です（DLL_SIM_ROBOT3構造体のint DsensorNumと double *DsensorDat）。\\
{{:ja:manual:add1.png|}}\\
図 13-1 距離センサを追加されたロボット\\
{{:ja:manual:add2.png|}}\\
図 13-2 コマンドウィンドウから距離センサの値を確認\\
===== 13.2 マルチジョイント（リンク機構） =====
Go Simulation!ではロボットのリンク構成は、基本的に図 13-3 (1)のようにスター型の構造を対象としています。即ち一つのボディに、一つのジョイント（モータ）という構成になっています。\\
Ver1.3.4より新たに図 13-3 (2)のように、サブジョイントを追加できるようにしました。これにより、リンク構造、即ち閉ループ型の構造をシミュレーションできるようになりました。なお、GUIからは設定操作できず、直接設定ファイルを書き込むことで対応できるようになっています。
{{:ja:manual:add3.png|}}\\
図 13-3 ジョイント構成\\
サブジョイントを追加するには、Design/Config.txtファイルの、以下に朱記したようにJointSub追記すると、サブジョイントが追加されます。\\
{{:ja:manual:add4.png|}}\\
\\
\\
作成例を図 13-4に示します。またサンプルプロジェクト一式を掲示板にアップしました（http://techno-road.com/board/view-topiclist/forum-9-go-simulation-.html）。\\
{{:ja:manual:add5.png|}}\\
図 13-4 マルチジョイントのサンプル\\
\\
\\
===== 13.3力・トルク指令=====
Go Simulation!では従来、各関節への指令は角度か速度が可能でしたが、__Ver 1.3.6より__力やトルクで指令を与えることが出来るようになりました。
関節のタイプがヒンジの場合はトルク指令、スライダの場合は力指令になります。
本章では、チュートリアル形式で、力・トルク指令を使ったシミュレーションについて解説します。デフォルトのロボットをベースに、右肩をトルク指令、左肩をスライダにして力指令タイプにしてみます。\\
==== トルク指令に設定====
新規にロボットを作成してください。\\
そして、本チュートリアルでは左肩(#18)をスライダ指令にしたいので、設計エディタで図13-5のように変更してください。\\
{{:ja:manual:add_trq1.png?400|}}\\
{{:ja:manual:add_trq2.png?400|}}\\
図 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)が実行されるようにする）。
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;
}
testTorqueRef()は次のようになっています。\\
右肩（１７番関節）と左肩（１８番関節）にそれぞれPD制御を施しています。\\
PF_SetRefMotorTorque(int id, double torque);を使用し、idに関節番号、torqueに指令トルク（力）を設定しています。\\
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
}
}
それではこのサンプルの動かし方を解説します。
ビルドして、Releaseフォルダ内に出来た、DllCont4.dllをコピーし、先程つくったロボットプロジェクトのPROGRAMフォルダ下に入れてください。
そして、コマンドウィンドウから
dll_run(DllCont4)
と入力すると先ほどのDLLが実行されトルク制御が行われるようになります。
この例では最初に説明したように、\\
「右肩＝ヒンジ＝トルク指令」\\
「左肩＝スライダ＝力指令」\\
になっているので、下図のような結果になり、サンプルでは右肩120[deg]上方、左肩0.3[m]前方 の位置に制御しています。
{{:ja:manual:add_trq4.png?400|}}
\\
\\
\\
\\