« 自然な歩行をするロボットによる凸凹チャレンジ① | トップページ | エクセルで自然な歩行モーションの作成 »

2018年6月29日 (金)

自然な歩き方をするロボットのC++ソースコード

こんにちは、またまた随分とご無沙汰しております。
 
二足歩行ロボットのシュミュレーション用C++ソースコード「自然な歩き方をする」タイプです。
・Microsoft Visual C++ 2017上でビルドしてGo simulation ver1.5.0上のDLLで動かしています。
・ロボットはKHR-3HVの設定になっていて、Go Simulation!上で安定して歩行します。(足を小さくするには、足首のKp・Kdを下げた方がよいです)
 
・「人間のように自然な歩き方をする」タイプです。着地時のエネルギーロスを回復する膝の曲げ角を近似式を作り計算しています。しかし、タイミングは正確に計算できないのでリニアなままですが、「膝を曲げて歩く」モーションよりも安定している感じなのでこの方が断然いいです。

         
        ・・・ずいぶん苦労しました、アホですよね( ^ω^)・・・
void Walking();
int gImsec = 0;
CRobot robo;

double  Hsin(double);
double  Hcos(double);
double     G = 9.8;

int    phase = 0;    //歩行周期カウンタ(一歩行周期4回)
int        n = 0;    //モーション分割カウンタ

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

//---------------ロボット設定------------------------------------
double    Hc = 0.245;//重心高さ(m)
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;

void __stdcall Dll_Run() {

	//Time count up
	int dt = (int)(robo.GetDt() / 0.001);
	gImsec += dt;

	if ((gImsec % dmt) == 0) {
		phase =  gImsec / (dmt  * nc) + 1;
		    n =((gImsec /  dmt) % nc) + 1;

		St = 0.100 / 2;//歩幅の設定(m)            
		Sw =   Bsw / 2;//歩隔は両足の間隔に設定(固定)           
		Sw_offset = Sw;//ロール側軸オフセット    

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

		Uh = St * 0.20;         //遊脚相の足上げ高さの設定(歩幅に対する比率)
		Turning = 0.0 * DEG2RAD;//旋回角度の設定

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

		//DLL終了
		if (phase == Max_step && n == nc) {
			PrintMsg("Walking is finished\n");
			robo.FinishDll();//finish DLL 
		}
	}
	return;
}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//-----------------------------------------------------------------------
// 歩行モーション
//-----------------------------------------------------------------------
void Walking() {

	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;
	}
	int     fp = phase & 1;       //前半・後半フラグ
	int     fl = (phase >> 1) & 1;//左足・右足フラグ

	double  t;
	double  Tc = sqrt(Hc / G);
	double   C = Hcos(Ts / Tc);
	double   S = Hsin(Ts / Tc);

	double  Px;                   //立脚足位置ピッチ
	double  Py;                   //  〃  ロール
	double  Ux;                   //遊脚足位置ピッチ
	double  Uy;                   //  〃  ロール
	double  Uz;                   //遊脚高さ軌跡
	double  Pratio;               //立脚脚長比率
	double  Uratio;               //遊脚脚長比率
	double  toe;                  //足首角
	double  Tmp = M_PI * 3 / 4;

	double  c1;
	double  cf;
	double  q0;                   //股ヨー
	double  q1;                   //股ロール
	double  q2;                   //股ピッチ
	double  q3;                   //膝
	double  q4;                   //足ピッチ
	double  q5;                   //足ロール
	double  q6;                   //腕
	double servoref[20];          //制御角
	for (i = 1; i <= 18; i++) { servoref[i] = 0; }

	//----------------着地時のロスによる補正-------------------------
	double    Pxs = St;                              //後脚の位置
	double    Vxs = Pxs / (Tc * S);                  //着地前速度
	double   leg1 = asin(Pxs / Lx);                  //後脚の角度
	double   leg2 = 0.36 * pow(sin(leg1 * 2.4), 0.9);//前脚の角度(着地ロスからの近似式)
	double    Pxe = sin(leg2) * Lx;                  //前脚の位置
	double    Vxe = Pxe / (Tc * S);                  //着地後速度
	double  ratio = 1 - (cos(leg1) / cos(leg2));     //前後脚長比率

	//------------------足位置の計算(誤差有)-------------------------
	if (fp == 1) {
		     t = (double)Ts / Tc * a[n];
		    Px = Vxs * Tc * Hsin(t);
		    Py = Sw / C * Hcos(t);
		    Ux = sin(a[n] * Tmp) / sin(Tmp) * -Pxe;
		    Uy = -Py;
		    Uz = sin(pow((a[n] / 2 + 0.50), 0.75) * M_PI);
		   toe =  asin(Uz * Uh / 0.120) * a[n];//踵から着地(足高÷足長)
		Pratio = 1.0;                 
		Uratio = 1 - ratio * a[n] - Uz * Uh / Lx;
		    q0 = Turning * a[n];
	}
	else {
		     t = -(double)Ts / Tc * b[n];
			Px = Vxe * Tc * Hsin(t);
		    Py = Sw / C * Hcos(t);
		    Ux = sin(b[n] * Tmp) / sin(Tmp) * Pxs;
		    Uy = -Py;
		    Uz = sin(pow((a[n] / 2), 0.75) * M_PI);
		   toe = -asin(Uz * Uh / 0.120) * b[n];//爪先で蹴り(足高÷足長)
		Pratio = 1 - ratio * b[n];             //ロス分曲げる
		Uratio = 1.0 - Uz * Uh / Lx;
		    q0 = -Turning * b[n];
	}

	//-------------------関節角の計算(簡易IK)--------------------------------
	//----------立脚相の関節角----------
	if (Pratio > 1) { Pratio = 1; }
	c1 = acos(Pratio);
	if (Px > Lx) { Px = Lx; }
	cf = -asin(Px / Lx);
	q1 = atan((Py - Sw_offset) / Ly * Pratio);
	q2 = cf + c1;
	q3 = c1 * 2;
	q4 = c1 - cf;
	q5 = q1;
	q6 = asin(Px / Lx);
	if (fl == 0) {
		//-----Rleg-----
		servoref[ 1] =  q0;
		servoref[ 3] =  q1;
		servoref[ 5] =  q2;
		servoref[ 7] =  q3;
		servoref[ 9] =  q4;
		servoref[11] =  q5;
		servoref[17] =  q6;
	}
	else {
		//-----Lleg-----
		servoref[ 2] =  q0;
		servoref[ 4] = -q1;
		servoref[ 6] =  q2;
		servoref[ 8] =  q3;
		servoref[10] =  q4;
		servoref[12] = -q5;
		servoref[18] =  q6;
	}
	//----------游脚相の関節角----------
	if (Uratio > 1) { Uratio = 1; }
	c1 = acos(Uratio);
	if (Ux > Lx) { Ux = Lx; }
	cf = -asin(Ux / Lx);
	q1 = -atan((Uy + Sw_offset) / (Ly * Uratio));
	q2 = cf + c1;
	q3 = c1 * 2;
	q4 = c1 - cf + toe;
	q5 = q1;
	q6 = -asin(Px / Lx);
	if (fl == 0) {
		//-----Lleg-----
		servoref[ 2] = -q0;
		servoref[ 4] =  q1;
		servoref[ 6] =  q2;
		servoref[ 8] =  q3;
		servoref[10] =  q4;
		servoref[12] =  q5;
		servoref[18] =  q6;
	}
	else {
		//-----Rleg-----
		servoref[ 1] = -q0;
		servoref[ 3] = -q1;
		servoref[ 5] =  q2;
		servoref[ 7] =  q3;
		servoref[ 9] =  q4;
		servoref[11] = -q5;
		servoref[17] =  q6;
	}
	//----------------------角度制御---------------------------------
	for (i = 1; i <= 18; i++) { robo.motor.SetRefPos(i, servoref[i]); }
	//---------------------------------------------------------------
	PrintMsg("phase=%02d, n=%02d, Px=%+5.3lf, Py=%+5.3lf\n", phase, n, Px, Py);
}

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

« 自然な歩行をするロボットによる凸凹チャレンジ① | トップページ | エクセルで自然な歩行モーションの作成 »

コメント

コメントを書く

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

« 自然な歩行をするロボットによる凸凹チャレンジ① | トップページ | エクセルで自然な歩行モーションの作成 »