3.3. 實作 - core.cpp步態產生部分解讀

DrGuero2001後來有更新版的ODE source code 2 (ifdef.jp)，在core.cpp部分在產生步態計算確實有不同。

因為我完成的解讀還是基於舊的core.cpp，更新部分也還沒看，暫時無視。
日文大多是DrGuero2001原本的註解，“//XD”開頭則是我加上的註解，為了讓自己看得懂。
雖然解讀還有不少錯誤，還是把整個core.cpp搭配註解貼上來參考。

/*
June 1,2021
Corrected setting errors of LEG, autoH, and autoHs.
Corrected (K0脚振り角度)

*/


#include < ode/ode.h >
#include < drawstuff/drawstuff.h >
#include < stdio.h >
#include < stdlib.h >
#include < windows.h 	>
#include < fstream >
#include < iostream >
#include < string >
#include < sstream >
#include < iomanip >
#include < math.h >
#include < conio.h >  
#include  "biped.h"
#include  "core.h"

Include不多解釋

#define LEG 180.0	//Update in June 1,2021 :Before revision(#define LEG 190.0)
//XD //找到了是用degree當單位

LEG是腳部伸直狀態的角度，只考慮髖關節與踝關節連接的直線，與上半身垂直先形成的角度，與小腿/膝關節/大腿的狀態無關。

using namespace std;
core::core(void){
	adjFR	=2.04;
	autoH	=170;	//Update in June 1,2021 :Before revision(autoH=180)
	//XD //單位不明，猜測是mm
	autoHs	=180;	//Update in June 1,2021 :Before revision(autoHs=190)
	//XD //單位不明，猜測是mm
	mode		=0;
	walkF	=0;
	pitch	=0;
	roll		=0;
}

autoH與autoHs是腳部的高度，定義是髖關節與踝關節垂直方向的高度，autoH是現在狀態會持續計算更新，autoHs是上限值。

void core::footCont(float x,float y,float h,int s){

footCont是根據輸入目前踝關節相對於髖關節的位置，計算產生個足部關節對應角度

//x:中点を0とする足前後方向距離（前+）
//XD  //http://ai2001.ifdef.jp/uvc/qa.jpg
//XD  //猜是腳底前後與中心距離，起始是距離，後面計算應該是角度ThetaX

//y:中点を0とする足左右方向距離（右+）
//XD  //猜是腳底左右與中心距離

//h:足首ロール軸から股関節ロール軸までの距離
//XD  //猜是腳踝與股關節的距離

//s:支持脚(0)遊脚(1)を指定
//XD  //支持腳猜是落地或是懸空

//XD  //還沒找到起始值如何決定，這是函數，使用時才輸入數值
//XD  //但是雙冒號有查到但還是看不懂。

http://ai2001.ifdef.jp/uvc/qa.jpg是DrGuero2001在QA中附加的說明圖片

	float k;
  //XD  //計算用的，應該是角度ThetaK

k變數被重複是用在很多地方，我這個初學者不了解有何好處，閱讀時很容易混淆用途。

	k = sqrt(x*x+(y*y+h*h));	//A0からK0までの距離
//XD  //A0腳踝?，K0股關節?

很單純的計算踝關節到髖關節的直線距離。

	if(k>LEG)k=LEG;			//計算エラー回避
	//XD  //避開計算錯誤，大概是要避免腳反摺

	x = asin(x/k);			//K0脚振り角度 Update in June 1,2021 :Before revision( x=asin(x/LEG) )
	//XD  //重複x，這看不懂，把x(距離)拿去當輸入，再把arcsin計算結果寫入x(角度)，單位是弧度

	k = acos(k/LEG);			//K0膝曲げ角度
	//XD  //重複k，這看不懂，把k(距離)拿去當輸入，再把arccos計算結果寫入k(角度)，單位是弧度

	fbAV=0;					//UVC評価の為、ジャイロは無効にする
	//XD //ジャイロGyro陀螺儀

	lrAV=0;
	//XD //我猜也是gyro相關

fbAV和lrAV，在biped.h有定義，是讀取自姿態的數據，但是不知為何會在footcont計算中歸零，或許是作者測試調整UVC用的。

	K0W[s]	= k+x;
	//XD //單位應該是角度
	//XD  //http://ai2001.ifdef.jp/uvc/qa.jpg圖上沒有

	HW[s]	= k*2;
	//XD //單位應該是角度
	//XD  //http://ai2001.ifdef.jp/uvc/qa.jpg對圖好理解
	
	A0W[s]	= k-x-0.003*fbAV;
	//XD //單位應該是角度
	//XD  //http://ai2001.ifdef.jp/uvc/qa.jpg圖上沒有
		
	k = atan(y/h);			//K1角度
	//XD //單位應該是角度
	//XD //看起來重複使用變數用來計算
	
	K1W[s] = k;
	//XD //看起來重複使用變數用來計算，再寫入目標變數
	
	if(s==0)	A1W[s] = -k-0.002*lrAV;
	//XD //腳懸空時的計算
	
	
	else		A1W[s] = -k+0.002*lrAV;
	//XD //腳著地時的計算，計算式看起來就是反向
}

只知道是產生每個對應關節的角度，但是前面來自陀螺儀的數據都歸零，表示陀螺不會對關節角度的計算產生影響。

// **********************
// *	*  歩行制御メイン  **メインmain
// **********************

這邊開始就是產生步態的主要部分，基本上是先決定腳掌的位置，再逆向IK透過FootCont計算各關節的角度。

void core::walk(void){
	short i,j;
	
	float k;

	switch(mode){

	////////////////////////
	//// 初期姿勢に移行 ////
	////////////////////////
	case 0: //XD 姿勢初始化 
		if(autoHs>autoH)	autoHs-=1;	
//XD //autoHs初始值180，autoH初始值170
		else				mode=10;
		//XD //chenge mode to idle
		//XD //mode initial value 0
		
		
		footCont( -adjFR, 0, autoHs,  0 );
		//XD //(軸足) adjFR initial 2.04
		footCont( -adjFR, 0, autoHs,  1 );
		//XD //(遊脚) adjFR initial 2.04
		break;
		//XD // exit switch

mode 0，會把關節先移到初始角度，是固定位置。

	//////////////////////
	//// アイドル状態 ////
	//////////////////////
	//XD // idle status

以下進入mode 10的待命狀態。

	case 10:
		K0W[0]	=  0;
		//XD //股関節0(軸足) value 0 
		K0W[1]	=  0;
		//XD //股関節1(遊脚) value 0

		//// パラメータ初期化 ////parameter initial
		dx[0]	=0;
		//前後方向脚位置
		dx[1]	=0;
		//前後方向脚位置
		fwr0		=0;
		//中心からの戻り股幅(軸足)
		fwr1		=0;
		//中心からの戻り股幅(遊脚)
		fwct		=0;
		//一周期カウンタ
		//XD //計算到步態週期的第幾個count
		dxi		=0;				//縦距離積分値
		dyi		=0;				//横距離積分値
		dy		=0;			//横振り距離
		jikuasi	=0;//XD  //jikuasi軸足
		fwctEnd	=48;			//一周期最大カウント数
		swf		=12;//横振り最大距離
		fhMax	=20;//足上げ最大高さ
		landRate=0.2;		//0.1一歩の内、両足接地期間の割合
		fh		=0;			//足上げ高さ指示
		//XD //推測是抬腳高度
		

		footCont( -adjFR, 0, autoH, 0 );
		//XD //(軸足)
		footCont( -adjFR, 0, autoH, 1 );
		//XD //(遊脚)

		if(walkF&0x01){
			//歩行フラグ	（b0:歩行  b1:未  b2:未）
			//XD // "&"  是位元運算and
			fw=20;			//中心からの振り出し股幅
			mode=20;
			//XD // switch to mode 20
		}
		break;

mode 10是待命狀態，腳部會稍微屈膝，也是固定姿態，回到這狀態時，有些參數會在這狀態重設。 然後檢查是否有walk的指令，就會指定跨步距離fw，再切換到mode 20。

	/////////////////////////////////////////////////////////////////
	//////////////////////// 　歩行制御   ///////////////////////////
	/////////////////////////////////////////////////////////////////

以下進入步行狀態，有mode 20 與 30兩種步行狀態

	case 20: 
	//XD // case 20 ? 沒有 break，所以會繼續執行case30，但是這樣case20和30就沒有差別。
	//XD //後面有差，就是20和30會共用大部分的計算，但是根據20或30調整部分計算
	case 30:

case 20 沒有break看起來就會繼續走 case 30

		//###########################################################
		//###################  UVC(上体垂直制御) ####################
		//###########################################################

以下就是根據UVC影響產步態的部分。

		if((jikuasi==0 && asiPress_r<-0.1 && asiPress_l>-0.1) ||
			 (jikuasi==1 && asiPress_r>-0.1 && asiPress_l<-0.1)){
//XD  //這超難看懂 ，邏輯&&且，||或
//XD  //看起來是分左右腳檢查，只需要左腳或右腳接地壓力數值有一邊成立

			k = 1.5 * 193 * sin(lrRad);	//// 左右方向変位 ////
			//XD //lrRad頭左右角度		右が-，放在biped.h 
			//XD //看來是用頭判斷身體偏左或偏右
			//XD //k計算用，不用在意起始值
			if(jikuasi==0)	dyi += k;
			//XD //軸腳時，dyi增加k
			else				dyi -= k;
			//XD //游腳時，dyi減少k
			
			if(dyi>0)		dyi=0;//横距離積分値
						if(dyi<-30)		dyi=-30;
			//XD //把dyi數值限制在-30與0之間
			
			k = 1.5 * 130 * sin(fbRad);	//// 前後方向変位 ////
			//XD //fbRad頭前後角度		前が-
			//XD //也是用頭判斷身體偏前或偏後
			//XD //k計算用，不用在意起始值
			dxi += k;
			//XD //dxi縦距離積分値，但是看不到什麼時候積分的
		}
		dyi*=0.90;						//減衰
		
		if(uvcOff==1){
		//XD //關閉UVC，把dxi和dyi歸零
			dxi=0;
			dyi=0;
		}

UVC的部分還沒看懂，沒辦法解釋。

		//###########################################################
		//########################  基本歩容  #######################
		//###########################################################

以下就是step by step ，逐步跨出步伐的計算

		//// 横振り ////
		k=swf*sinf(M_PI*(fwct)/fwctEnd);//sinカーブ //XD カーブcurve
		//XD //k計算用，不用在意起始值，只有量值沒有方向性
		//XD //swf，起始值12
		//XD //M_PI圓周率，sinf就是sin，只是傳回f浮點數，輸入是弧度
		//XD //fwct會由零逐步增加到最大值fwctEnd
		//XD //算式看懂了，但是意義不明
		
		if(jikuasi==0)	dy=  k;//右振り
		//XD //軸腳時，dy等於k
		else				dy= -k;//左振り
		//XD //游腳時，dy等於負k

		//// 軸足側前振り制御 ////
		if(fwct 100){							//振り出し幅リミット
		//XD //跨步距離有檢查正方向上限
			dxi		   -= dx[jikuasi]-100;
			//XD //超過100的部分，就減少dxi積分
			dx[jikuasi] = 100;
			//XD // 然後回歸100
		}
		if(dx[jikuasi]<-100){
		//XD //跨步距離也檢查負方向下限
			dxi		   -= dx[jikuasi]+100;
			//XD //低過-100的部分，就減少dxi積分
			dx[jikuasi] =-100;
			//XD // 然後回歸-100
		}
		if(dx[jikuasi^1]> 100) dx[jikuasi^1] = 100;	//振り出し幅リミット抑制
		//XD //換腳，跨步距離有檢查正方向上限，然後回歸100
		if(dx[jikuasi^1]<-100) dx[jikuasi^1] =-100;
		//XD //換腳，跨步距離也檢查負方向下限，然後回歸-100

		//// 足上制御 ////
		//XD //控制抬腳高度
		i=landRate*fwctEnd;
		//XD //算出兩腳都落地的步態count
		if( fwct>i )	fh = fhMax * sinf( M_PI*(fwct-i)/(fwctEnd-i) );
		//XD //超出落地的步態count後，才計算抬腳高度，一樣用步態比率計算
		else			fh = 0;

		//// 脚制御関数呼び出し ////
		//XD //関数function
		if(jikuasi==0){
		//是算完腳尖位置，才呼叫footCont來計算，轉軸角度
			footCont( dx[0]-adjFR	, -dy-dyi+1, autoH,		0 );
			footCont( dx[1]-adjFR	,  dy-dyi+1, autoH-fh,	1 );
			
		}
		else{
			footCont( dx[0]-adjFR	, -dy-dyi+1, autoH-fh,	0 );
			footCont( dx[1]-adjFR	,  dy-dyi+1, autoH,		1 );
		}

也還沒完全懂，大約是一個完整跨步是fwctEnd = 48 個step，前半周期大概是跨出動作，後半周期是收回動作，所以計算方式不同。 然後懸空腳與落地腳的計算不同。

		//###########################################################
		//###########  CPG（歩周期生成、今回は簡易仕様） ############
		//###########################################################

前面的部分是一個跨步週期之中進行的計算。 CPG這段，就是檢查是否完成了一個跨步週期，就要進行左右腳切換懸空或落地狀態，然後進行下一個週期的計算。

		if(fwct==fwctEnd){	//簡易仕様（固定周期）
		//XD //步態做滿週期時，
			jikuasi^=1;
			//是換腳動作，^=1是XOR 1。0101切換。
			fwct=1;
			//XD //回歸起始值1
			dxi=0;
			//XD //歸零
			fwr0 = dx[jikuasi];
			//XD //軸足
			fwr1 = dx[jikuasi^1];
			//XD //軸足，^=1是指數1。結果不會變阿?，看不懂。
						fh=0;
			//XD //歸零
			
			mode=20;
			//XD //case切到20
			if(fw==20){			//歩幅制御			
				landRate=0.1;
				//XD //回到初始值
				fw=40;
				//XD //把跨步距離加到40
			}
		}
		else  ++fwct;
		//XD //週期加1
		break;
	}
}

然後就是不斷重複計算了