日記

フリーページ

2019年09月15日
XML
カテゴリ:Linux
高速化を追求した(?)オフライン歩行パターン生成の基本プログラムが完成した。
今回は、ソースを見てもらうために気を使って書いてみた。(^^;;;

高速化のポイントは、
1)グローバル変数を使用しない(十分CPUが速いので、複数コアで共有の必要がない)
2)割り算しない(シンプルだが重要)
3)おなじループで複数処理をおこなう(条件分岐を減らす)
4)構造体配列を使って、同じタイミングで使う変数を近くのアドレスに配置(オフセットアドレス命令)
5)コンパイラの邪魔をしない(良かれと思ったひと手間が逆効果)
6)アセンブラを吐き出させて確認(-save-tempsオプション)
結構、ベタベタに無駄っぽく書く方が速いという・・・。



今回は、全体座標のX成分とY成分を同時に計算(高速化のため)しているのだけど、
グラフ化しないとちゃんと目的の軌道になっているかわからない・・・。
上記グラフは、機体の向きを45度から90度ずつ向きを変えて計算した結果。
両足がそろっている状態から、左足に体重移動、右足を一歩前へ、右足に体重移動、
左足を右足と揃えて、重心を体の真ん中で停止。



機体の向きをX方向に向けるとおなじみのグラフになる。
今回は、両足支持期の床反力(ZMP)の移動をスムーズにできるように、
変移区間の幅をある程度変えることができるようにしてある。
歩行周期や、この辺りのパラメータを変えて、より安定した歩行を探れるようにする。
ちなみに、目標ZMPを入力して、計算結果で出てきた重心軌道を使い、
エクセル上で、速度、加速度を求め、ZMP方程式(p=x-Zc/g*x'')に代入すると、
目標ZMPとかなり近い結果がでてくる。これで一安心。

あっ、ソースコードは、以下のとおりです。

#include <stdio.h> // printf,fopen
#include <math.h> // sinf,cosf
#include <sys/time.h> // gettimeofday
#define g  9.8  // 重力加速度
#define PI  3.141592654
#define torad  PI/180.0
#define TRACK_N0 15 // 始動、停止
#define TRACK_N1 30 // 一歩
#define TRACK_N  90 // 歩行開始
struct timeval tvs,tve;
typedef struct
{
 float P;
 float L;
 float Q0;
 float Q1;
 float D0;
 float D1;
 float X0;
 float X1;
} TDMA;
typedef struct
{
 int DSP_N;  // double stance phase:両足支持期
 float DSP_T[14]; // DSP_N max=7*2
 float P0[4];
 float P1[4];
} TRACK;
typedef struct
{
 float p[3];
 float theta;
} IKF;
void TDMA_init(TDMA tdm[],float a,float b,int N);
void TDMA_set (TDMA tdm[],TRACK tra);
void TDMA_calc(TDMA tdm[],float a,float b,int N);
/********** メインルーチン **********/
int main(int argc, char **argv)
{
 int i;
 float step_length,step_width,step_angle;
 IKF Sbody,Srfoot,Slfoot;
 IKF Ebody,Erfoot,Elfoot;
 float track_dt,track_Zc;
 float track_Vs[2],track_Ve[2];
 float tdma_a,tdma_b;
 TRACK  track;
 TDMA tdma[TRACK_N + 1];
/********** ファイルオープン **********/
 FILE *file;
 file = fopen("output.txt","w");
 if (file == NULL) {
  printf("Can't open file:%s\n","output.txt");
 }
/********** 初 期 化 **********/
 step_length = 0.1;
 step_width = 0.1;
 step_angle = 0.0 * torad;
 Sbody.p[0] = 0.0; // BODYの位置
 Sbody.p[1] = 0.0;
 Sbody.p[2] = 0.2;
 Sbody.theta = 0.0 * torad;
 Slfoot.p[0] = Sbody.p[0] + step_width/2.0*sinf(-Sbody.theta); // 左足の位置
 Slfoot.p[1] = Sbody.p[1] + step_width/2.0*cosf(-Sbody.theta);
 Slfoot.p[2] = 0.0;
 Slfoot.theta = Sbody.theta;
 Srfoot.p[0] = Sbody.p[0] - step_width/2.0*sinf(-Sbody.theta); // 右足の位置
 Srfoot.p[1] = Sbody.p[1] - step_width/2.0*cosf(-Sbody.theta);
 Srfoot.p[2] = 0.0;
 Srfoot.theta = Sbody.theta;
 
 Ebody.theta = Slfoot.theta + step_angle; // BODYの目標値
 Ebody.p[0] = Slfoot.p[0] - step_width/2.0*sinf(-Slfoot.theta) + step_length*cosf(Ebody.theta); // 右足から踏み出し
 Ebody.p[1] = Slfoot.p[1] - step_width/2.0*cosf(-Slfoot.theta) + step_length*sinf(Ebody.theta);   
 Ebody.p[2] = 0.2;
 Elfoot.p[0] = Ebody.p[0] + step_width/2.0*sinf(-Ebody.theta); // 左足の目標値
 Elfoot.p[1] = Ebody.p[1] + step_width/2.0*cosf(-Ebody.theta);
 Elfoot.p[2] = 0.0;
 Elfoot.theta = Ebody.theta;
 Erfoot.p[0] = Ebody.p[0] - step_width/2.0*sinf(-Ebody.theta); // 右足の目標値
 Erfoot.p[1] = Ebody.p[1] - step_width/2.0*cosf(-Ebody.theta);
 Erfoot.p[2] = 0.0;
 Erfoot.theta = Ebody.theta;
 track_dt = 0.02;  // 0.02 * TRACK_N1 = 0.6sec(一歩の周期)
 track_Zc = Sbody.p[2]; // 重心高
 track_Vs[0] = 0.0;  // 初速度
 track_Vs[1] = 0.0;
 track_Ve[0] = 0.0;  // 終端速度
 track_Ve[1] = 0.0;
 
/********** TDMA初期化 **********/
gettimeofday(&tvs,NULL);
 tdma_a = - track_Zc/(g*track_dt*track_dt);
 tdma_b = 2.0*track_Zc/(g*track_dt*track_dt) + 1.0;
 TDMA_init(tdma,tdma_a,tdma_b,TRACK_N);
gettimeofday(&tve,NULL);
 printf("TDMA初期化 %ld usec\n",(long)((tve.tv_sec-tvs.tv_sec)*1000000+tve.tv_usec-tvs.tv_usec));
/********** ZMP設定 **********/
 track.DSP_N = 7*2;
 for ( i = 1; i < track.DSP_N ; i++ ) {
  track.DSP_T[i] = (cosf(PI/track.DSP_N*(i-1)) - cosf(PI/track.DSP_N*i))/2.0;
 }
gettimeofday(&tvs,NULL);
 track.P0[0] = Sbody.p[0];
 track.P0[1] = Slfoot.p[0];
 track.P0[2] = Erfoot.p[0];
 track.P0[3] = Ebody.p[0];
 track.P1[0] = Sbody.p[1];
 track.P1[1] = Slfoot.p[1];
 track.P1[2] = Erfoot.p[1];
 track.P1[3] = Ebody.p[1];
 TDMA_set (tdma,track);
 tdma[0].D0  += tdma_a * track_Vs[0] * track_dt; // 初速度
 tdma[0].D1  += tdma_a * track_Vs[1] * track_dt;
 tdma[TRACK_N].D0 -= tdma_a * track_Ve[0] * track_dt; // 終端速度
 tdma[TRACK_N].D1 -= tdma_a * track_Ve[1] * track_dt;
gettimeofday(&tve,NULL);
 printf("ZMP設定    %ld usec\n",(long)((tve.tv_sec-tvs.tv_sec)*1000000+tve.tv_usec-tvs.tv_usec));
/********** 重心軌道計算 **********/
gettimeofday(&tvs,NULL);
 TDMA_calc(tdma,tdma_a,tdma_b,TRACK_N);
gettimeofday(&tve,NULL);
 printf("軌道計算   %ld usec\n",(long)((tve.tv_sec-tvs.tv_sec)*1000000+tve.tv_usec-tvs.tv_usec));
/********** 結果出力 **********/
 for ( i = 0; i < TRACK_N + 1; i++ ) {
  fprintf(file,"%12.6e,%12.6e,%12.6e,%12.6e\n",tdma[i].D0,tdma[i].X0,tdma[i].D1,tdma[i].X1);
 }
/********** ファイルクローズ **********/
 fclose(file);
 return 0;
}
/********** サブルーチン **********/
void TDMA_init(TDMA tdm[],float a,float b,int N)
{
 int i;
 tdm[0].P = - a / ( a + b );
 for ( i = 1; i < N; i++ ) {
  tdm[i].L = 1.0 / ( b + a * tdm[i - 1].P );
  tdm[i].P = - a * tdm[i].L;
 }
 tdm[i].L = 1.0 / ( b + a + a * tdm[i - 1].P );
}
void TDMA_set (TDMA tdm[],TRACK tra)
{
 int i;
 float track_00,track_01;
 float track_10,track_11;
 float track_20,track_21;
 for (i = 0; i < TRACK_N0 - tra.DSP_N/2 + 1; i++ ) {
  tdm[i].D0 = tra.P0[0];
  tdm[i].D1 = tra.P1[0];
  tdm[TRACK_N0 + tra.DSP_N/2 + TRACK_N1*2 + i].D0 = tra.P0[3];
  tdm[TRACK_N0 + tra.DSP_N/2 + TRACK_N1*2 + i].D1 = tra.P1[3];
 }
 for (i = 0; i < TRACK_N1 - tra.DSP_N + 1; i++ ) {
  tdm[TRACK_N0 + tra.DSP_N/2 + i].D0 = tra.P0[1];
  tdm[TRACK_N0 + tra.DSP_N/2 + i].D1 = tra.P1[1];
  tdm[TRACK_N0 + tra.DSP_N/2 + TRACK_N1 + i].D0 = tra.P0[2];
  tdm[TRACK_N0 + tra.DSP_N/2 + TRACK_N1 + i].D1 = tra.P1[2];
 }
 track_00 = tra.P0[0];
 track_01 = tra.P1[0];
 track_10 = tra.P0[1];
 track_11 = tra.P1[1];
 track_20 = tra.P0[2];
 track_21 = tra.P1[2];
 for (i = 1; i < tra.DSP_N; i++ ) {
  track_00 += (tra.P0[1] - tra.P0[0]) * tra.DSP_T[i];
  track_01 += (tra.P1[1] - tra.P1[0]) * tra.DSP_T[i];
  track_10 += (tra.P0[2] - tra.P0[1]) * tra.DSP_T[i];
  track_11 += (tra.P1[2] - tra.P1[1]) * tra.DSP_T[i];
  track_20 += (tra.P0[3] - tra.P0[2]) * tra.DSP_T[i];
  track_21 += (tra.P1[3] - tra.P1[2]) * tra.DSP_T[i];
  tdm[TRACK_N0 - tra.DSP_N/2 + i].D0 = track_00;
  tdm[TRACK_N0 - tra.DSP_N/2 + i].D1 = track_01;
  tdm[TRACK_N0 - tra.DSP_N/2 + TRACK_N1 + i].D0 = track_10;
  tdm[TRACK_N0 - tra.DSP_N/2 + TRACK_N1 + i].D1 = track_11;
  tdm[TRACK_N0 - tra.DSP_N/2 + TRACK_N1*2 + i].D0 = track_20;
  tdm[TRACK_N0 - tra.DSP_N/2 + TRACK_N1*2 + i].D1 = track_21;
 }
}
void TDMA_calc(TDMA tdm[],float a,float b,int N)
{
 int i;
 tdm[0].Q0 = tdm[0].D0 / ( a + b );
 tdm[0].Q1 = tdm[0].D1 / ( a + b );
 for ( i = 1; i < N + 1; i++ ) {
  tdm[i].Q0 = ( tdm[i].D0 - a * tdm[i - 1].Q0 ) * tdm[i].L;
  tdm[i].Q1 = ( tdm[i].D1 - a * tdm[i - 1].Q1 ) * tdm[i].L;
 }
 tdm[N].X0 = tdm[N].Q0;
 tdm[N].X1 = tdm[N].Q1;
 for (i = N - 1; i > -1; i-- ) {
  tdm[i].X0 = tdm[i].P * tdm[i + 1].X0 + tdm[i].Q0;
  tdm[i].X1 = tdm[i].P * tdm[i + 1].X1 + tdm[i].Q1;
 }
}








最終更新日  2019年09月15日 14時49分59秒

PR

キーワードサーチ

▼キーワード検索

プロフィール


nisiken2002

お気に入りブログ

まだ登録されていません

Copyright (c) 1997-2019 Rakuten, Inc. All Rights Reserved.