« 自然な歩行をするKHR-3HV用ロボットモーション | トップページ | エクセルで歩行モーションの作成 »

2016年12月26日 (月)

歩行シュミュレーション用のC++ソースコード

こんばんは、またまたずいぶんご無沙汰しております。

ロボットの二足歩行シュミュレーション用C++ソースコードです。(修正しました)

・Microsoft Visual C++ 2010上でビルドしてGo simulation上のDLLで動かしています。

・エクセルのマクロと整合させてあります。設定値などはエクセルシートをご覧ください。

・ロボットはKHR-3HVの設定になっていて、Go simulation上で安定して歩行します。

・設定値を変えれば他のロボットでも歩行のシュミュレーションができると思います。

・今回のは「膝を曲げて歩く」タイプです。 歩幅や速さも自由に設定できます。

・前回よりの主な修正は、歩きだしの所を直しました。変数名なども見直しました。モーションを繋げる時の参考にして下さい。

・足首の踵着地や爪先の蹴りを加えました。少し旋回も設定可能にしました。

・歩隔(足の開き)は静止時の両足中心の間隔と同じです。変えると游脚が面倒なことになってしまうので変える事はできていません。

・フラグにより二歩を四分割してモーションを作っています、エクセルシートに説明を加えました。

 
	double  Hsin(double);
	double  Hcos(double);

	double    Pi = 3.141592;
	double   Deg = 180 / Pi;
	double     G = 9.8;      

	int   gImsec = 0;
	int      stc = 0;
	int        n = 0;

	//------------モーション時間設定---------------------------------
	int      dmt =  50;           //モーション間隔(ms)
	int       nc =   6;           //モーション分割回数
	int Max_step =  30;           //歩数(偶数)
	//---------------------------------------------------------------
	double    Ts = (double)dmt *  (double)nc / 1000;//一歩の半分(一歩の時間 = dmt × nc × 2 )

	//---------------ロボット設定------------------------------------
	double    Hc = 0.245;         //重心高さ(m)
	double    kh = 0.950;         //立脚相腰高さ(腰下げ)の設定
	double    Lx = 0.130;         //ピッチ側の関節間隔(m)
	double    Ly = 0.207;         //ロール側の関節間隔(m)
	double   Bsw = 0.056;         //両足の間隔(m)
	//---------------------------------------------------------------
	double    St = 0;             
	double    Sw = 0;             
	double    Uh = 0;             
	double Sw_offset;             
	double   Turning = 0;         

	void Walking(DLL_SIM_ROBOT4 *sim);


//-----------------------------------------------------------------------
void __stdcall Dll4_Run(DLL_SIM_ROBOT4 *sim) {

	//Time count up
	int idt = (int)(sim->dt / 0.001);
	gImsec += idt;

	if ((gImsec % dmt) == 0) {
		stc =   gImsec /(dmt  * nc) + 1;//歩数カウンタ
		  n = ((gImsec / dmt) % nc) + 1;//モーションカウンタ

		St  = 0.100;                    //歩幅設定(m)
		Sw  = Bsw  ;                    //歩隔設定(m)
		Sw_offset = Bsw / 2;            //重心~足の横オフセット(m)

		//歩き始めの歩幅設定例//
		if(stc <=  6)            { St = 0.070;}
		if(stc <=  4)            { St = 0.030;}
		if(stc ==  2)            { St = 0;     Sw = Bsw * 0.5;}
		if(stc ==  1)            { St = 0;     Sw = Bsw * 0.5; Sw_offset = 0.0;}
		//止まる時の歩幅設定例//
		if(stc >= (Max_step - 5)){ St = 0.070;}
		if(stc >= (Max_step - 3)){ St = 0.030;}
		if(stc == (Max_step - 1)){ St = 0;     Sw = Bsw * 0.5;}
		if(stc == (Max_step - 0)){ St = 0;     Sw = Bsw * 0.5; Sw_offset = 0.0;}

		Turning = 0.0/ Deg;             //旋回角の設定(度)
		     Uh = St * 0.15;             //遊脚相足上げ高さの設定

		//DLL終了
		if (stc == Max_step && n==nc){
			sim->bFinish = true;//DLL finish flag
		}

		Walking(sim);                   //歩行モーションへ→
	}
	return;
}

//-----------------------------------------------------------------------
// 歩行モーション
//-----------------------------------------------------------------------
void Walking(DLL_SIM_ROBOT4 *sim){

	//計算用比率
	int i;
	double a[100];
	double b[100];
	for (i = 0; i <= nc; i++) {
		a[i] =               (double)i  / (double)nc;
		b[i] = ((double)nc - (double)i) / (double)nc;
	}

	double  Px;         //立脚足位置ピッチ
	double  Py;         // 〃  ロール
	double  Ux;         //遊脚足位置ピッチ
	double  Uy;         // 〃  ロール
	double  Uref;       //遊脚軌道

	double  toe;        //足首角
	double  Tmp = 2.356;
	double  t;
    double  angle;

	//関節角
	double  c1;  
	double  cf;
	double  q1;         //股ピッチ
	double  q2;         //膝
	double  q3;         //足ピッチ
	double  q4;         //股ロール
	double  q5;         //足ロール
	double  q6;         //腕

	        St = St / 2;
	        Sw = Sw / 2;
	double  Tc = sqrt(Hc / G );
	double   C = Hcos(Ts / Tc);
	double   S = Hsin(Ts / Tc);
	double  Vx = St / ( Tc * S );         //歩速
	double  Zx = sqrt(Lx*Lx - St*St) * kh;//立脚高さピッチ側
	double  Zy = Ly - (Lx - Zx);          //立脚高さロール側
	double  Zu;                           //遊脚高さ
	double  Pl;                           //立脚長
	double  Ul;                           //遊脚長

	int     fp =  stc     & 1;            //前半・後半フラグ
	int     fl = (stc>>1) & 1;            //左足・右足フラグ

	double servoref[20];//関節制御角
	for (i = 1; i <= 18; i++){ servoref[i] = 0; }

	//-----------------------位置の計算-------------------------
	if (fp == 1) {
		 t = (double)Ts / Tc  * a[n];
		Px = Vx * Tc * Hsin(t);
		Py = Sw /  C * Hcos(t);
		Ux = sin(a[n] * Tmp) / sin(Tmp) * -St ;
		Uy = -Py;
		Uref = sin(pow((a[n] / 2 + 0.50), 0.75) * Pi);
		angle = Turning * a[n];
		toe = asin(Uref * Uh / 0.120) * a[n];//踵から着地
	}
	else {
		 t = -(double)Ts / Tc  * b[n] ;
		Px = Vx * Tc * Hsin(t);
		Py = Sw /  C * Hcos(t);
		Ux = sin(b[n] * Tmp) / sin(Tmp) * St ;
		Uy = -Py;
		Uref = sin(pow((a[n] / 2), 0.75) * Pi);
		angle = -Turning * b[n];
		toe = -asin(Uref * Uh / 0.120) * b[n];//爪先蹴り
	}

	//-------------------------立脚相の関節角-------------------
	Pl =  sqrt(Zx*Zx + Px*Px);
	   if(Pl > Lx){Pl = Lx;}
	c1 =  acos(Pl / Lx);
	cf = -atan(Px / Zx);
	q1 = cf + c1;
	q2 = c1 * 2;
	q3 = c1 - cf;
	q4 = atan((Py - Sw_offset) / Zy);
	q5 = q4;
	q6 = Px / Zx;

	if (fl == 0) {
		//---RFoot-------------
		servoref[ 1] =  angle * Deg;
		servoref[ 5] =  q1 * Deg;
		servoref[ 7] =  q2 * Deg;
		servoref[ 9] =  q3 * Deg;
		servoref[ 3] =  q4 * Deg;
		servoref[11] =  q5 * Deg;
		servoref[17] =  q6 * Deg;
	}
	else {
		//---LFoot-------------
		servoref[ 2] =  angle * Deg;
		servoref[ 6] =  q1 * Deg;
		servoref[ 8] =  q2 * Deg;
		servoref[10] =  q3 * Deg;
		servoref[ 4] = -q4 * Deg;
		servoref[12] = -q5 * Deg;
		servoref[18] =  q6 * Deg;
	}

	//------------------------游脚相の関節角--------------------
	Zu =  Zx - Uref * Uh;
	Ul =  sqrt(Zu*Zu + Ux*Ux);
	   if(Ul > Lx){Ul = Lx;}
	c1 =  acos(Ul / Lx);
	cf = -atan(Ux / Zu);
	q1 = cf + c1;
	q2 = c1 * 2;
	q3 = c1 - cf + toe;
	q4 = -atan((Uy + Sw_offset) / (Zy - Uref * Uh));
	q5 = q4 * 0.9;
	q6 = -Px / Zx;

	if (fl == 0) {
		//---LFoot-------------
		servoref[ 2] =  -angle * Deg;
		servoref[ 6] =  q1 * Deg;
		servoref[ 8] =  q2 * Deg;
		servoref[10] =  q3 * Deg;
		servoref[ 4] =  q4 * Deg;
		servoref[12] =  q5 * Deg;
		servoref[18] =  q6 * Deg;
	}
	else {
		//---RFoot-------------
		servoref[ 1] =  -angle * Deg;
		servoref[ 5] =  q1 * Deg;
		servoref[ 7] =  q2 * Deg;
		servoref[ 9] =  q3 * Deg;
		servoref[ 3] = -q4 * Deg;
		servoref[11] = -q5 * Deg;
		servoref[17] =  q6 * Deg;
	}

	//---------------脚部角度制御------------------------------------
	for (i = 1; i <=18; i++) { sim->ServoRefDeg[i] = servoref[i]; }
	//---------------------------------------------------------------

}

double  Hsin(double x)
{
	return (exp(x) - exp(-x)) / 2;
}
double Hcos(double x)
{
	return (exp(x) + exp(-x)) / 2;
}

« 自然な歩行をするKHR-3HV用ロボットモーション | トップページ | エクセルで歩行モーションの作成 »

コメント

コメントを書く

(ウェブ上には掲載しません)

« 自然な歩行をするKHR-3HV用ロボットモーション | トップページ | エクセルで歩行モーションの作成 »