目次

13. 追加機能

Go Simulation!はリリース後も随時機能を追加しております。
ここでは追加機能についてまとめます。

13.1 距離センサ

距離センサを追加することが可能です。
GUIから操作はできませんが、直接設定ファイルを書き込むことで追加できます。
例えば、以下の内容を、デフォルトのロボットプロジェクト/Design フォルダの下に、dsensor.txtというファイル名で保存するだけで設定完了です。

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)のように腰パーツに距離センサが3つ追加されたことが確認できます。さらに前方にかがむと(2)のように距離センサの反射された場所が●で表示されることが確認できます。
このとき、図 13-2 のように、コマンドウィンドウでDSensor0, Desonsr1, DSensor2と入力して、各距離センサの値を確認することができます。もちろんスクリプトから利用することが可能です。C++(DLL)から利用することも可能です(DLL_SIM_ROBOT3構造体のint DsensorNumと double *DsensorDat)。


図 13-1 距離センサを追加されたロボット


図 13-2 コマンドウィンドウから距離センサの値を確認

13.2 マルチジョイント(リンク機構)

 Go Simulation!ではロボットのリンク構成は、基本的に図 13-3 (1)のようにスター型の構造を対象としています。即ち一つのボディに、一つのジョイント(モータ)という構成になっています。
 Ver1.3.4より新たに図 13-3 (2)のように、サブジョイントを追加できるようにしました。これにより、リンク構造、即ち閉ループ型の構造をシミュレーションできるようになりました。なお、GUIからは設定操作できず、直接設定ファイルを書き込むことで対応できるようになっています。


図 13-3 ジョイント構成

 サブジョイントを追加するには、Design/Config.txtファイルの、以下に朱記したようにJointSub追記すると、サブジョイントが追加されます。




 作成例を図 13-4に示します。またサンプルプロジェクト一式を掲示板にアップしました(http://techno-road.com/board/view-topiclist/forum-9-go-simulation-.html)。


図 13-4 マルチジョイントのサンプル


13.3力・トルク指令

Go Simulation!では従来、各関節への指令は角度か速度が可能でしたが、Ver 1.3.6より力やトルクで指令を与えることが出来るようになりました。 関節のタイプがヒンジの場合はトルク指令、スライダの場合は力指令になります。

本章では、チュートリアル形式で、力・トルク指令を使ったシミュレーションについて解説します。デフォルトのロボットをベースに、右肩をトルク指令、左肩をスライダにして力指令タイプにしてみます。

トルク指令に設定

新規にロボットを作成してください。
そして、本チュートリアルでは左肩(#18)をスライダ指令にしたいので、設計エディタで図13-5のように変更してください。



図 13-5 右肩のジョイントをスライダに変更

サーボモータをトルク指令にするために、サーボモータ設定エディタで図13-6のように#17と#18をトルク指令に変更してください。

図 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()は次のようになっています。
右肩(17番関節)と左肩(18番関節)にそれぞれ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]前方 の位置に制御しています。





株式会社テクノロード
http:// techno-road.com/